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 #if 0
  60 static void main_timer_interrupt(void)
  61 {
  62         static uint8_t cpt = 0;
  63         cpt++;
  64         sei();
  65         if ((cpt & 0x3) == 0)
  66                 scheduler_interrupt();
  67 }
  68 #endif
  69
  70 #define LED1_TOGGLE()   PORTB ^= 0x20;
  71
  72
  73 uint16_t counter;
  74
  75 void do_led_blink(void *dummy)
  76 {
  77         (void)dummy;
  78
  79 #if 1 /* simple blink */
  80         LED1_TOGGLE();
  81 #endif
  82         //counter ++;
  83         printf("\r\n%"PRId16"\r\n", counter);
  84         counter = 0;
  85
  86 }
  87
  88
  89 /* for i2c */
  90 //uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
  91
  92
  93 double Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
  94 double Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
  95 double Magnet_Vector[3]= {0,0,0}; //Store the acceleration in a vector
  96
  97 double Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
  98 double Omega_P[3]= {0,0,0};//Omega Proportional correction
  99 double Omega_I[3]= {0,0,0};//Omega Integrator
 100 double Omega[3]= {0,0,0};
 101
 102 double Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here
 103
 104 double DCM_Matrix[3][3]= {
 105   {
 106     1,0,0  }
 107   ,{
 108     0,1,0  }
 109   ,{
 110     0,0,1  }
 111 };
 112
 113 double Temporary_Matrix[3][3]={
 114   {
 115     0,0,0  }
 116   ,{
 117     0,0,0  }
 118   ,{
 119     0,0,0  }
 120 };
 121
 122 double errorRollPitch[3]= {0,0,0};
 123 double errorYaw[3]= {0,0,0};
 124 double errorCourse=180;
 125 double COGX=0; //Course overground X axis
 126 double COGY=1; //Course overground Y axis
 127
 128
 129
 130 #define OUTPUTMODE 2
 131
 132 #define ToRad(x) (x*0.01745329252)  // *pi/180
 133 #define ToDeg(x) (x*57.2957795131)  // *180/pi
 134
 135 /*
 136 #define Gyro_Gain_X 0.92 //X axis Gyro gain
 137 #define Gyro_Gain_Y 0.92 //Y axis Gyro gain
 138 #define Gyro_Gain_Z 0.94 //Z axis Gyro gain
 139 */
 140 #define Gyro_Gain_X (1.) //X axis Gyro gain
 141 #define Gyro_Gain_Y (1.) //Y axis Gyro gain
 142 #define Gyro_Gain_Z (1.) //Z axis Gyro gain
 143
 144 #define Gyro_Scaled_X(x) ((x)*ToRad(Gyro_Gain_X)) //Return the scaled ADC raw data of the gyro in radians for second
 145 #define Gyro_Scaled_Y(x) ((x)*ToRad(Gyro_Gain_Y)) //Return the scaled ADC raw data of the gyro in radians for second
 146 #define Gyro_Scaled_Z(x) ((x)*ToRad(Gyro_Gain_Z)) //Return the scaled ADC raw data of the gyro in radians for second
 147
 148 double G_Dt=0.02;    // Integration time (DCM algorithm)
 149
 150 #define GRAVITY 1.01 //this equivalent to 1G in the raw data coming from the accelerometer
 151 #define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square
 152
 153
 154 #define Kp_ROLLPITCH (1.515/GRAVITY)
 155 #define Ki_ROLLPITCH (0.00101/GRAVITY)
 156
 157 #define Kp_YAW 1.2
 158 //#define Kp_YAW 2.5      //High yaw drift correction gain - use with caution!
 159 #define Ki_YAW 0.00005
 160
 161 #define MAGNETIC_DECLINATION 4.0
 162
 163 #define constrain(v, a, b) (((v)<(a))?(a):((v)>(b)?(b):(v)))
 164
 165
 166 int mag_offset[3];
 167 double Heading;
 168 double Heading_X;
 169 double Heading_Y;
 170
 171
 172 // Euler angles
 173 double roll;
 174 double pitch;
 175 double yaw;
 176
 177 int SENSOR_SIGN[]= {
 178         1,1,1,   // GYROX, GYROY, GYROZ,
 179         1,1,1,   // ACCELX, ACCELY, ACCELZ,
 180         1,1,1    // MAGX, MAGY, MAGZ,
 181 };
 182
 183 void quaternion2euler(void)
 184 {
 185         /*
 186         roll = atan2f(2. * (q0*q1 + q2*q3), 1. - 2. * (q1*q1 + q2*q2));
 187         pitch = asinf(2 * (q0*q2 - q3*q1));
 188         yaw =  atan2f(2. * (q0*q3 + q1*q2), 1. - 2. * (q2*q2 + q3*q3));
 189         */
 190         roll = atan2f(2.0f * (q0 * q1 + q2 * q3), q0*q0 - q1*q1 - q2*q2 + q3*q3);
 191         pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
 192         yaw = atan2f(2.0f * (q1 * q2 + q0 * q3), q0*q0 + q1*q1 - q2*q2 - q3*q3);
 193 }
 194
 195 #define swap_u16(a) (((a>>8)&0xff) | (((a&0xFF)<<8)))
 196
 197 void imu_init(void)
 198 {
 199         /*
 200         bma150_init();
 201         itg3200_init();
 202         ak8975_read_sensitivity();
 203         */
 204         /*
 205         scheduler_add_periodical_event_priority(update_gyro, NULL,
 206                                                 1000000L / SCHEDULER_UNIT,
 207                                                 GYRO_PRIO);
 208         */
 209         mpu6050_init();
 210 }
 211
 212 int imu_loop(void)
 213 {
 214         struct fat_file_struct *fd = NULL;
 215         int16_t mpu6050_axes[10];
 216         char buf[128];
 217         int16_t len;
 218         uint32_t ms;
 219         uint8_t flags;
 220
 221         while (1) {
 222
 223                 counter ++;
 224                 mpu6050_read_all_axes(mpu6050_axes);
 225                 /*
 226                 for (i=0;i<7; i++){
 227                         printf("%"PRId16" ", mpu6050_axes[i]);
 228                 }
 229                 printf("\r\n");
 230                 */
 231                 /*
 232                 printf("%3.3f %3.3f %3.3f\r\n",
 233                        mpu6050_gx,
 234                        mpu6050_gy,
 235                        mpu6050_gz);
 236                 continue;
 237                 */
 238
 239                 /*
 240                 MadgwickAHRSupdateIMU(mpu6050_gx,
 241                                       mpu6050_gy,
 242                                       mpu6050_gz,
 243                                       mpu6050_ax,
 244                                       mpu6050_ay,
 245                                       mpu6050_az);
 246                 */
 247
 248                 MadgwickAHRSupdate(mpu6050_gx,
 249                                    mpu6050_gy,
 250                                    mpu6050_gz,
 251                                    mpu6050_ax,
 252                                    mpu6050_ay,
 253                                    mpu6050_az,
 254                                    mpu6050_mx,
 255                                    mpu6050_my,
 256                                    mpu6050_mz
 257                                    );
 258
 259                 quaternion2euler();
 260
 261                 /*
 262                 mpu6050_axes[7] = swap_u16(mpu6050_axes[7]);
 263                 mpu6050_axes[8] = swap_u16(mpu6050_axes[8]);
 264                 mpu6050_axes[9] = swap_u16(mpu6050_axes[9]);
 265                 */
 266                 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", roll, pitch, yaw);
 267
 268                 IRQ_LOCK(flags);
 269                 ms = global_ms;
 270                 IRQ_UNLOCK(flags);
 271
 272                 if (fd != NULL) {
 273                         len = snprintf(buf, sizeof(buf),
 274                                 "%"PRIu32"\t"
 275                                 "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
 276                                 "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
 277                                 "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
 278                                 ms,
 279                                 mpu6050_gx, mpu6050_gy, mpu6050_gz,
 280                                 mpu6050_ax, mpu6050_ay, mpu6050_az,
 281                                 mpu6050_mx, mpu6050_my, mpu6050_mz);
 282                         if (fat_write_file(fd, (unsigned char *)buf, len) != len) {
 283                                 printf_P(PSTR("error writing to file\n"));
 284                                 return -1;
 285                         }
 286                 }
 287
 288                 printf("%"PRIu32"\t", ms);
 289                 printf("gyro %+3.3f\t%+3.3f\t%+3.3f\t\t",
 290                         mpu6050_gx, mpu6050_gy, mpu6050_gz);
 291                 printf("accel %+3.3f\t%+3.3f\t%+3.3f\t\t",
 292                         mpu6050_ax, mpu6050_ay, mpu6050_az);
 293                 printf("magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
 294                         mpu6050_mx, mpu6050_my, mpu6050_mz);
 295
 296                 //printf("%+.4d %+.4d %+.4d\r\n", mpu6050_axes[7], mpu6050_axes[8], mpu6050_axes[9]);
 297                 //printf("%+3.3f\r\n", mpu6050_temp);//, mpu6050_axes[9]);
 298                 //printf("%+3.3f\t%+3.3f\t%+3.3f\r\n", mpu6050_mx, mpu6050_my, mpu6050_mz );
 299
 300         }
 301
 302         return 0;
 303 }