main.cpp

00001 /* MPU9250 Basic Example Code
00002  by: Kris Winer
00003  date: September 20, 2016
00004  license: Beerware - Use this code however you'd like. If you 
00005  find it useful you can buy me a beer some time.
00006  
00007  Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
00008  getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
00009  allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
00010  Mahony filter algorithms. 
00011  
00012  SDA and SCL should have external pull-up resistors (to 3.3V).
00013  */
00014  
00015 #include "mbed.h"
00016 #include "MPU9250.h"
00017 #include "BMP280.h"
00018 #include "math.h"
00019 
00020    MPU9250 mpu9250;  // Instantiate MPU9250 class
00021    
00022    BMP280 bmp280;   // Instantiate BMP280 class
00023    
00024    Timer t;
00025    
00026    InterruptIn myInterrupt(P0_8); // One nRF52 Dev Board variant uses pin 8, one uses pin 10
00027 
00028 /* Serial pc(USBTX, USBRX); // tx, rx*/
00029   Serial pc(P0_12, P0_14); // tx, rx  
00030 
00031 float sum = 0;
00032 uint32_t sumCount = 0;
00033 char buffer[14];
00034 uint8_t whoami = 0;
00035 double Temperature, Pressure; // stores BMP280 pressures sensor pressure and temperature
00036 int32_t rawPress, rawTemp;   // pressure and temperature raw count output for BMP280
00037 
00038 int32_t readBMP280Temperature()
00039 {
00040   uint8_t rawData[3];  // 20-bit pressure register data stored here
00041   bmp280.readBytes(BMP280_ADDRESS, BMP280_TEMP_MSB, 3, &rawData[0]);  
00042   return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
00043 }
00044 
00045 int32_t readBMP280Pressure()
00046 {
00047   uint8_t rawData[3];  // 20-bit pressure register data stored here
00048   bmp280.readBytes(BMP280_ADDRESS, BMP280_PRESS_MSB, 3, &rawData[0]);  
00049   return (int32_t) (((int32_t) rawData[0] << 16 | (int32_t) rawData[1] << 8 | rawData[2]) >> 4);
00050 }
00051 
00052 // Returns temperature in DegC, resolution is 0.01 DegC. Output value of
00053 // “5123” equals 51.23 DegC.
00054 int32_t bmp280_compensate_T(int32_t adc_T)
00055 {
00056   int32_t var1, var2, T;
00057   var1 = ((((adc_T >> 3) - ((int32_t)dig_T1 << 1))) * ((int32_t)dig_T2)) >> 11;
00058   var2 = (((((adc_T >> 4) - ((int32_t)dig_T1)) * ((adc_T >> 4) - ((int32_t)dig_T1))) >> 12) * ((int32_t)dig_T3)) >> 14;
00059   t_fine = var1 + var2;
00060   T = (t_fine * 5 + 128) >> 8;
00061   return T;
00062 }
00063 
00064 // Returns pressure in Pa as unsigned 32 bit integer in Q24.8 format (24 integer bits and 8
00065 //fractional bits).
00066 //Output value of “24674867” represents 24674867/256 = 96386.2 Pa = 963.862 hPa
00067 uint32_t bmp280_compensate_P(int32_t adc_P)
00068 {
00069   long long var1, var2, p;
00070   var1 = ((long long)t_fine) - 128000;
00071   var2 = var1 * var1 * (long long)dig_P6;
00072   var2 = var2 + ((var1*(long long)dig_P5)<<17);
00073   var2 = var2 + (((long long)dig_P4)<<35);
00074   var1 = ((var1 * var1 * (long long)dig_P3)>>8) + ((var1 * (long long)dig_P2)<<12);
00075   var1 = (((((long long)1)<<47)+var1))*((long long)dig_P1)>>33;
00076   if(var1 == 0)
00077   {
00078     return 0;
00079     // avoid exception caused by division by zero
00080   }
00081   p = 1048576 - adc_P;
00082   p = (((p<<31) - var2)*3125)/var1;
00083   var1 = (((long long)dig_P9) * (p>>13) * (p>>13)) >> 25;
00084   var2 = (((long long)dig_P8) * p)>> 19;
00085   p = ((p + var1 + var2) >> 8) + (((long long)dig_P7)<<4);
00086   return (uint32_t)p;
00087 }
00088 
00089 void myinthandler() // interrupt handler
00090 {
00091   newData = true;
00092 }
00093         
00094         
00095 int main()
00096 {
00097   pc.baud(9600);  
00098   myled = 0; // turn off led
00099   
00100   wait(5);
00101   
00102   //Set up I2C
00103   i2c.frequency(400000);  // use fast (400 kHz) I2C  
00104    
00105   t.start(); // enable system timer
00106   
00107   myled = 1; // turn on led
00108     
00109   myInterrupt.rise(&myinthandler);  // define interrupt for INT pin output of MPU9250
00110   
00111   // Read the WHO_AM_I register, this is a good test of communication
00112   whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
00113   pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
00114   myled = 1;
00115   
00116   if (whoami == 0x71) // WHO_AM_I should always be 0x71
00117   {  
00118     pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00119     pc.printf("MPU9250 is online...\n\r");
00120     wait(1);
00121     
00122     mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00123 
00124     mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
00125     pc.printf("x-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[0]);  
00126     pc.printf("y-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[1]);  
00127     pc.printf("z-axis self test: acceleration trim within: %f pct of factory value\n\r", SelfTest[2]);  
00128     pc.printf("x-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[3]);  
00129     pc.printf("y-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[4]);  
00130     pc.printf("z-axis self test: gyration trim within: %f pct of factory value\n\r", SelfTest[5]);  
00131  
00132     mpu9250.getAres(); // Get accelerometer sensitivity
00133     mpu9250.getGres(); // Get gyro sensitivity
00134     mpu9250.getMres(); // Get magnetometer sensitivity
00135     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00136     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00137     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00138 
00139     mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00140     pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00141     pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00142     pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00143     pc.printf("x accel bias = %f\n\r", accelBias[0]);
00144     pc.printf("y accel bias = %f\n\r", accelBias[1]);
00145     pc.printf("z accel bias = %f\n\r", accelBias[2]);
00146     wait(2);
00147 
00148     mpu9250.initMPU9250(); 
00149     pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00150     wait(1);
00151 
00152     mpu9250.initAK8963(magCalibration);
00153     pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
00154     pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00155     pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00156     if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00157     if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00158     if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00159     if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00160 
00161     pc.printf("Mag Calibration: Wave device in a figure eight until done!");
00162     wait(4);
00163     mpu9250.magcalMPU9250(magBias, magScale);
00164     pc.printf("Mag Calibration done!\n\r");
00165     pc.printf("x mag bias = %f\n\r", magBias[0]);
00166     pc.printf("y mag bias = %f\n\r", magBias[1]);
00167     pc.printf("z mag bias = %f\n\r", magBias[2]);
00168     wait(2);
00169     }
00170     
00171    else
00172    
00173    {
00174     pc.printf("Could not connect to MPU9250: \n\r");
00175     pc.printf("%#x \n",  whoami);
00176     myled = 0;
00177  
00178     while(1) ; // Loop forever if communication doesn't happen
00179     }
00180     
00181     // Read the WHO_AM_I register of the BMP-280, this is a good test of communication
00182     uint8_t c = bmp280.readByte(BMP280_ADDRESS, BMP280_ID);   
00183     if(c == 0x58) {
00184  
00185     pc.printf("BMP-280 is 0x%x\n\r", c);
00186     pc.printf("BMP-280 should be 0x58\n\r");
00187     pc.printf("BMP-280 online...\n\r");
00188    
00189     //bmp280.BMP280Init();
00190     
00191   // Set T and P oversampling rates and sensor mode
00192   bmp280.writeByte(BMP280_ADDRESS, BMP280_CTRL_MEAS, Tosr << 5 | Posr << 2 | Mode);
00193   // Set standby time interval in normal mode and bandwidth
00194   bmp280.writeByte(BMP280_ADDRESS, BMP280_CONFIG, SBy << 5 | IIRFilter << 2);
00195   uint8_t calib[24];
00196   bmp280.readBytes(BMP280_ADDRESS, BMP280_CALIB00, 24, &calib[0]);
00197   dig_T1 = (uint16_t)(((uint16_t) calib[1] << 8) | calib[0]);
00198   dig_T2 = ( int16_t)((( int16_t) calib[3] << 8) | calib[2]);
00199   dig_T3 = ( int16_t)((( int16_t) calib[5] << 8) | calib[4]);
00200   dig_P1 = (uint16_t)(((uint16_t) calib[7] << 8) | calib[6]);
00201   dig_P2 = ( int16_t)((( int16_t) calib[9] << 8) | calib[8]);
00202   dig_P3 = ( int16_t)((( int16_t) calib[11] << 8) | calib[10]);
00203   dig_P4 = ( int16_t)((( int16_t) calib[13] << 8) | calib[12]);
00204   dig_P5 = ( int16_t)((( int16_t) calib[15] << 8) | calib[14]);
00205   dig_P6 = ( int16_t)((( int16_t) calib[17] << 8) | calib[16]);
00206   dig_P7 = ( int16_t)((( int16_t) calib[19] << 8) | calib[18]);
00207   dig_P8 = ( int16_t)((( int16_t) calib[21] << 8) | calib[20]);
00208   dig_P9 = ( int16_t)((( int16_t) calib[23] << 8) | calib[22]);
00209 
00210   pc.printf("dig_T1 is %d\n\r", dig_T1);
00211   pc.printf("dig_T2 is %d\n\r", dig_T2);
00212   pc.printf("dig_T3 is %d\n\r", dig_T3);
00213   pc.printf("dig_P1 is %d\n\r", dig_P1);
00214   pc.printf("dig_P2 is %d\n\r", dig_P2);
00215   pc.printf("dig_P3 is %d\n\r", dig_P3);
00216   pc.printf("dig_P4 is %d\n\r", dig_P4);
00217   pc.printf("dig_P5 is %d\n\r", dig_P5);
00218   pc.printf("dig_P6 is %d\n\r", dig_P6);
00219   pc.printf("dig_P7 is %d\n\r", dig_P7);
00220   pc.printf("dig_P8 is %d\n\r", dig_P8);
00221   pc.printf("dig_P9 is %d\n\r", dig_P9);
00222   
00223   pc.printf("BMP-280 calibration complete...\n\r");
00224   
00225    }
00226    
00227    else 
00228    
00229    {
00230     pc.printf("BMP-280 is 0x%x\n\r", c);
00231     pc.printf("BMP-280 should be 0x55\n\r");
00232     while(1); // idle here forever
00233    }
00234    
00235  /* Main Loop*/
00236  while(1) {
00237   
00238   // If intPin goes high, all data registers have new data
00239   // if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // OUse polling to check for data ready  
00240    if(newData){   // wait for interrupt for data ready
00241     newData = false;  // reset newData flag
00242     
00243     mpu9250.readMPU9250Data(MPU9250Data); // INT cleared on any read
00244 
00245 //    mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
00246     // Now we'll calculate the accleration value into actual g's
00247     ax = (float)MPU9250Data[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00248     ay = (float)MPU9250Data[1]*aRes - accelBias[1];   
00249     az = (float)MPU9250Data[2]*aRes - accelBias[2];  
00250         
00251  //   mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00252     // Calculate the gyro value into actual degrees per second
00253     gx = (float)MPU9250Data[4]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00254     gy = (float)MPU9250Data[5]*gRes - gyroBias[1]; 
00255     gz = (float)MPU9250Data[6]*gRes - gyroBias[2];   
00256   
00257    }
00258  
00259     if(mpu9250.readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) {
00260 
00261    mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
00262     // Calculate the magnetometer values in milliGauss
00263     // Include factory calibration per data sheet and user environmental corrections
00264     mx = (float)magCount[0]*mRes*magCalibration[0] - magBias[0];  // get actual magnetometer value, this depends on scale being set
00265     my = (float)magCount[1]*mRes*magCalibration[1] - magBias[1];  
00266     mz = (float)magCount[2]*mRes*magCalibration[2] - magBias[2];  
00267     mx *= magScale[0]; // poor man's soft iron calibration
00268     my *= magScale[1];
00269     mz *= magScale[2];  
00270    }
00271    
00272     Now = t.read_us();
00273     deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00274     lastUpdate = Now;
00275     
00276     sum += deltat;
00277     sumCount++;
00278     
00279    // Pass gyro rate as rad/s
00280   mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00281 //  mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00282 
00283     // Serial print and/or display at 1 s rate independent of data rates
00284     delt_t = t.read_ms() - count;
00285     if (delt_t > 1000) { // update LCD once per second independent of read rate
00286 
00287     pc.printf("ax = %f", 1000*ax); 
00288     pc.printf(" ay = %f", 1000*ay); 
00289     pc.printf(" az = %f  mg\n\r", 1000*az); 
00290 
00291     pc.printf("gx = %f", gx); 
00292     pc.printf(" gy = %f", gy); 
00293     pc.printf(" gz = %f  deg/s\n\r", gz); 
00294     
00295     pc.printf("mx = %f", mx); 
00296     pc.printf(" my = %f", my); 
00297     pc.printf(" mz = %f  mG\n\r", mz); 
00298     
00299  //   tempCount = mpu9250.readTempData();  // Read the adc values
00300     temperature = ((float) MPU9250Data[3]) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00301     pc.printf("gyro temperature = %f  C\n\r", temperature); 
00302     
00303     pc.printf("q0, q1, q2, q3 = %f %f %f %f\n\r",q[0], q[1], q[2], q[3]);
00304     
00305     rawPress =  readBMP280Pressure();
00306     Pressure = (float) bmp280_compensate_P(rawPress)/25600.0f; // Pressure in mbar
00307     rawTemp =   readBMP280Temperature();
00308     Temperature = (float) bmp280_compensate_T(rawTemp)/100.0f;
00309 
00310     float altitude = 145366.45f*(1.0f - powf(Pressure/1013.25f, 0.190284f) );
00311     pc.printf("Temperature = %f C\n\r", Temperature);
00312     pc.printf("Pressure = %f Pa\n\r", Pressure); 
00313     pc.printf("Altitude = %f feet\n\r", altitude); 
00314 
00315   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00316   // In this coordinate system, the positive z-axis is down toward Earth. 
00317   // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00318   // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00319   // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00320   // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00321   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00322   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00323   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00324     yaw   = atan2f(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
00325     pitch = -asinf(2.0f * (q[1] * q[3] - q[0] * q[2]));
00326     roll  = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00327     pitch *= 180.0f / PI;
00328     yaw   *= 180.0f / PI; 
00329     yaw   += 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00330     roll  *= 180.0f / PI;
00331 
00332     pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00333     pc.printf("average rate = %f Hz \n\r", (float) sumCount/sum);
00334     
00335     myled= !myled;
00336     count = t.read_ms(); 
00337 
00338     if(count > 1<<21) {
00339         t.start(); // start the timer over again if ~30 minutes has passed
00340         count = 0;
00341         deltat= 0;
00342         lastUpdate = t.read_us();
00343     }
00344     sum = 0;
00345     sumCount = 0; 
00346 }
00347 
00348 }
00349  
00350 }