Added gyro overflow checking and handling. Helps avoid YSTTM

src/main/drivers/accgyro/accgyro_mpu.c

@@ -183,6 +183,23 @@ bool mpuGyroRead(gyroDev_t *gyro)
     return true;
 }
 
+gyroOverflow_e mpuGyroCheckOverflow(const gyroDev_t *gyro)
+{
+    // we cannot detect overflow directly, so assume overflow if absolute gyro rate is large
+    gyroOverflow_e ret = GYRO_OVERFLOW_NONE;
+    const int16_t overflowValue = 0x7C00; // this is a slightly conservative value, could probably be as high as 0x7FF0
+    if (gyro->gyroADCRaw[X] > overflowValue || gyro->gyroADCRaw[X] < -overflowValue) {
+        ret |= GYRO_OVERFLOW_X;
+    }
+    if (gyro->gyroADCRaw[Y] > overflowValue || gyro->gyroADCRaw[Y] < -overflowValue) {
+        ret |= GYRO_OVERFLOW_Y;
+    }
+    if (gyro->gyroADCRaw[Z] > overflowValue || gyro->gyroADCRaw[Z] < -overflowValue) {
+        ret |= GYRO_OVERFLOW_Z;
+    }
+    return ret;
+}
+
 bool mpuGyroReadSPI(gyroDev_t *gyro)
 {
     static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};

src/main/drivers/accgyro/accgyro_mpu.h

@@ -176,6 +176,13 @@ enum accel_fsr_e {
     NUM_ACCEL_FSR
 };
 
