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
    }
    
 }