4 #include <aversive/irq_lock.h>
5 #include <aversive/wait.h>
6 #include <aversive/pgmspace.h>
11 //#include "i2cm_sw.h"
16 #include "i2c_helper.h"
18 //#include "itg3200.h"
27 #include "MadgwickAHRS.h"
32 #include <avr/pgmspace.h>
33 #include <avr/sleep.h>
35 #include "fat_config.h"
36 #include "partition.h"
38 #include "sd_raw_config.h"
42 #include <aversive/error.h>
46 //#define LED_PRIO 170
58 void i2c_recvevent(uint8_t *buf, int8_t size)
64 void i2c_sendevent(int8_t size)
70 static void main_timer_interrupt(void)
72 static uint8_t cpt = 0;
76 scheduler_interrupt();
80 #define LED1_TOGGLE() PORTB ^= 0x20;
85 void do_led_blink(void *dummy)
89 #if 1 /* simple blink */
93 printf("\r\n%"PRId16"\r\n", counter);
99 static uint8_t find_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name, struct fat_dir_entry_struct* dir_entry)
103 while(fat_read_dir(dd, dir_entry))
105 if(strcmp(dir_entry->long_name, name) == 0)
115 static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name)
117 struct fat_dir_entry_struct file_entry;
118 if(!find_file_in_dir(fs, dd, name, &file_entry))
121 return fat_open_file(fs, &file_entry);
127 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
130 double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
131 double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
132 double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
134 double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
135 double Omega_P[3]= {0,0,0};//Omega Proportional correction
136 double Omega_I[3]= {0,0,0};//Omega Integrator
137 double Omega[3]= {0,0,0};
139 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
141 double DCM_Matrix[3][3]= {
150 double Temporary_Matrix[3][3]={
159 double errorRollPitch[3]= {0,0,0};
160 double errorYaw[3]= {0,0,0};
161 double errorCourse=180;
162 double COGX=0; //Course overground X axis
163 double COGY=1; //Course overground Y axis
169 #define ToRad(x) (x*0.01745329252) // *pi/180
170 #define ToDeg(x) (x*57.2957795131) // *180/pi
173 #define Gyro_Gain_X 0.92 //X axis Gyro gain
174 #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
175 #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
177 #define Gyro_Gain_X (1.) //X axis Gyro gain
178 #define Gyro_Gain_Y (1.) //Y axis Gyro gain
179 #define Gyro_Gain_Z (1.) //Z axis Gyro gain
181 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
182 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
183 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
185 double G_Dt=0.02; // Integration time (DCM algorithm)
187 #define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
188 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
191 #define Kp_ROLLPITCH (1.515/GRAVITY)
192 #define Ki_ROLLPITCH (0.00101/GRAVITY)
195 //#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
196 #define Ki_YAW 0.00005
198 #define MAGNETIC_DECLINATION 4.0
200 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
215 1,1,1, // GYROX, GYROY, GYROZ,
216 1,1,1, // ACCELX, ACCELY, ACCELZ,
217 1,1,1 // MAGX, MAGY, MAGZ,
222 double read_adc(uint8_t index)
228 itg3200_read_axe(0, &value);
229 return (double) (SENSOR_SIGN[index] * value);
231 itg3200_read_axe(1, &value);
232 return (double) (SENSOR_SIGN[index] * value);
234 itg3200_read_axe(2, &value);
235 return (double) (SENSOR_SIGN[index] * value);
237 bma150_read_axe(0, &value);
238 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
240 bma150_read_axe(1, &value);
241 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
243 bma150_read_axe(2, &value);
244 return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
250 uint8_t measure_time = 0;
251 void read_sensors(void)
258 measure_time ++;//= (measure_time +1)%3;
259 if (measure_time%2 == 0) {
260 err = ak8500_start_measure();
262 printf("mag start err %X\r\n", err);
266 else if (measure_time%2 == 1) {
267 err = ak8500_read_all_axes(&axes);
270 Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0];
271 Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1];
272 Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2];
276 mag_x = SENSOR_SIGN[6] * axes[0];
277 mag_y = SENSOR_SIGN[7] * axes[1];
278 mag_z = SENSOR_SIGN[8] * axes[2];
279 Magnet_Vector[0] = mag_x;
280 Magnet_Vector[1] = mag_y;
281 Magnet_Vector[2] = mag_z;
285 printf("mag read err %X\r\n", err);
289 printf("%d %d %d\r\n",
296 printf("%f %f %f\r\n",
302 Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
303 Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
304 Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
306 Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll
307 Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch
308 Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw
310 Accel_Vector[0]=9.81 * read_adc(3); // acc x
311 Accel_Vector[1]=9.81 * read_adc(4); // acc y
312 Accel_Vector[2]=9.81 * read_adc(5); // acc z
319 void quaternion2euler(void)
322 roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
323 pitch = asinf(2 * (q0*q2 - q3*q1));
324 yaw = atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
326 roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
327 pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
328 yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
331 static struct fat_file_struct *open_log_file(void)
333 struct fat_file_struct *fd;
334 struct fat_fs_struct *fs;
335 struct partition_struct *partition ;
336 struct fat_dir_struct *dd;
337 struct fat_dir_entry_struct directory;
338 struct fat_dir_entry_struct file_entry;
342 /* setup sd card slot */
343 if (!sd_raw_init()) {
345 printf_P(PSTR("MMC/SD initialization failed\n"));
350 /* open first partition */
351 partition = partition_open(sd_raw_read,
352 sd_raw_read_interval,
353 #if SD_RAW_WRITE_SUPPORT
354 sd_raw_write, sd_raw_write_interval,
361 /* If the partition did not open, assume the storage device
362 * is a "superfloppy", i.e. has no MBR.
364 partition = partition_open(sd_raw_read,
365 sd_raw_read_interval,
366 #if SD_RAW_WRITE_SUPPORT
368 sd_raw_write_interval,
376 printf_P(PSTR("opening partition failed\n"));
382 /* open file system */
383 fs = fat_open(partition);
386 printf_P(PSTR("opening filesystem failed\n"));
391 /* open root directory */
392 fat_get_dir_entry_of_path(fs, "/", &directory);
393 dd = fat_open_dir(fs, &directory);
396 printf_P(PSTR("opening root directory failed\n"));
401 /* print some card information as a boot message */
402 //print_disk_info(fs);
404 printf("choose log file name\n");
406 snprintf(name, sizeof(name), "log%.4d", i++);
407 if (!find_file_in_dir(fs, dd, name, &file_entry))
411 printf("create log file %s\n", name);
412 if (!fat_create_file(dd, name, &file_entry)) {
413 printf_P(PSTR("error creating file: "));
416 fd = open_file_in_dir(fs, dd, name);
418 printf_P(PSTR("error opening "));
425 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
435 struct fat_file_struct *fd = NULL;
436 int16_t mpu6050_axes[10];
445 ak8975_read_sensitivity();
448 scheduler_add_periodical_event_priority(update_gyro, NULL,
449 1000000L / SCHEDULER_UNIT,
455 fd = open_log_file();
457 printf("open log failed\r\n");
463 mpu6050_read_all_axes(mpu6050_axes);
466 printf("%"PRId16" ", mpu6050_axes[i]);
471 printf("%3.3f %3.3f %3.3f\r\n",
479 MadgwickAHRSupdateIMU(mpu6050_gx,
487 MadgwickAHRSupdate(mpu6050_gx,
501 mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
502 mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
503 mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
505 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
512 len = snprintf(buf, sizeof(buf),
514 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
515 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
516 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
518 mpu6050_gx, mpu6050_gy, mpu6050_gz,
519 mpu6050_ax, mpu6050_ay, mpu6050_az,
520 mpu6050_mx, mpu6050_my, mpu6050_mz);
521 if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
522 printf_P(PSTR("error writing to file\n"));
527 printf("%"PRIu32"\t", ms);
528 printf("gyro %+3.3f\t%+3.3f\t%+3.3f\t\t",
529 mpu6050_gx, mpu6050_gy, mpu6050_gz);
530 printf("accel %+3.3f\t%+3.3f\t%+3.3f\t\t",
531 mpu6050_ax, mpu6050_ay, mpu6050_az);
532 printf("magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
533 mpu6050_mx, mpu6050_my, mpu6050_mz);
535 //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
536 //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
537 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );