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