Merge pull request #5219 from betaflight/revert-5200-gyro_overflow_ch…

…eck_use_filtered_gyro Revert "Change gyro_overflow_detect to use filtered instead of raw gyro data"
betaflight · Feb 17, 2018 · 3656963 · 3656963
2 parents 3fbff5a + e5364ee
commit 3656963
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 26 deletions.
diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/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};

diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -211,6 +211,7 @@ typedef struct mpuDetectionResult_s {
 
 struct gyroDev_s;
 void mpuGyroInit(struct gyroDev_s *gyro);
+gyroOverflow_e mpuGyroCheckOverflow(const struct gyroDev_s *gyro);
 bool mpuGyroRead(struct gyroDev_s *gyro);
 bool mpuGyroReadSPI(struct gyroDev_s *gyro);
 void mpuDetect(struct gyroDev_s *gyro);

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -153,9 +153,6 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 #define GYRO_SYNC_DENOM_DEFAULT 4
 #endif
 
-#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps)
-#define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps)
-
 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
@@ -731,9 +728,13 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
     // 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) {
-        if (abs(gyroSensor->gyroDev.gyroADC[X]) < GYRO_OVERFLOW_RESET_THRESHOLD
-              && abs(gyroSensor->gyroDev.gyroADC[Y]) < GYRO_OVERFLOW_RESET_THRESHOLD
-              && abs(gyroSensor->gyroDev.gyroADC[Z]) < GYRO_OVERFLOW_RESET_THRESHOLD) {
+        const float gyroRateX = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale;
+        const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale;
+        const float gyroRateZ = 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
             // reset requires good OK values on all axes
             if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
@@ -743,25 +744,14 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
             // not a consecutive OK value, so reset the overflow time
             gyroSensor->overflowTimeUs = currentTimeUs;
         }
-    } else {
+    }
 #ifndef SIMULATOR_BUILD
-        // check for overflow in the axes set in overflowAxisMask
-        gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
-        if (abs(gyroSensor->gyroDev.gyroADC[X]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
-            overflowCheck |= GYRO_OVERFLOW_X;
-        }
-        if (abs(gyroSensor->gyroDev.gyroADC[Y]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
-            overflowCheck |= GYRO_OVERFLOW_Y;
-        }
-        if (abs(gyroSensor->gyroDev.gyroADC[Z]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) {
-            overflowCheck |= GYRO_OVERFLOW_Z;
-        }
-        if (overflowCheck & overflowAxisMask) {
-            gyroSensor->overflowDetected = true;
-            gyroSensor->overflowTimeUs = currentTimeUs;
-        }
-#endif // SIMULATOR_BUILD
+    // check for overflow in the axes set in overflowAxisMask
+    if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
+        gyroSensor->overflowDetected = true;
+        gyroSensor->overflowTimeUs = currentTimeUs;
     }
+#endif // SIMULATOR_BUILD
 #else
     UNUSED(gyroSensor);
     UNUSED(currentTimeUs);
@@ -809,6 +799,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
@@ -876,9 +869,6 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
             }
         }
     }
-    if (gyroConfig()->checkOverflow) {
-        checkForOverflow(gyroSensor, currentTimeUs);
-    }
 }
 
 FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)