Dynamic kP gain for IMU calculations

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,10 +294,13 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
 
     // Compute and apply integral feedback if enabled
     if(imuRuntimeConfig->dcm_ki > 0.0f) {
-        float dcmKiGain = imuRuntimeConfig->dcm_ki;
+        // Stop integrating if spinning beyond the certain limit
-        integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
+        if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
-        integralFBy += dcmKiGain * ey * dt;
+            float dcmKiGain = imuRuntimeConfig->dcm_ki;
-        integralFBz += dcmKiGain * ez * dt;
+            integralFBx += dcmKiGain * ex * dt;    // integral error scaled by Ki
+            integralFBy += dcmKiGain * ey * dt;
+            integralFBz += dcmKiGain * ez * dt;
+        }
     }
     else {
         integralFBx = 0.0f;    // prevent integral windup
@@ -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;

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];