Kris Winer / Mbed 2 deprecated MPU9250AHRS

Basic program to obtain properly-scaled gyro, accelerometer, and magnetometer data from the MPU-9250 9-axis motion sensor and do 9 DoF sensor fusion using the open-source Madgwick and Mahony sensor fusion filters. Running on STM32F401RE Nucleo board at 84 MHz achieves sensor fusion filter update rates of ~5000 Hz.

Dependencies:   ST_401_84MHZ mbed

Files at this revision

API Documentation at this revision

Revision 2:4e59a37182df, committed 2014-08-05

Comitter:
onehorse
Date:
Tue Aug 05 01:37:23 2014 +0000
Parent:
1:71c319f03fda
Commit message:
Corrected self test

Changed in this revision

MPU9250.h Show annotated file Show diff for this revision Revisions of this file 
diff -r 71c319f03fda -r 4e59a37182df MPU9250.h
--- a/MPU9250.h	Tue Aug 05 01:34:45 2014 +0000
+++ b/MPU9250.h	Tue Aug 05 01:37:23 2014 +0000
@@ -592,44 +592,85 @@
 // Accelerometer and gyroscope self test; check calibration wrt factory settings
 void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
 {
-   uint8_t rawData[4] = {0, 0, 0, 0};
+   uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
    uint8_t selfTest[6];
+   int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3];
    float factoryTrim[6];
+   uint8_t FS = 0;
    
-   // Configure the accelerometer for self-test
-   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g
-   writeByte(MPU9250_ADDRESS, GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
-   wait(0.25);  // Delay a while to let the device execute the self-test
-   rawData[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis self-test results
-   rawData[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis self-test results
-   rawData[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis self-test results
-   rawData[3] = readByte(MPU9250_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results
-   // Extract the acceleration test results first
-   selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer
-   selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer
-   selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer
-   // Extract the gyration test results first
-   selfTest[3] = rawData[0]  & 0x1F ; // XG_TEST result is a five-bit unsigned integer
-   selfTest[4] = rawData[1]  & 0x1F ; // YG_TEST result is a five-bit unsigned integer
-   selfTest[5] = rawData[2]  & 0x1F ; // ZG_TEST result is a five-bit unsigned integer   
-   // Process results to allow final comparison with factory set values
-   factoryTrim[0] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[0] - 1.0f)/30.0f))); // FT[Xa] factory trim calculation
-   factoryTrim[1] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[1] - 1.0f)/30.0f))); // FT[Ya] factory trim calculation
-   factoryTrim[2] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[2] - 1.0f)/30.0f))); // FT[Za] factory trim calculation
-   factoryTrim[3] =  ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[3] - 1.0f) ));             // FT[Xg] factory trim calculation
-   factoryTrim[4] =  (-25.0f*131.0f)*(pow( 1.046f , (selfTest[4] - 1.0f) ));             // FT[Yg] factory trim calculation
-   factoryTrim[5] =  ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[5] - 1.0f) ));             // FT[Zg] factory trim calculation
+  writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
+  writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
+  writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
+  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
+  writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
+
+  for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
+  
+  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+  aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+  aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+  aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  
+    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+  gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+  gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+  gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  }
+  
+  for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
+  aAvg[ii] /= 200;
+  gAvg[ii] /= 200;
+  }
+  
+// Configure the accelerometer for self-test
+   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
+   writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+   delay(25); // Delay a while to let the device stabilize
+
+  for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
+  
+  readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+  aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+  aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+  aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  
+    readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+  gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+  gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+  gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+  }
+  
+  for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
+  aSTAvg[ii] /= 200;
+  gSTAvg[ii] /= 200;
+  }
+  
+ // Configure the gyro and accelerometer for normal operation
+   writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
+   writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
+   delay(25); // Delay a while to let the device stabilize
    
- //  Output self-test results and factory trim calculation if desired
- //  Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
- //  Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
- //  Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
- //  Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);
+   // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+   selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
+   selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
+   selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
+   selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
+   selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
+   selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
 
+  // Retrieve factory self-test value from self-test code reads
+   factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
+   factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
+   factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
+   factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
+   factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
+   factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
+ 
  // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
- // To get to percent, must multiply by 100 and subtract result from 100
-   for (int i = 0; i < 6; i++) {
-     destination[i] = 100.0f + 100.0f*(selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences
+ // To get percent, must multiply by 100
+   for (int i = 0; i < 3; i++) {
+     destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences
+     destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences
    }
    
 }