Merge remote-tracking branch 'digitalentity/betaflight-imu-fixes' into betaflight

src/main/blackbox/blackbox.c

@@ -206,6 +206,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
     {"accSmooth",  0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"accSmooth",  1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     {"accSmooth",  2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+    {"attitude",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+    {"attitude",   1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
+    {"attitude",   2, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
     /* Motors only rarely drops under minthrottle (when stick falls below mincommand), so predict minthrottle for it and use *unsigned* encoding (which is large for negative numbers but more compact for positive ones): */
     {"motor",      0, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(AVERAGE_2), .Pencode = ENCODING(SIGNED_VB), CONDITION(AT_LEAST_MOTORS_1)},
     /* Subsequent motors base their I-frame values on the first one, P-frame values on the average of last two frames: */
@@ -275,6 +278,7 @@ typedef struct blackboxMainState_s {
     int16_t rcCommand[4];
     int16_t gyroADC[XYZ_AXIS_COUNT];
     int16_t accSmooth[XYZ_AXIS_COUNT];
+    int16_t attitude[XYZ_AXIS_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
     int16_t servo[MAX_SUPPORTED_SERVOS];
 
@@ -549,6 +553,7 @@ static void writeIntraframe(void)
 
     blackboxWriteSigned16VBArray(blackboxCurrent->gyroADC, XYZ_AXIS_COUNT);
     blackboxWriteSigned16VBArray(blackboxCurrent->accSmooth, XYZ_AXIS_COUNT);
+    blackboxWriteSigned16VBArray(blackboxCurrent->attitude, XYZ_AXIS_COUNT);
 
     //Motors can be below minthrottle when disarmed, but that doesn't happen much
     blackboxWriteUnsignedVB(blackboxCurrent->motor[0] - masterConfig.escAndServoConfig.minthrottle);
@@ -675,6 +680,7 @@ static void writeInterframe(void)
     //Since gyros, accs and motors are noisy, base their predictions on the average of the history:
     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, gyroADC),   XYZ_AXIS_COUNT);
     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, accSmooth), XYZ_AXIS_COUNT);
+    blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, attitude), XYZ_AXIS_COUNT);
     blackboxWriteMainStateArrayUsingAveragePredictor(offsetof(blackboxMainState_t, motor),     motorCount);
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_TRICOPTER)) {
@@ -923,6 +929,10 @@ static void loadMainState(void)
         blackboxCurrent->accSmooth[i] = accSmooth[i];
     }
 
+    blackboxCurrent->attitude[0] = attitude.values.roll;
+    blackboxCurrent->attitude[0] = attitude.values.pitch;
+    blackboxCurrent->attitude[0] = attitude.values.yaw;
+
     for (i = 0; i < motorCount; i++) {
         blackboxCurrent->motor[i] = motor[i];
     }

src/main/config/config.c

@@ -396,7 +396,7 @@ static void resetConf(void)
     // global settings
     masterConfig.current_profile_index = 0;     // default profile
     masterConfig.dcm_kp = 10000;                // 1.0 * 10000
-    masterConfig.dcm_ki = 30;                   // 0.003 * 10000
+    masterConfig.dcm_ki = 0;                    // 0.003 * 10000
 
     resetAccelerometerTrims(&masterConfig.accZero);
 

src/main/flight/imu.c

@@ -50,6 +50,12 @@
 
 #include "config/runtime_config.h"
 
+// the limit (in degrees/second) beyond which we stop integrating
+// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
+// which results in false gyro drift. See
+// http://gentlenav.googlecode.com/files/fastRotations.pdf
+#define SPIN_RATE_LIMIT 20
+
 int16_t accSmooth[XYZ_AXIS_COUNT];
 int32_t accSum[XYZ_AXIS_COUNT];
 
@@ -209,6 +215,19 @@ static bool imuUseFastGains(void)
     return !ARMING_FLAG(ARMED) && millis() < 20000;
 }
 
+// Taken from http://gentlenav.googlecode.com/files/fastRotations.pdf
+static float imuGetPGainScaleFactor(float spin_rate)
+{
+    if (spin_rate < DEGREES_TO_RADIANS(50)) {
+        return 1.0f;
+    }
+    else if (spin_rate > DEGREES_TO_RADIANS(500)) {
+        return 10.0f;
+    }
+
+    return spin_rate / DEGREES_TO_RADIANS(50);
+}
+
 static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
                                 bool useAcc, float ax, float ay, float az,
                                 bool useMag, float mx, float my, float mz,
