main.cpp

00001 //------------------------------------------------------------------------------
00002 // Attitude measurement using IMU(MPU-9250)
00003 //------------------------------------------------------------------------------
00004 //----include
00005 #include "mbed.h"
00006 #include "MPU9250.h"
00007 //#include "EKF.h"
00008 //----variable
00009 char buffer[14];
00010 //----Instance
00011 MPU9250 mpu9250;
00012 Timer t;
00013 Serial pc(USBTX, USBRX);
00014 //****MAIN****
00015 int main()
00016 {
00017   //----Serial baud rate
00018   pc.baud(921600);
00019   //----I2C clock rate
00020   i2c.frequency(400000);
00021   //----System clock
00022   //pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
00023   //----Timer start
00024   t.start();
00025   // Read the WHO_AM_I register, this is a good test of communication
00026   uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
00027   pc.printf("I2C check : 0x%x\n\r", whoami);
00028   //----Self check
00029     if (whoami == 0x71)
00030     {
00031         pc.printf("MPU9250 is online\n\r");
00032         sprintf(buffer, "0x%x", whoami);
00033         wait(3);
00034         //----Reset registers to default in preparation for device calibration
00035         mpu9250.resetMPU9250();
00036         //----Start by performing self test and reporting values
00037         mpu9250.MPU9250SelfTest(SelfTest);
00038         pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
00039         pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
00040         pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
00041         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
00042         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
00043         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
00044         // Calibrate gyro and accelerometers, load biases in bias registers
00045         mpu9250.calibrateMPU9250(gyroBias, accelBias);
00046         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00047         pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00048         pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00049         pc.printf("x accel bias = %f\n\r", accelBias[0]);
00050         pc.printf("y accel bias = %f\n\r", accelBias[1]);
00051         pc.printf("z accel bias = %f\n\r", accelBias[2]);
00052         wait(3);
00053         //----Initialize device for active mode read of acclerometer, gyroscope, and temperature
00054         mpu9250.initMPU9250();
00055         pc.printf("MPU9250 initialized for active data mode....\n\r");
00056         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00057         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00058         wait(3);
00059     }
00060     else
00061     {
00062         pc.printf("Could not connect to MPU9250: \n\r");
00063         pc.printf("%#x \n",  whoami);
00064         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00065         //----Loop forever if communication doesn't happen
00066         while(1) ;
00067     }
00068     //----Get accelerometer sensitivity
00069     mpu9250.getAres();
00070     //----Get gyro sensitivity
00071     mpu9250.getGres();
00072     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00073     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00074     
00075     //****LOOP****
00076     while(1)
00077     {
00078         //----Time interval
00079         Now = t.read_us();
00080         delt_t = (Now - lastUpdate)/1000000.0f;
00081         lastUpdate = Now;
00082         // Read the x/y/z adc values
00083         mpu9250.readAccelData(accelCount);
00084         // Calculate the accleration value into actual g's
00085         ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00086         ay = (float)accelCount[1]*aRes - accelBias[1];
00087         az = (float)accelCount[2]*aRes - accelBias[2];
00088         th_ax = atan2(ay,sqrt(ax*ax+az*az))*(180.0f/PI);
00089         th_ay = -1*atan2(ax,az)*(180.0f/PI);
00090         // th_az = atan2(az,sqrt(ax*ax+ay*ay))*(180.0f/PI);
00091         
00092         /*
00093         th_ax_LPF = 0.95*pre_th_ax + 0.05*th_ax;
00094         th_ay_LPF = 0.95*pre_th_ay + 0.05*th_ay;
00095         // th_az_LPF = 0.95*pre_th_az + 0.05*th_az;
00096         pre_th_ax = th_ax;
00097         pre_th_ay = th_ay;
00098         // pre_th_az = th_az;
00099         */
00100 
00101         // Read the x/y/z adc values
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         //----Serial print
00120         sum_dt += delt_t;
00121         if (sum_dt > 0.0050f)
00122         {
00123             //--Angle from gyroscope and accel sensor [deg.]
00124             pc.printf("%8.3f , %8.3f , %8.3f\n", t.read(), th_gx, th_gy);
00125             //pc.printf("%8.3f , %8.3f , %8.3f ,%8.3f , %8.3f , %8.3f , %8.3f , %8.3f , %8.3f\n", t.read(), th_ax, th_ay, th_gx, th_gy, th_x, th_y, th_x_d, th_y_d);
00126             sum_dt = 0.0f;
00127         }
00128     }
00129 }