main.cpp

00001 //---------------------------------------------------------------------------
00002 // Attitude measurement using some attitude estimation filter
00003 // Filter : Complementary filter / Extended Kalman filter / Madgewick filter
00004 // IMU : MPU-9250
00005 // Written by Akira Heya
00006 // DATE : 2018/12/05
00007 //---------------------------------------------------------------------------
00008 
00009 //----include
00010 #include "mbed.h"
00011 #include "MPU9250.h"
00012 #include "EKF.h"
00013 #include "math.h"
00014 
00015 //----variable
00016 char buffer[14];
00017 //----Instance
00018 MPU9250 mpu9250;
00019 EKF ekf;
00020 Timer t;
00021 Serial pc(USBTX, USBRX);
00022 
00023 //****MAIN****
00024 int main()
00025 {
00026   //----Serial baud rate
00027   pc.baud(921600);
00028   //----I2C clock rate
00029   i2c.frequency(400000);
00030   //----System clock
00031   //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
00032   //----Timer start
00033   t.start();
00034   // Read the WHO_AM_I register, this is a good test of communication
00035   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
00036   pc.printf("I2C check : 0x%x\n\r", whoami);
00037   //----Self check
00038     if (whoami == 0x71)
00039     {
00040         pc.printf("MPU9250 is online\n\r");
00041         sprintf(buffer, "0x%x", whoami);
00042         wait(1);
00043         //----Reset registers to default in preparation for device calibration
00044         mpu9250.resetMPU9250();
00045         //----Start by performing self test and reporting values
00046         mpu9250.MPU9250SelfTest(SelfTest);
00047         pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
00048         pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
00049         pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
00050         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
00051         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
00052         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
00053         // Calibrate gyro and accelerometers, load biases in bias registers
00054         mpu9250.calibrateMPU9250(gyroBias, accelBias);
00055         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00056         pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00057         pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00058         pc.printf("x accel bias = %f\n\r", accelBias[0]);
00059         pc.printf("y accel bias = %f\n\r", accelBias[1]);
00060         pc.printf("z accel bias = %f\n\r", accelBias[2]);
00061         wait(1);
00062         //----Initialize device for active mode read of acclerometer, gyroscope, and temperature
00063         mpu9250.initMPU9250();
00064         pc.printf("MPU9250 initialized for active data mode....\n\r");
00065         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00066         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00067         wait(1);
00068     }
00069     else
00070     {
00071         pc.printf("Could not connect to MPU9250: \n\r");
00072         pc.printf("%#x \n",  whoami);
00073         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00074         //----Loop forever if communication doesn't happen
00075         while(1) ;
00076     }
00077     //----Get accelerometer sensitivity
00078     mpu9250.getAres();
00079     //----Get gyro sensitivity
00080     mpu9250.getGres();
00081     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00082     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00083     wait(1);
00084     //****LOOP****
00085     while(1)
00086     {
00087         //----Time interval
00088         Now = t.read_us();
00089         delt_t = (Now - lastUpdate)/1000000.0f;
00090         lastUpdate = Now;
00091         
00092         //----Acceleration sensor
00093         mpu9250.readAccelData(accelCount);
00094         // Calculate the accleration value into actual g's
00095         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00096         ay = (float)accelCount[1]*aRes - accelBias[1];
00097         az = (float)accelCount[2]*aRes - accelBias[2];
00098         th_ax = atan2(ay,sqrt(ax*ax+az*az))*(180.0f/PI);
00099         th_ay = -1*atan2(ax,az)*(180.0f/PI);
00100 
00101         //----Gyroscope
00102         mpu9250.readGyroData(gyroCount);
00103         // Calculate the gyro value into actual degrees per second
00104         gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00105         gy = (float)gyroCount[1]*gRes - gyroBias[1];
00106         gz = (float)gyroCount[2]*gRes - gyroBias[2];
00107 
00108         th_gx += (pre_gx + gx) * delt_t/2.0f;
00109         th_gy += (pre_gy + gy) * delt_t/2.0f;
00110         th_gz += (pre_gz + gz) * delt_t/2.0f;
00111         pre_gx = gx;
00112         pre_gy = gy;
00113         pre_gz = gz;
00114 
00115         //----Complementary filter
00116         th_x = 0.95*(th_x + (pre_gx + gx) * delt_t/2.0f) + 0.05*th_ax;
00117         th_y = 0.95*(th_y + (pre_gy + gy) * delt_t/2.0f) + 0.05*th_ay;
00118         
00119         //----Extended Kalman filter
00120         dt0_ekf = t.read_us();
00121         ekf.ExtendedKalmanFilterUpdate(th_ax, th_ay, pre_gx, pre_gy, pre_gz);
00122         dt1_ekf = t.read_us() - dt0_ekf;
00123         
00124         //----Madgwick filter
00125         dt0_mwf = t.read_us();
00126         mpu9250.MadgwickFilterUpdate_6axis(ax, ay, az, gx, gy, gz);
00127         roll  = atan2(2.0f * (q1 * q2 + q3 * q4), q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4);
00128         roll  *= 180.0f / PI;
00129         //roll = 0.995f*pre_roll + 0.005f*roll;
00130         
00131         pitch = -asin(2.0f * (q2 * q4 - q1 * q3));
00132         pitch *= 180.0f / PI;
00133         //pitch = 0.995f*pre_pitch + 0.005f*pitch;
00134         dt1_mwf = t.read_us() - dt0_mwf;
00135         
00136         pre_roll = roll;
00137         pre_pitch = pitch;
00138         
00139         //----Serial print
00140         sum_dt += delt_t;
00141         if (sum_dt > 0.0050f)
00142         {
00143             //pc.printf("%f, %f\n", dt1_ekf, dt1_mwf);
00144             pc.printf("%8.3f , %8.3f , %8.3f ,%8.3f\n", t.read(), th_x, th_y, estAlpha, estBeta);
00145             sum_dt = 0.0f;
00146         }
00147     }
00148 }