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 #include "sd_log.h"
  40
  41 #include <uart.h>
  42 #include <stdio.h>
  43 #include <aversive/error.h>
  44 #include "cmdline.h"
  45
  46
  47 //#define LED_PRIO           170
  48 #define GYRO_PRIO           100
  49
  50 int internal_mag_x;
  51 int internal_mag_y;
  52 int internal_mag_z;
  53
  54 int mag_x;
  55 int mag_y;
  56 int mag_z;
  57
  58
  59 void i2c_recvevent(uint8_t *buf, int8_t size)
  60 {
  61         (void)buf;
  62         (void)size;
  63 }
  64
  65 void i2c_sendevent(int8_t size)
  66 {
  67         (void)size;
  68 }
  69
  70 #if 0
  71 static void main_timer_interrupt(void)
  72 {
  73         static uint8_t cpt = 0;
  74         cpt++;
  75         sei();
  76         if ((cpt & 0x3) == 0)
  77                 scheduler_interrupt();
  78 }
  79 #endif
  80
  81 #define LED1_TOGGLE()   PORTB ^= 0x20;
  82
  83
  84 uint16_t counter;
  85
  86 void do_led_blink(void *dummy)
  87 {
  88         (void)dummy;
  89
  90 #if 1 /* simple blink */
  91         LED1_TOGGLE();
  92 #endif
  93         //counter ++;
  94         printf("\r\n%"PRId16"\r\n", counter);
  95         counter = 0;
  96
  97 }
  98
  99
 100 /* for i2c */
 101 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
 102
 103
 104 double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
 105 double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
 106 double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
 107
 108 double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
 109 double Omega_P[3]= {0,0,0};//Omega Proportional correction
 110 double Omega_I[3]= {0,0,0};//Omega Integrator
 111 double Omega[3]= {0,0,0};
 112
 113 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 114
 115 double DCM_Matrix[3][3]= {
 116   {
 117     1,0,0  }
 118   ,{
 119     0,1,0  }
 120   ,{
 121     0,0,1  }
 122 };
 123
 124 double Temporary_Matrix[3][3]={
 125   {
 126     0,0,0  }
 127   ,{
 128     0,0,0  }
 129   ,{
 130     0,0,0  }
 131 };
 132
 133 double errorRollPitch[3]= {0,0,0};
 134 double errorYaw[3]= {0,0,0};
 135 double errorCourse=180;
 136 double COGX=0; //Course overground X axis
 137 double COGY=1; //Course overground Y axis
 138
 139
 140
 141 #define OUTPUTMODE 2
 142
 143 #define ToRad(x) (x*0.01745329252)  // *pi/180
 144 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 145
 146 /*
 147 #define Gyro_Gain_X 0.92 //X axis Gyro gain
 148 #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
 149 #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
 150 */
 151 #define Gyro_Gain_X (1.) //X axis Gyro gain
 152 #define Gyro_Gain_Y (1.) //Y axis Gyro gain
 153 #define Gyro_Gain_Z (1.) //Z axis Gyro gain
 154
 155 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
 156 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
 157 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
 158
 159 double G_Dt=0.02;    // Integration time (DCM algorithm)
 160
 161 #define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
 162 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
 163
 164
 165 #define Kp_ROLLPITCH (1.515/GRAVITY)
 166 #define Ki_ROLLPITCH (0.00101/GRAVITY)
 167
 168 #define Kp_YAW 1.2
 169 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 170 #define Ki_YAW 0.00005
 171
 172 #define MAGNETIC_DECLINATION 4.0
 173
 174 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
 175
 176
 177 int mag_offset[3];
 178 double Heading;
 179 double Heading_X;
 180 double Heading_Y;
 181
 182
 183 // Euler angles
 184 double roll;
 185 double pitch;
 186 double yaw;
 187
 188 int SENSOR_SIGN[]= {
 189         1,1,1,   // GYROX, GYROY, GYROZ,
 190         1,1,1,   // ACCELX, ACCELY, ACCELZ,
 191         1,1,1    // MAGX, MAGY, MAGZ,
 192 };
 193
 194 #if 0
 195
 196 double read_adc(uint8_t index)
 197 {
 198         int16_t value;
 199
 200         switch(index) {
 201                 case 0:
 202                         itg3200_read_axe(0, &value);
 203                         return (double) (SENSOR_SIGN[index] * value);
 204                 case 1:
 205                         itg3200_read_axe(1, &value);
 206                         return (double) (SENSOR_SIGN[index] * value);
 207                 case 2:
 208                         itg3200_read_axe(2, &value);
 209                         return (double) (SENSOR_SIGN[index] * value);
 210                 case 3:
 211                         bma150_read_axe(0, &value);
 212                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 213                 case 4:
 214                         bma150_read_axe(1, &value);
 215                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 216                 case 5:
 217                         bma150_read_axe(2, &value);
 218                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 219         }
 220         return 0.0;
 221 }
 222
 223
 224 uint8_t measure_time = 0;
 225 void read_sensors(void)
 226 {
 227         uint8_t flags;
 228         uint8_t err;
 229         uint16_t axes[3];
 230
 231         /* read mag */
 232         measure_time ++;//= (measure_time +1)%3;
 233         if (measure_time%2 == 0) {
 234                 err = ak8500_start_measure();
 235                 if (err) {
 236                         printf("mag start err %X\r\n", err);
 237                         measure_time = 0;
 238                 }
 239         }
 240         else if (measure_time%2 == 1) {
 241                 err = ak8500_read_all_axes(&axes);
 242                 if (err == 0) {
 243                         /*
 244                         Magnet_Vector[0] = (double)SENSOR_SIGN[6] * (double)axes[0];
 245                         Magnet_Vector[1] = (double)SENSOR_SIGN[7] * (double)axes[1];
 246                         Magnet_Vector[2] = (double)SENSOR_SIGN[8] * (double)axes[2];
 247                         */
 248                         /*
 249                         */
 250                         mag_x = SENSOR_SIGN[6] * axes[0];
 251                         mag_y = SENSOR_SIGN[7] * axes[1];
 252                         mag_z = SENSOR_SIGN[8] * axes[2];
 253                         Magnet_Vector[0] = mag_x;
 254                         Magnet_Vector[1] = mag_y;
 255                         Magnet_Vector[2] = mag_z;
 256
 257                 }
 258                 else {
 259                         printf("mag read err %X\r\n", err);
 260                 }
 261         }
 262         /*
 263         printf("%d %d %d\r\n",
 264                mag_x,
 265                mag_y,
 266                mag_z
 267                );
 268         */
 269         /*
 270         printf("%f %f %f\r\n",
 271                Magnet_Vector[0],
 272                Magnet_Vector[1],
 273                Magnet_Vector[2]);
 274         */
 275         /*
 276         Gyro_Vector[0]=Gyro_Scaled_X(read_adc(0)); //gyro x roll
 277         Gyro_Vector[1]=Gyro_Scaled_Y(read_adc(1)); //gyro y pitch
 278         Gyro_Vector[2]=Gyro_Scaled_Z(read_adc(2)); //gyro Z yaw
 279         */
 280         Gyro_Vector[0]=ToRad(read_adc(0)); //gyro x roll
 281         Gyro_Vector[1]=ToRad(read_adc(1)); //gyro y pitch
 282         Gyro_Vector[2]=ToRad(read_adc(2)); //gyro Z yaw
 283
 284         Accel_Vector[0]=9.81 * read_adc(3); // acc x
 285         Accel_Vector[1]=9.81 * read_adc(4); // acc y
 286         Accel_Vector[2]=9.81 * read_adc(5); // acc z
 287
 288 }
 289
 290 #endif
 291
 292
 293 void quaternion2euler(void)
 294 {
 295         /*
 296         roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
 297         pitch = asinf(2 * (q0*q2 - q3*q1));
 298         yaw =  atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
 299         */
 300         roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
 301         pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
 302         yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
 303 }
 304
 305 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
 306
 307 void imu_init(void)
 308 {
 309         /*
 310         bma150_init();
 311         itg3200_init();
 312         ak8975_read_sensitivity();
 313         */
 314         /*
 315         scheduler_add_periodical_event_priority(update_gyro, NULL,
 316                                                 1000000L / SCHEDULER_UNIT,
 317                                                 GYRO_PRIO);
 318         */
 319         mpu6050_init();
 320 }
 321
 322 int imu_loop(void)
 323 {
 324         struct fat_file_struct *fd = NULL;
 325         int16_t mpu6050_axes[10];
 326         char buf[128];
 327         int16_t len;
 328         uint32_t ms;
 329         uint8_t flags;
 330
 331         while (1) {
 332
 333                 counter ++;
 334                 mpu6050_read_all_axes(mpu6050_axes);
 335                 /*
 336                 for (i=0;i<7; i++){
 337                         printf("%"PRId16" ", mpu6050_axes[i]);
 338                 }
 339                 printf("\r\n");
 340                 */
 341                 /*
 342                 printf("%3.3f %3.3f %3.3f\r\n",
 343                        mpu6050_gx,
 344                        mpu6050_gy,
 345                        mpu6050_gz);
 346                 continue;
 347                 */
 348
 349                 /*
 350                 MadgwickAHRSupdateIMU(mpu6050_gx,
 351                                       mpu6050_gy,
 352                                       mpu6050_gz,
 353                                       mpu6050_ax,
 354                                       mpu6050_ay,
 355                                       mpu6050_az);
 356                 */
 357
 358                 MadgwickAHRSupdate(mpu6050_gx,
 359                                    mpu6050_gy,
 360                                    mpu6050_gz,
 361                                    mpu6050_ax,
 362                                    mpu6050_ay,
 363                                    mpu6050_az,
 364                                    mpu6050_mx,
 365                                    mpu6050_my,
 366                                    mpu6050_mz
 367                                    );
 368
 369                 quaternion2euler();
 370
 371                 /*
 372                 mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
 373                 mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
 374                 mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
 375                 */
 376                 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
 377
 378                 IRQ_LOCK(flags);
 379                 ms = global_ms;
 380                 IRQ_UNLOCK(flags);
 381
 382                 if (fd != NULL) {
 383                         len = snprintf(buf, sizeof(buf),
 384                                 "%"PRIu32"\t"
 385                                 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
 386                                 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
 387                                 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
 388                                 ms,
 389                                 mpu6050_gx, mpu6050_gy, mpu6050_gz,
 390                                 mpu6050_ax, mpu6050_ay, mpu6050_az,
 391                                 mpu6050_mx, mpu6050_my, mpu6050_mz);
 392                         if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
 393                                 printf_P(PSTR("error writing to file\n"));
 394                                 return -1;
 395                         }
 396                 }
 397
 398                 printf("%"PRIu32"\t", ms);
 399                 printf("gyro %+3.3f\t%+3.3f\t%+3.3f\t\t",
 400                         mpu6050_gx, mpu6050_gy, mpu6050_gz);
 401                 printf("accel %+3.3f\t%+3.3f\t%+3.3f\t\t",
 402                         mpu6050_ax, mpu6050_ay, mpu6050_az);
 403                 printf("magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
 404                         mpu6050_mx, mpu6050_my, mpu6050_mz);
 405
 406                 //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
 407                 //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
 408                 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );
 409
 410         }
 411
 412         return 0;
 413 }