X-Git-Url: http://git.droids-corp.org/?p=protos%2Fimu.git;a=blobdiff_plain;f=imu.c;h=0833a8115dc3f12793f6a2d359c5844a3470a629;hp=8486960c1bf5b056e83eba3276fc7f3ffe884849;hb=849b29b998a8cbc10cd1900d3e5dd1a8488b77f6;hpb=96d834bdfb8df4e3369ca1b3c7bc7bc8534fda31 diff --git a/imu.c b/imu.c index 8486960..0833a81 100644 --- a/imu.c +++ b/imu.c @@ -1,396 +1,112 @@ #include #include +#include -#include -#include - +#include #include -#include - - -#include -//#include "i2cm_sw.h" +#include - -#include "math.h" +#include #include "i2c_helper.h" -//#include "bma150.h" -//#include "itg3200.h" -//#include "ak8500.h" - #include "mpu6050.h" - #include "vector.h" #include "matrix.h" - #include "MadgwickAHRS.h" +#include "sd_log.h" +#include "imu.h" +#include "main.h" +static struct callout imu_timer; -#define LED_PRIO 170 -#define GYRO_PRIO 100 - -int internal_mag_x; -int internal_mag_y; -int internal_mag_z; - -int mag_x; -int mag_y; -int mag_z; - - -void i2c_recvevent(uint8_t * buf, int8_t size) -{ -} +double roll; +double pitch; +double yaw; -void i2c_sendevent(int8_t size) +void quaternion2euler(void) { + roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3); + pitch = -asinf(2.0f * (q1 * q3 - q0 * q2)); + yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3); } - -static void main_timer_interrupt(void) +static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) { - static uint8_t cpt = 0; - cpt++; - sei(); - if ((cpt & 0x3) == 0) - scheduler_interrupt(); -} - + int16_t mpu6050_axes[10]; -#define LED1_TOGGLE() PORTB ^= 0x20; + (void)arg; + mpu6050_read_all_axes(mpu6050_axes); -uint16_t counter; + MadgwickAHRSupdate(mpu6050_gx, + mpu6050_gy, + mpu6050_gz, + mpu6050_ax, + mpu6050_ay, + mpu6050_az, + mpu6050_mx, + mpu6050_my, + mpu6050_mz + ); -void do_led_blink(void *dummy) -{ -#if 1 /* simple blink */ - LED1_TOGGLE(); -#endif - //counter ++; - printf("\r\n%"PRId16"\r\n", counter); - counter = 0; + quaternion2euler(); + callout_schedule(cm, tim, 2); } - - - -/* for i2c */ -//uint8_t command_buf[I2C_SEND_BUFFER_SIZE]; - - -float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector -float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector -float Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector - -float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data -float Omega_P[3]= {0,0,0};//Omega Proportional correction -float Omega_I[3]= {0,0,0};//Omega Integrator -float Omega[3]= {0,0,0}; - -float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here - -float DCM_Matrix[3][3]= { - { - 1,0,0 } - ,{ - 0,1,0 } - ,{ - 0,0,1 } -}; - -float Temporary_Matrix[3][3]={ - { - 0,0,0 } - ,{ - 0,0,0 } - ,{ - 0,0,0 } -}; - -float errorRollPitch[3]= {0,0,0}; -float errorYaw[3]= {0,0,0}; -float errorCourse=180; -float COGX=0; //Course overground X axis -float COGY=1; //Course overground Y axis - - - -#define OUTPUTMODE 2 - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -/* -#define Gyro_Gain_X 0.92 //X axis Gyro gain -#define Gyro_Gain_Y 0.92 //Y axis Gyro gain -#define Gyro_Gain_Z 0.94 //Z axis Gyro gain -*/ -#define Gyro_Gain_X (1.) //X axis Gyro gain -#define Gyro_Gain_Y (1.) //Y axis Gyro gain -#define Gyro_Gain_Z (1.) //Z axis Gyro gain - -#define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second -#define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second - -float G_Dt=0.02; // Integration time (DCM algorithm) - -#define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer -#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square - - -#define Kp_ROLLPITCH (1.515/GRAVITY) -#define Ki_ROLLPITCH (0.00101/GRAVITY) - -#define Kp_YAW 1.2 -//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution! -#define Ki_YAW 0.00005 - -#define MAGNETIC_DECLINATION 4.0 - -#define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v))) - - -int mag_offset[3]; -float Heading; -float Heading_X; -float Heading_Y; - - -// Euler angles -float roll; -float pitch; -float yaw; - -int SENSOR_SIGN[]= { - 1,1,1, // GYROX, GYROY, GYROZ, - 1,1,1, // ACCELX, ACCELY, ACCELZ, - 1,1,1 // MAGX, MAGY, MAGZ, -}; - -#if 0 - -float read_adc(uint8_t index) +void imu_init(void) { - int16_t value; + mpu6050_init(); - switch(index) { - case 0: - itg3200_read_axe(0, &value); - return (float) (SENSOR_SIGN[index] * value); - case 1: - itg3200_read_axe(1, &value); - return (float) (SENSOR_SIGN[index] * value); - case 2: - itg3200_read_axe(2, &value); - return (float) (SENSOR_SIGN[index] * value); - case 3: - bma150_read_axe(0, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); - case 4: - bma150_read_axe(1, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); - case 5: - bma150_read_axe(2, &value); - return (float) (SENSOR_SIGN[index] * bma15_axe2g(value)); - } - return 0.0; + callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO); + callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */ } - -uint8_t measure_time = 0; -void read_sensors(void) +void imu_get_info(struct imu_info *imu) { - uint8_t flags; - uint8_t err; - uint16_t axes[3]; - - /* read mag */ - measure_time ++;//= (measure_time +1)%3; - if (measure_time%2 == 0) { - err = ak8500_start_measure(); - if (err) { - printf("mag start err %X\r\n", err); - measure_time = 0; - } - } - else if (measure_time%2 == 1) { - err = ak8500_read_all_axes(&axes); - if (err == 0) { - /* - Magnet_Vector[0] = (float)SENSOR_SIGN[6] * (float)axes[0]; - Magnet_Vector[1] = (float)SENSOR_SIGN[7] * (float)axes[1]; - Magnet_Vector[2] = (float)SENSOR_SIGN[8] * (float)axes[2]; - */ - /* - */ - mag_x = SENSOR_SIGN[6] * axes[0]; - mag_y = SENSOR_SIGN[7] * axes[1]; - mag_z = SENSOR_SIGN[8] * axes[2]; - Magnet_Vector[0] = mag_x; - Magnet_Vector[1] = mag_y; - Magnet_Vector[2] = mag_z; - - } - else { - printf("mag read err %X\r\n", err); - } - } - /* - printf("%d %d %d\r\n", - mag_x, - mag_y, - mag_z - ); - */ - /* - printf("%f %f %f\r\n", - Magnet_Vector[0], - Magnet_Vector[1], - Magnet_Vector[2]); - */ - /* - Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll - Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch - Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw - */ - Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll - Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch - Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw - - Accel_Vector[0]=9.81 * read_adc(3); // acc x - Accel_Vector[1]=9.81 * read_adc(4); // acc y - Accel_Vector[2]=9.81 * read_adc(5); // acc z - + imu->gx = mpu6050_gx; + imu->gy = mpu6050_gy; + imu->gz = mpu6050_gz; + imu->ax = mpu6050_ax; + imu->ay = mpu6050_ay; + imu->az = mpu6050_az; + imu->mx = mpu6050_mx; + imu->my = mpu6050_my; + imu->mz = mpu6050_mz; + imu->temp = 0; } -#endif - - -void quaternion2euler(void) -{ - /* - roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2)); - pitch = asinf(2 * (q0*q2 - q3*q1)); - yaw = atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3)); - */ - roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3); - pitch = -asinf(2.0f * (q1 * q3 - q0 * q2)); - yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3); -} -#define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8))) -int main(void) +int imu_loop(void) { - int16_t temp; - //uint8_t err; - uint16_t * ptr; - uint8_t a; - int i; - - - int16_t mpu6050_axes[10]; - - /* UART */ - uart_init(); - - fdevopen(uart0_dev_send, uart0_dev_recv); - -#if 0 - i2c_init(I2C_MODE_MASTER, 1/* I2C_MAIN_ADDR */); - i2c_register_recv_event(i2c_recvevent); - i2c_register_send_event(i2c_sendevent); -#else - - i2cm_NUM_init(); -#endif - - - - // LED output - DDRB |= 0x20; - - - /* TIMER */ - timer_init(); - timer0_register_OV_intr(main_timer_interrupt); - - - /* SCHEDULER */ - scheduler_init(); - - - sei(); - - - /* - bma150_init(); - itg3200_init(); - ak8975_read_sensitivity(); - */ - scheduler_add_periodical_event_priority(do_led_blink, NULL, - 1000000L / SCHEDULER_UNIT, - LED_PRIO); - /* - scheduler_add_periodical_event_priority(update_gyro, NULL, - 1000000L / SCHEDULER_UNIT, - GYRO_PRIO); - */ - mpu6050_init(); - - Mad_f32_init(); + char buf[128]; + int16_t len; + uint32_t ms; + uint8_t flags; + struct imu_info imu; while (1) { - counter ++; - mpu6050_read_all_axes(mpu6050_axes); - /* - for (i=0;i<7; i++){ - printf("%"PRId16" ", mpu6050_axes[i]); - } - printf("\r\n"); - */ - /* - printf("%3.3f %3.3f %3.3f\r\n", - mpu6050_gx, - mpu6050_gy, - mpu6050_gz); - continue; - */ - - /* - MadgwickAHRSupdateIMU(mpu6050_gx, - mpu6050_gy, - mpu6050_gz, - mpu6050_ax, - mpu6050_ay, - mpu6050_az); - */ - MadgwickAHRSupdate(mpu6050_gx, - mpu6050_gy, - mpu6050_gz, - mpu6050_ax, - mpu6050_ay, - mpu6050_az, - mpu6050_mx, - mpu6050_my, - mpu6050_mz - ); - - quaternion2euler(); - - /* - mpu6050_axes[7] = swap_u16(mpu6050_axes[7]); - mpu6050_axes[8] = swap_u16(mpu6050_axes[8]); - mpu6050_axes[9] = swap_u16(mpu6050_axes[9]); - */ - printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw); - //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]); - //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]); - //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz ); + IRQ_LOCK(flags); + ms = global_ms; + imu_get_info(&imu); + IRQ_UNLOCK(flags); + + if (sd_log_enabled()) { + len = snprintf(buf, sizeof(buf), + "%"PRIu32"\t" + "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t" + "accel %+3.3f\t%+3.3f\t%+3.3f\t\t" + "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n", + ms, + imu.gx, imu.gy, imu.gz, + imu.ax, imu.ay, imu.az, + imu.mx, imu.my, imu.mz); + if (sd_log_write(buf, len) != len) { + printf_P(PSTR("error writing to file\n")); + return -1; + } + } }