+typedef enum {
+    GYRO_OVERFLOW_NONE = 0x00,
+    GYRO_OVERFLOW_X = 0x01,
+    GYRO_OVERFLOW_Y = 0x02,
+    GYRO_OVERFLOW_Z = 0x04
+} gyroOverflow_e;
+
 typedef enum {
     MPU_NONE,
     MPU_3050,
@@ -204,8 +211,10 @@ typedef struct mpuDetectionResult_s {
 
 struct gyroDev_s;
 void mpuGyroInit(struct gyroDev_s *gyro);
-struct accDev_s;
+gyroOverflow_e mpuGyroCheckOverflow(const struct gyroDev_s *gyro);
-bool mpuAccRead(struct accDev_s *acc);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadSPI(struct gyroDev_s *gyro);
 void mpuDetect(struct gyroDev_s *gyro);
+
+struct accDev_s;
+bool mpuAccRead(struct accDev_s *acc);

src/main/flight/pid.c

@@ -440,21 +440,7 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
                 BEEP_ON;
             }
             if (axis == FD_YAW) {
-                // on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate
+                errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
-#if !(defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_ICM20649))
-#define GYRO_POTENTIAL_OVERFLOW_RATE 1990.0f
-                if (gyroRate > GYRO_POTENTIAL_OVERFLOW_RATE || gyroRate < -GYRO_POTENTIAL_OVERFLOW_RATE) {
-                    // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
-                    // This can cause an overflow and sign reversal in the output.
-                    // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
-                    // If there is a sign reversal we will actually increase crash-induced yaw spin
-                    // so best thing to do is set error to zero.
-                    errorRate = 0.0f;
-                } else
-#endif
-                {
-                    errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw);
-                }
             } else {
                 // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash
                 if (sensors(SENSOR_ACC)) {
@@ -522,8 +508,9 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
 
             previousRateError[axis] = rD;
 
-            // if crash recovery is on and accelerometer enabled then check for a crash
+            // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
-            if (pidProfile->crash_recovery) {
+            // no point in trying to recover if the crash is so severe that the gyro overflows
+            if (pidProfile->crash_recovery && !gyroOverflowDetected()) {
                 if (ARMING_FLAG(ARMED)) {
                     if (motorMixRange >= 1.0f && !inCrashRecoveryMode
                         && ABS(delta) > crashDtermThreshold
@@ -545,8 +532,8 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT
             axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
         }
 
-        // Disable PID control at zero throttle
+        // Disable PID control if at zero throttle or if gyro overflow detected
-        if (!pidStabilisationEnabled) {
+        if (!pidStabilisationEnabled || gyroOverflowDetected()) {
             axisPID_P[axis] = 0;
             axisPID_I[axis] = 0;
             axisPID_D[axis] = 0;

src/main/interface/settings.c

@@ -332,6 +332,7 @@ const clivalue_t valueTable[] = {
     { "gyro_notch2_hz",             VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
     { "gyro_notch2_cutoff",         VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
     { "moron_threshold",            VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
+    { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
 #if defined(GYRO_USES_SPI)
 #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
     { "gyro_use_32khz",             VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) },

src/main/sensors/gyro.c

@@ -110,6 +110,8 @@ typedef struct gyroSensor_s {
     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
     filterApplyFnPtr notchFilterDynApplyFn;
     biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
+    timeUs_t overflowTimeUs;
+    bool overflowDetected;
 } gyroSensor_t;
 
 static FAST_RAM gyroSensor_t gyroSensor1;
@@ -127,6 +129,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 #define GYRO_SYNC_DENOM_DEFAULT 4
 #endif
 
+#if (defined(USE_GYRO_SPI_MPU6500) \
+  || defined(USE_GYRO_SPI_MPU9250) \
+  || defined(USE_GYRO_SPI_ICM20601) \
+  || defined(USE_GYRO_SPI_ICM20649) \
+  || defined(USE_GYRO_SPI_ICM20689))
+#define GYRO_CHECK_OVERFLOW_DEFAULT  true
+#else
+#define GYRO_CHECK_OVERFLOW_DEFAULT  false
+#endif
+
 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
@@ -142,7 +154,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_soft_notch_hz_1 = 400,
     .gyro_soft_notch_cutoff_1 = 300,
     .gyro_soft_notch_hz_2 = 200,
-    .gyro_soft_notch_cutoff_2 = 100
+    .gyro_soft_notch_cutoff_2 = 100,
+    .checkOverflow = GYRO_CHECK_OVERFLOW_DEFAULT
 );
 
 
@@ -607,16 +620,52 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
 #if defined(USE_GYRO_SLEW_LIMITER)
 FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 {
-    int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
+    int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
-    if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
+    if (gyroConfig()->checkOverflow) {
-        newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
+        // don't use the slew limiter if overflow checking is on
-    } else {
+        return ret;
-        gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro;
     }
-    return newRawGyro;
+    if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
+        // there has been a large change in value, so assume overflow has occurred and return the previous value
+        ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
+    } else {
+        gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
+    }
+    return ret;
 }
 #endif
 
+static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
+{
+    // check for overflow to handle Yaw Spin To The Moon (YSTTM)
+    // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
+    // This can cause an overflow and sign reversal in the output.
+    // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
+    if (gyroSensor->overflowDetected) {
+        const float gyroRateX = (float)gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
+        const float gyroRateY = (float)gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
+        const float gyroRateZ = (float)gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale;
+        static const int overflowResetThreshold = 1800;
+        if (abs(gyroRateX) < overflowResetThreshold
+              && abs(gyroRateY) < overflowResetThreshold
+              && abs(gyroRateZ) < overflowResetThreshold) {
+            // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
+            if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
+                gyroSensor->overflowDetected = false;
+            }
+        } else {
+            // not a consecutive OK value, so reset the overflow time
+            gyroSensor->overflowTimeUs = currentTimeUs;
+        }
+    }
+#ifndef SIMULATOR_BUILD
+    if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) != GYRO_OVERFLOW_NONE) {
+        gyroSensor->overflowDetected = true;
+        gyroSensor->overflowTimeUs = currentTimeUs;
+    }
+#endif
+}
+
 static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 {
     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
@@ -654,6 +703,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
     accumulationLastTimeSampledUs = currentTimeUs;
     accumulatedMeasurementTimeUs += sampleDeltaUs;
 
+    if (gyroConfig()->checkOverflow) {
+        checkForOverflow(gyroSensor, currentTimeUs);
+    }
     if (gyroDebugMode == DEBUG_NONE) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
@@ -665,9 +717,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
             gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
             gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
             gyro.gyroADCf[axis] = gyroADCf;
-            // integrate using trapezium rule to avoid bias
+            if (!gyroSensor->overflowDetected) {
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
+                // integrate using trapezium rule to avoid bias
-            gyroPrevious[axis] = gyroADCf;
+                accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
+                gyroPrevious[axis] = gyroADCf;
+            }
         }
     } else {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -699,9 +753,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
             gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
 
             gyro.gyroADCf[axis] = gyroADCf;
-            // integrate using trapezium rule to avoid bias
+            if (!gyroSensor->overflowDetected) {
-            accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
+                // integrate using trapezium rule to avoid bias
-            gyroPrevious[axis] = gyroADCf;
+                accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
+                gyroPrevious[axis] = gyroADCf;
+            }
         }
     }
 }
@@ -745,3 +801,8 @@ int16_t gyroRateDps(int axis)
 {
     return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
 }
+
+bool gyroOverflowDetected(void)
+{
+    return gyroSensor1.overflowDetected;
+}

src/main/sensors/gyro.h

@@ -63,6 +63,8 @@ typedef struct gyroConfig_s {
     uint16_t gyro_soft_notch_cutoff_1;
     uint16_t gyro_soft_notch_hz_2;
     uint16_t gyro_soft_notch_cutoff_2;
+    uint16_t overflowResetThreshold;
+    bool checkOverflow;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);
@@ -83,3 +85,4 @@ bool isGyroCalibrationComplete(void);
 void gyroReadTemperature(void);
 int16_t gyroGetTemperature(void);
 int16_t gyroRateDps(int axis);
+bool gyroOverflowDetected(void);