7 #include <aversive/wait.h>
12 //#include "i2cm_sw.h"
17 #include "i2c_helper.h"
19 //#include "itg3200.h"
27 #include "MadgwickAHRS.h"
42 void i2c_recvevent(uint8_t * buf, int8_t size)
43 void i2c_recvevent(uint8_t *buf, int8_t size)
49 void i2c_sendevent(int8_t size)
55 static void main_timer_interrupt(void)
57 static uint8_t cpt = 0;
61 scheduler_interrupt();
65 #define LED1_TOGGLE() PORTB ^= 0x20;
70 void do_led_blink(void *dummy)
74 #if 1 /* simple blink */
78 printf("\r\n%"PRId16"\r\n", counter);
87 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
90 double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
91 double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
92 double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
94 double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
95 double Omega_P[3]= {0,0,0};//Omega Proportional correction
96 double Omega_I[3]= {0,0,0};//Omega Integrator
97 double Omega[3]= {0,0,0};
99 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
101 double DCM_Matrix[3][3]= {
110 double Temporary_Matrix[3][3]={
119 double errorRollPitch[3]= {0,0,0};
120 double errorYaw[3]= {0,0,0};
121 double errorCourse=180;
122 double COGX=0; //Course overground X axis
123 double COGY=1; //Course overground Y axis
129 #define ToRad(x) (x*0.01745329252) // *pi/180
130 #define ToDeg(x) (x*57.2957795131) // *180/pi
133 #define Gyro_Gain_X 0.92 //X axis Gyro gain
134 #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
135 #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
137 #define Gyro_Gain_X (1.) //X axis Gyro gain
138 #define Gyro_Gain_Y (1.) //Y axis Gyro gain
139 #define Gyro_Gain_Z (1.) //Z axis Gyro gain
141 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
142 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
143 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
145 double G_Dt=0.02; // Integration time (DCM algorithm)
147 #define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
148 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
151 #define Kp_ROLLPITCH (1.515/GRAVITY)
152 #define Ki_ROLLPITCH (0.00101/GRAVITY)
155 //#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
156 #define Ki_YAW 0.00005
158 #define MAGNETIC_DECLINATION 4.0
160 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
175 1,1,1, // GYROX, GYROY, GYROZ,
176 1,1,1, // ACCELX, ACCELY, ACCELZ,
177 1,1,1 // MAGX, MAGY, MAGZ,
182 double read_adc(uint8_t index)
188 itg3200_read_axe(0, &value);
189 return (double) (SENSOR_SIGN[index] * value);
191 itg3200_read_axe(1, &value);
192 return (double) (SENSOR_SIGN[index] * value);
194 itg3200_read_axe(2, &value);
195 return (double) (SENSOR_SIGN[index] * value);
197 bma150_read_axe(0, &value);
198 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
200 bma150_read_axe(1, &value);
201 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
203 bma150_read_axe(2, &value);
204 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
210 uint8_t measure_time = 0;
211 void read_sensors(void)
218 measure_time ++;//= (measure_time +1)%3;
219 if (measure_time%2 == 0) {
220 err = ak8500_start_measure();
222 printf("mag start err %X\r\n", err);
226 else if (measure_time%2 == 1) {
227 err = ak8500_read_all_axes(&axes);
230 Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0];
231 Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1];
232 Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2];
236 mag_x = SENSOR_SIGN[6] * axes[0];
237 mag_y = SENSOR_SIGN[7] * axes[1];
238 mag_z = SENSOR_SIGN[8] * axes[2];
239 Magnet_Vector[0] = mag_x;
240 Magnet_Vector[1] = mag_y;
241 Magnet_Vector[2] = mag_z;
245 printf("mag read err %X\r\n", err);
249 printf("%d %d %d\r\n",
256 printf("%f %f %f\r\n",
262 Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
263 Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
264 Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
266 Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll
267 Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch
268 Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw
270 Accel_Vector[0]=9.81 * read_adc(3); // acc x
271 Accel_Vector[1]=9.81 * read_adc(4); // acc y
272 Accel_Vector[2]=9.81 * read_adc(5); // acc z
279 void quaternion2euler(void)
282 roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
283 pitch = asinf(2 * (q0*q2 - q3*q1));
284 yaw = atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
286 roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
287 pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
288 yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
291 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
301 int16_t mpu6050_axes[10];
306 fdevopen(uart0_dev_send, uart0_dev_recv);
309 i2c_init(I2C_MODE_MASTER, 1/* I2C_MAIN_ADDR */);
310 i2c_register_recv_event(i2c_recvevent);
311 i2c_register_send_event(i2c_sendevent);
325 timer0_register_OV_intr(main_timer_interrupt);
338 ak8975_read_sensitivity();
340 scheduler_add_periodical_event_priority(do_led_blink, NULL,
341 1000000L / SCHEDULER_UNIT,
344 scheduler_add_periodical_event_priority(update_gyro, NULL,
345 1000000L / SCHEDULER_UNIT,
354 mpu6050_read_all_axes(mpu6050_axes);
357 printf("%"PRId16" ", mpu6050_axes[i]);
362 printf("%3.3f %3.3f %3.3f\r\n",
370 MadgwickAHRSupdateIMU(mpu6050_gx,
378 MadgwickAHRSupdate(mpu6050_gx,
392 mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
393 mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
394 mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
396 printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
397 //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
398 //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
399 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );