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: mbed
Fork of MPU9250AHRS by
Diff: MPU9250.h
- Revision:
- 3:3e04c1c03cab
- Parent:
- 2:4e59a37182df
- Child:
- 5:d31487b34216
--- a/MPU9250.h Tue Aug 05 01:37:23 2014 +0000 +++ b/MPU9250.h Fri Dec 18 12:59:56 2015 +0000 @@ -216,8 +216,8 @@ float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value -#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral -#define Ki 0.0f +#define Kp 5.1f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral +#define Ki 1.0f float pitch, yaw, roll; float deltat = 0.0f; // integration interval for both filter schemes @@ -625,7 +625,7 @@ // 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 + //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 @@ -648,7 +648,7 @@ // 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 + //delay(25); // Delay a while to let the device stabilize // 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 @@ -659,18 +659,18 @@ 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 + factoryTrim[0] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[0] - (float)1.0) )); // FT[Xa] factory trim calculation + factoryTrim[1] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[1] - (float)1.0) )); // FT[Ya] factory trim calculation + factoryTrim[2] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[2] - (float)1.0) )); // FT[Za] factory trim calculation + factoryTrim[3] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[3] - (float)1.0) )); // FT[Xg] factory trim calculation + factoryTrim[4] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[4] - (float)1.0) )); // FT[Yg] factory trim calculation + factoryTrim[5] = (float)(2620/1<<FS)*(pow( (float)1.01 , ((float)selfTest[5] - (float)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 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 + destination[i] = (float)100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i]; // Report percent differences + destination[i+3] = (float)100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3]; // Report percent differences } }