imu.c

   1 #include <stdio.h>
   2 #include <string.h>
   3
   4 #include <aversive/irq_lock.h>
   5 #include <aversive/wait.h>
   6 #include <aversive/pgmspace.h>
   7 #include <uart.h>
   8
   9
  10 #include <i2c.h>
  11 //#include "i2cm_sw.h"
  12
  13
  14 #include "math.h"
  15
  16 #include "i2c_helper.h"
  17 //#include "bma150.h"
  18 //#include "itg3200.h"
  19 //#include "ak8500.h"
  20
  21 #include "main.h"
  22 #include "mpu6050.h"
  23
  24 #include "vector.h"
  25 #include "matrix.h"
  26
  27 #include "MadgwickAHRS.h"
  28
  29
  30
  31 #include <string.h>
  32 #include <avr/pgmspace.h>
  33 #include <avr/sleep.h>
  34 #include "fat.h"
  35 #include "fat_config.h"
  36 #include "partition.h"
  37 #include "sd_raw.h"
  38 #include "sd_raw_config.h"
  39
  40 #include <uart.h>
  41 #include <stdio.h>
  42 #include <aversive/error.h>
  43 #include "cmdline.h"
  44
  45
  46 //#define LED_PRIO           170
  47 #define GYRO_PRIO           100
  48
  49 int internal_mag_x;
  50 int internal_mag_y;
  51 int internal_mag_z;
  52
  53 int mag_x;
  54 int mag_y;
  55 int mag_z;
  56
  57
  58 void i2c_recvevent(uint8_t *buf, int8_t size)
  59 {
  60         (void)buf;
  61         (void)size;
  62 }
  63
  64 void i2c_sendevent(int8_t size)
  65 {
  66         (void)size;
  67 }
  68
  69 #if 0
  70 static void main_timer_interrupt(void)
  71 {
  72         static uint8_t cpt = 0;
  73         cpt++;
  74         sei();
  75         if ((cpt & 0x3) == 0)
  76                 scheduler_interrupt();
  77 }
  78 #endif
  79
  80 #define LED1_TOGGLE()   PORTB ^= 0x20;
  81
  82
  83 uint16_t counter;
  84
  85 void do_led_blink(void *dummy)
  86 {
  87         (void)dummy;
  88
  89 #if 1 /* simple blink */
  90         LED1_TOGGLE();
  91 #endif
  92         //counter ++;
  93         printf("\r\n%"PRId16"\r\n", counter);
  94         counter = 0;
  95
  96 }
  97
  98
  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)
 100 {
 101         (void)fs;
 102
 103     while(fat_read_dir(dd, dir_entry))
 104     {
 105         if(strcmp(dir_entry->long_name, name) == 0)
 106         {
 107             fat_reset_dir(dd);
 108             return 1;
 109         }
 110     }
 111
 112     return 0;
 113 }
 114
 115 static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name)
 116 {
 117     struct fat_dir_entry_struct file_entry;
 118     if(!find_file_in_dir(fs, dd, name, &file_entry))
 119         return 0;
 120
 121     return fat_open_file(fs, &file_entry);
 122 }
 123
 124
 125
 126 /* for i2c */
 127 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 128
 129
 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
 133
 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};
 138
 139 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 140
 141 double DCM_Matrix[3][3]= {
 142   {
 143     1,0,0  }
 144   ,{
 145     0,1,0  }
 146   ,{
 147     0,0,1  }
 148 };
 149
 150 double Temporary_Matrix[3][3]={
 151   {
 152     0,0,0  }
 153   ,{
 154     0,0,0  }
 155   ,{
 156     0,0,0  }
 157 };
 158
 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
 164
 165
 166
 167 #define OUTPUTMODE 2
 168
 169 #define ToRad(x) (x*0.01745329252)  // *pi/180
 170 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 171
 172 /*
 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
 176 */
 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
 180
 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
 184
 185 double G_Dt=0.02;    // Integration time (DCM algorithm)
 186
 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
 189
 190
 191 #define Kp_ROLLPITCH (1.515/GRAVITY)
 192 #define Ki_ROLLPITCH (0.00101/GRAVITY)
 193
 194 #define Kp_YAW 1.2
 195 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 196 #define Ki_YAW 0.00005
 197
 198 #define MAGNETIC_DECLINATION 4.0
 199
 200 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
 201
 202
 203 int mag_offset[3];
 204 double Heading;
 205 double Heading_X;
 206 double Heading_Y;
 207
 208
 209 // Euler angles
 210 double roll;
 211 double pitch;
 212 double yaw;
 213
 214 int SENSOR_SIGN[]= {
 215         1,1,1,   // GYROX, GYROY, GYROZ,
 216         1,1,1,   // ACCELX, ACCELY, ACCELZ,
 217         1,1,1    // MAGX, MAGY, MAGZ,
 218 };
 219
 220 #if 0
 221
 222 double read_adc(uint8_t index)
 223 {
 224         int16_t value;
 225
 226         switch(index) {
 227                 case 0:
 228                         itg3200_read_axe(0, &value);
 229                         return (double) (SENSOR_SIGN[index] * value);
 230                 case 1:
 231                         itg3200_read_axe(1, &value);
 232                         return (double) (SENSOR_SIGN[index] * value);
 233                 case 2:
 234                         itg3200_read_axe(2, &value);
 235                         return (double) (SENSOR_SIGN[index] * value);
 236                 case 3:
 237                         bma150_read_axe(0, &value);
 238                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 239                 case 4:
 240                         bma150_read_axe(1, &value);
 241                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 242                 case 5:
 243                         bma150_read_axe(2, &value);
 244                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 245         }
 246         return 0.0;
 247 }
 248
 249
 250 uint8_t measure_time = 0;
 251 void read_sensors(void)
 252 {
 253         uint8_t flags;
 254         uint8_t err;
 255         uint16_t axes[3];
 256
 257         /* read mag */
 258         measure_time ++;//= (measure_time +1)%3;
 259         if (measure_time%2 == 0) {
 260                 err = ak8500_start_measure();
 261                 if (err) {
 262                         printf("mag start err %X\r\n", err);
 263                         measure_time = 0;
 264                 }
 265         }
 266         else if (measure_time%2 == 1) {
 267                 err = ak8500_read_all_axes(&axes);
 268                 if (err == 0) {
 269                         /*
 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];
 273                         */
 274                         /*
 275                         */
 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;
 282
 283                 }
 284                 else {
 285                         printf("mag read err %X\r\n", err);
 286                 }
 287         }
 288         /*
 289         printf("%d %d %d\r\n",
 290                mag_x,
 291                mag_y,
 292                mag_z
 293                );
 294         */
 295         /*
 296         printf("%f %f %f\r\n",
 297                Magnet_Vector[0],
 298                Magnet_Vector[1],
 299                Magnet_Vector[2]);
 300         */
 301         /*
 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
 305         */
 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
 309
 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
 313
 314 }
 315
 316 #endif
 317
 318
 319 void quaternion2euler(void)
 320 {
 321         /*
 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));
 325         */
 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);
 329 }
 330
 331 static struct fat_file_struct *open_log_file(void)
 332 {
 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;
 339         int16_t i = 0;
 340         char name[16];
 341
 342         /* setup sd card slot */
 343         if (!sd_raw_init()) {
 344 #if SD_DEBUG
 345                 printf_P(PSTR("MMC/SD initialization failed\n"));
 346 #endif
 347                 return NULL;
 348         }
 349
 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,
 355 #else
 356                 0, 0,
 357 #endif
 358                 0);
 359
 360         if (!partition) {
 361                 /* If the partition did not open, assume the storage device
 362                  * is a "superfloppy", i.e. has no MBR.
 363                  */
 364                 partition = partition_open(sd_raw_read,
 365                         sd_raw_read_interval,
 366 #if SD_RAW_WRITE_SUPPORT
 367                         sd_raw_write,
 368                         sd_raw_write_interval,
 369 #else
 370                         0,
 371                         0,
 372 #endif
 373                         -1);
 374                 if (!partition) {
 375 #if SD_DEBUG
 376                         printf_P(PSTR("opening partition failed\n"));
 377 #endif
 378                         return NULL;
 379                 }
 380         }
 381
 382         /* open file system */
 383         fs = fat_open(partition);
 384         if (!fs) {
 385 #if SD_DEBUG
 386                 printf_P(PSTR("opening filesystem failed\n"));
 387 #endif
 388                 return NULL;
 389         }
 390
 391         /* open root directory */
 392         fat_get_dir_entry_of_path(fs, "/", &directory);
 393         dd = fat_open_dir(fs, &directory);
 394         if (!dd) {
 395 #if SD_DEBUG
 396                 printf_P(PSTR("opening root directory failed\n"));
 397 #endif
 398                 return NULL;
 399         }
 400
 401         /* print some card information as a boot message */
 402         //print_disk_info(fs);
 403
 404         printf("choose log file name\n");
 405         while (1) {
 406                 snprintf(name, sizeof(name), "log%.4d", i++);
 407                 if (!find_file_in_dir(fs, dd, name, &file_entry))
 408                         break;
 409         }
 410
 411         printf("create log file %s\n", name);
 412         if (!fat_create_file(dd, name, &file_entry)) {
 413                 printf_P(PSTR("error creating file: "));
 414         }
 415
 416         fd = open_file_in_dir(fs, dd, name);
 417         if (!fd) {
 418                 printf_P(PSTR("error opening "));
 419                 return NULL;
 420         }
 421
 422         return fd;
 423 }
 424
 425 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
 426 int imu_loop(void)
 427 {
 428         //int16_t temp;
 429         //uint8_t err;
 430         //uint16_t * ptr;
 431         //uint8_t a;
 432         //int i;
 433
 434
 435         struct fat_file_struct *fd = NULL;
 436         int16_t mpu6050_axes[10];
 437         char buf[128];
 438         int16_t len;
 439         uint32_t ms;
 440         uint8_t flags;
 441
 442         /*
 443         bma150_init();
 444         itg3200_init();
 445         ak8975_read_sensitivity();
 446         */
 447         /*
 448         scheduler_add_periodical_event_priority(update_gyro, NULL,
 449                                                 1000000L / SCHEDULER_UNIT,
 450                                                 GYRO_PRIO);
 451         */
 452         mpu6050_init();
 453
 454         if (1) {
 455                 fd = open_log_file();
 456                 if (fd == NULL)
 457                         printf("open log failed\r\n");
 458         }
 459
 460         while (1) {
 461
 462                 counter ++;
 463                 mpu6050_read_all_axes(mpu6050_axes);
 464                 /*
 465                 for (i=0;i<7; i++){
 466                         printf("%"PRId16" ", mpu6050_axes[i]);
 467                 }
 468                 printf("\r\n");
 469                 */
 470                 /*
 471                 printf("%3.3f %3.3f %3.3f\r\n",
 472                        mpu6050_gx,
 473                        mpu6050_gy,
 474                        mpu6050_gz);
 475                 continue;
 476                 */
 477
 478                 /*
 479                 MadgwickAHRSupdateIMU(mpu6050_gx,
 480                                       mpu6050_gy,
 481                                       mpu6050_gz,
 482                                       mpu6050_ax,
 483                                       mpu6050_ay,
 484                                       mpu6050_az);
 485                 */
 486
 487                 MadgwickAHRSupdate(mpu6050_gx,
 488                                    mpu6050_gy,
 489                                    mpu6050_gz,
 490                                    mpu6050_ax,
 491                                    mpu6050_ay,
 492                                    mpu6050_az,
 493                                    mpu6050_mx,
 494                                    mpu6050_my,
 495                                    mpu6050_mz
 496                                    );
 497
 498                 quaternion2euler();
 499
 500                 /*
 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]);
 504                 */
 505                 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
 506
 507                 IRQ_LOCK(flags);
 508                 ms = global_ms;
 509                 IRQ_UNLOCK(flags);
 510
 511                 if (fd != NULL) {
 512                         len = snprintf(buf, sizeof(buf),
 513                                 "%"PRIu32"\t"
 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",
 517                                 ms,
 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"));
 523                                 return -1;
 524                         }
 525                 }
 526
 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);
 534
 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 );
 538
 539         }
 540
 541         return 0;
 542 }