imu.c

   1 #include <stdio.h>
   2 #include <string.h>
   3
   4 #include <scheduler.h>
   5 #include <timer.h>
   6
   7 #include <aversive/wait.h>
   8 #include <uart.h>
   9
  10
  11 #include <i2c.h>
  12 //#include "i2cm_sw.h"
  13
  14
  15 #include "math.h"
  16
  17 #include "i2c_helper.h"
  18 //#include "bma150.h"
  19 //#include "itg3200.h"
  20 //#include "ak8500.h"
  21
  22 #include "mpu6050.h"
  23
  24 #include "vector.h"
  25 #include "matrix.h"
  26
  27 #include "MadgwickAHRS.h"
  28
  29
  30 #define LED_PRIO           170
  31 #define GYRO_PRIO           100
  32
  33 int internal_mag_x;
  34 int internal_mag_y;
  35 int internal_mag_z;
  36
  37 int mag_x;
  38 int mag_y;
  39 int mag_z;
  40
  41
  42 void i2c_recvevent(uint8_t * buf, int8_t size)
  43 void i2c_recvevent(uint8_t *buf, int8_t size)
  44 {
  45         (void)buf;
  46         (void)size;
  47 }
  48
  49 void i2c_sendevent(int8_t size)
  50 {
  51         (void)size;
  52 }
  53
  54
  55 static void main_timer_interrupt(void)
  56 {
  57         static uint8_t cpt = 0;
  58         cpt++;
  59         sei();
  60         if ((cpt & 0x3) == 0)
  61                 scheduler_interrupt();
  62 }
  63
  64
  65 #define LED1_TOGGLE()   PORTB ^= 0x20;
  66
  67
  68 uint16_t counter;
  69
  70 void do_led_blink(void *dummy)
  71 {
  72         (void)dummy;
  73
  74 #if 1 /* simple blink */
  75         LED1_TOGGLE();
  76 #endif
  77         //counter ++;
  78         printf("\r\n%"PRId16"\r\n", counter);
  79         counter = 0;
  80
  81 }
  82
  83
  84
  85
  86 /* for i2c */
  87 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
  88
  89
  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
  93
  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};
  98
  99 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 100
 101 double DCM_Matrix[3][3]= {
 102   {
 103     1,0,0  }
 104   ,{
 105     0,1,0  }
 106   ,{
 107     0,0,1  }
 108 };
 109
 110 double Temporary_Matrix[3][3]={
 111   {
 112     0,0,0  }
 113   ,{
 114     0,0,0  }
 115   ,{
 116     0,0,0  }
 117 };
 118
 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
 124
 125
 126
 127 #define OUTPUTMODE 2
 128
 129 #define ToRad(x) (x*0.01745329252)  // *pi/180
 130 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 131
 132 /*
 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
 136 */
 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
 140
 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
 144
 145 double G_Dt=0.02;    // Integration time (DCM algorithm)
 146
 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
 149
 150
 151 #define Kp_ROLLPITCH (1.515/GRAVITY)
 152 #define Ki_ROLLPITCH (0.00101/GRAVITY)
 153
 154 #define Kp_YAW 1.2
 155 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 156 #define Ki_YAW 0.00005
 157
 158 #define MAGNETIC_DECLINATION 4.0
 159
 160 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
 161
 162
 163 int mag_offset[3];
 164 double Heading;
 165 double Heading_X;
 166 double Heading_Y;
 167
 168
 169 // Euler angles
 170 double roll;
 171 double pitch;
 172 double yaw;
 173
 174 int SENSOR_SIGN[]= {
 175         1,1,1,   // GYROX, GYROY, GYROZ,
 176         1,1,1,   // ACCELX, ACCELY, ACCELZ,
 177         1,1,1    // MAGX, MAGY, MAGZ,
 178 };
 179
 180 #if 0
 181
 182 double read_adc(uint8_t index)
 183 {
 184         int16_t value;
 185
 186         switch(index) {
 187                 case 0:
 188                         itg3200_read_axe(0, &value);
 189                         return (double) (SENSOR_SIGN[index] * value);
 190                 case 1:
 191                         itg3200_read_axe(1, &value);
 192                         return (double) (SENSOR_SIGN[index] * value);
 193                 case 2:
 194                         itg3200_read_axe(2, &value);
 195                         return (double) (SENSOR_SIGN[index] * value);
 196                 case 3:
 197                         bma150_read_axe(0, &value);
 198                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 199                 case 4:
 200                         bma150_read_axe(1, &value);
 201                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 202                 case 5:
 203                         bma150_read_axe(2, &value);
 204                         return (double) (SENSOR_SIGN[index] * bma15_axe2g(value));
 205         }
 206         return 0.0;
 207 }
 208
 209
 210 uint8_t measure_time = 0;
 211 void read_sensors(void)
 212 {
 213         uint8_t flags;
 214         uint8_t err;
 215         uint16_t axes[3];
 216
 217         /* read mag */
 218         measure_time ++;//= (measure_time +1)%3;
 219         if (measure_time%2 == 0) {
 220                 err = ak8500_start_measure();
 221                 if (err) {
 222                         printf("mag start err %X\r\n", err);
 223                         measure_time = 0;
 224                 }
 225         }
 226         else if (measure_time%2 == 1) {
 227                 err = ak8500_read_all_axes(&axes);
 228                 if (err == 0) {
 229                         /*
 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];
 233                         */
 234                         /*
 235                         */
 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;
 242
 243                 }
 244                 else {
 245                         printf("mag read err %X\r\n", err);
 246                 }
 247         }
 248         /*
 249         printf("%d %d %d\r\n",
 250                mag_x,
 251                mag_y,
 252                mag_z
 253                );
 254         */
 255         /*
 256         printf("%f %f %f\r\n",
 257                Magnet_Vector[0],
 258                Magnet_Vector[1],
 259                Magnet_Vector[2]);
 260         */
 261         /*
 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
 265         */
 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
 269
 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
 273
 274 }
 275
 276 #endif
 277
 278
 279 void quaternion2euler(void)
 280 {
 281         /*
 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));
 285         */
 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);
 289 }
 290
 291 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
 292 int main(void)
 293 {
 294         int16_t temp;
 295         //uint8_t err;
 296         uint16_t * ptr;
 297         uint8_t a;
 298         int i;
 299
 300
 301         int16_t mpu6050_axes[10];
 302
 303         /* UART */
 304         uart_init();
 305
 306         fdevopen(uart0_dev_send, uart0_dev_recv);
 307
 308 #if 0
 309         i2c_init(I2C_MODE_MASTER,  1/* I2C_MAIN_ADDR */);
 310         i2c_register_recv_event(i2c_recvevent);
 311         i2c_register_send_event(i2c_sendevent);
 312 #else
 313
 314         i2cm_NUM_init();
 315 #endif
 316
 317
 318
 319         // LED output
 320         DDRB |= 0x20;
 321
 322
 323         /* TIMER */
 324         timer_init();
 325         timer0_register_OV_intr(main_timer_interrupt);
 326
 327
 328         /* SCHEDULER */
 329         scheduler_init();
 330
 331
 332         sei();
 333
 334
 335         /*
 336         bma150_init();
 337         itg3200_init();
 338         ak8975_read_sensitivity();
 339         */
 340         scheduler_add_periodical_event_priority(do_led_blink, NULL,
 341                                                 1000000L / SCHEDULER_UNIT,
 342                                                 LED_PRIO);
 343         /*
 344         scheduler_add_periodical_event_priority(update_gyro, NULL,
 345                                                 1000000L / SCHEDULER_UNIT,
 346                                                 GYRO_PRIO);
 347         */
 348         mpu6050_init();
 349
 350         Mad_f32_init();
 351
 352         while (1) {
 353                 counter ++;
 354                 mpu6050_read_all_axes(mpu6050_axes);
 355                 /*
 356                 for (i=0;i<7; i++){
 357                         printf("%"PRId16" ", mpu6050_axes[i]);
 358                 }
 359                 printf("\r\n");
 360                 */
 361                 /*
 362                 printf("%3.3f %3.3f %3.3f\r\n",
 363                        mpu6050_gx,
 364                        mpu6050_gy,
 365                        mpu6050_gz);
 366                 continue;
 367                 */
 368
 369                 /*
 370                 MadgwickAHRSupdateIMU(mpu6050_gx,
 371                                       mpu6050_gy,
 372                                       mpu6050_gz,
 373                                       mpu6050_ax,
 374                                       mpu6050_ay,
 375                                       mpu6050_az);
 376                 */
 377
 378                 MadgwickAHRSupdate(mpu6050_gx,
 379                                    mpu6050_gy,
 380                                    mpu6050_gz,
 381                                    mpu6050_ax,
 382                                    mpu6050_ay,
 383                                    mpu6050_az,
 384                                    mpu6050_mx,
 385                                    mpu6050_my,
 386                                    mpu6050_mz
 387                                    );
 388
 389                 quaternion2euler();
 390
 391                 /*
 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]);
 395                 */
 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 );
 400
 401         }
 402
 403         return 0;
 404 }