@@ -220,6 +239,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     float ex = 0, ey = 0, ez = 0;
     float qa, qb, qc;
 
+    // Calculate general spin rate (rad/s)
+    float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
+
     // Use raw heading error (from GPS or whatever else)
     if (useYaw) {
         while (yawError >  M_PIf) yawError -= (2.0f * M_PIf);
@@ -272,11 +294,14 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
     // Compute and apply integral feedback if enabled
     if(imuRuntimeConfig->dcm_ki > 0.0f) {
+        // Stop integrating if spinning beyond the certain limit
+        if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
             float dcmKiGain = imuRuntimeConfig->dcm_ki;
             integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
             integralFBy += dcmKiGain * ey * dt;
             integralFBz += dcmKiGain * ez * dt;
         }
+    }
     else {
         integralFBx = 0.0f;    // prevent integral windup
         integralFBy = 0.0f;
@@ -288,6 +313,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
     if (imuUseFastGains()) {
         dcmKpGain *= 10;
     }
+    else {
+        dcmKpGain *= imuGetPGainScaleFactor(spin_rate);
+    }
 
     // Apply proportional and integral feedback
     gx += dcmKpGain * ex + integralFBx;
@@ -342,13 +370,13 @@ static bool imuIsAccelerometerHealthy(void)
     int32_t accMagnitude = 0;
 
     for (axis = 0; axis < 3; axis++) {
-        accMagnitude += (int32_t)accADC[axis] * accADC[axis];
+        accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
     }
 
     accMagnitude = accMagnitude * 100 / ((int32_t)acc_1G * acc_1G);
 
-    // Accept accel readings only in range 0.85g - 1.15g
+    // Accept accel readings only in range 0.90g - 1.10g
-    return (72 < accMagnitude) && (accMagnitude < 133);
+    return (81 < accMagnitude) && (accMagnitude < 121);
 }
 
 static bool isMagnetometerHealthy(void)
@@ -370,8 +398,6 @@ static void imuCalculateEstimatedAttitude(void)
     uint32_t deltaT = currentTime - previousIMUUpdateTime;
     previousIMUUpdateTime = currentTime;
 
-    // Smooth and use only valid accelerometer readings
-    if (imuIsAccelerometerHealthy()) {
     // If reading is considered valid - apply filter
     for (axis = 0; axis < 3; axis++) {
         if (imuRuntimeConfig->acc_cut_hz > 0) {
@@ -381,6 +407,8 @@ static void imuCalculateEstimatedAttitude(void)
         }
     }
 
+    // Smooth and use only valid accelerometer readings
+    if (imuIsAccelerometerHealthy()) {
         useAcc = true;
     }
 

src/main/flight/imu.h

@@ -27,6 +27,7 @@ extern int32_t accSum[XYZ_AXIS_COUNT];
 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)
 #define DECIDEGREES_TO_DEGREES(angle) (angle / 10)
 #define DECIDEGREES_TO_RADIANS(angle) ((angle / 10.0f) * 0.0174532925f)
+#define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
 
 typedef union {
     int16_t raw[XYZ_AXIS_COUNT];

src/main/io/serial_cli.c

@@ -416,8 +416,8 @@ const clivalue_t valueTable[] = {
     { "max_angle_inclination",      VAR_UINT16 | MASTER_VALUE,  &masterConfig.max_angle_inclination, 100, 1200 },
 
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE,  &masterConfig.gyroConfig.gyroMovementCalibrationThreshold, 0, 128 },
-    { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, 0, 20000 },
+    { "imu_dcm_kp",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_kp, 0, 50000 },
-    { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_ki, 0, 20000 },
+    { "imu_dcm_ki",                 VAR_UINT16 | MASTER_VALUE,  &masterConfig.dcm_ki, 0, 50000 },
 
     { "alt_hold_deadband",          VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_deadband, 1, 250 },
     { "alt_hold_fast_change",       VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].rcControlsConfig.alt_hold_fast_change, 0, 1 },