Merge pull request #4827 from martinbudden/bfa_gyro_overflow_options

Add option to check gyro overflow on yaw or all axes

src/main/interface/settings.c

@@ -274,6 +274,12 @@ static const char * const lookupTableMax7456Clock[] = {
 };
 #endif
 
+#ifdef USE_GYRO_OVERFLOW_CHECK
+static const char * const lookupTableGyroOverflowCheck[] = {
+    "OFF", "YAW", "ALL"
+};
+#endif
+
 const lookupTableEntry_t lookupTables[] = {
     { lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) },
     { lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) },
@@ -327,6 +333,9 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_RANGEFINDER
     { lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) },
 #endif
+#ifdef USE_GYRO_OVERFLOW_CHECK
+    { lookupTableGyroOverflowCheck, sizeof(lookupTableGyroOverflowCheck) / sizeof(char *) },
+#endif
 };
 
 const clivalue_t valueTable[] = {
@@ -344,7 +353,9 @@ 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) },
+#ifdef USE_GYRO_OVERFLOW_CHECK
+    { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
+#endif
 #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/interface/settings.h

@@ -74,6 +74,9 @@ typedef enum {
 #endif
 #ifdef USE_RANGEFINDER
     TABLE_RANGEFINDER_HARDWARE,
+#endif
+#ifdef USE_GYRO_OVERFLOW_CHECK
+    TABLE_GYRO_OVERFLOW_CHECK,
 #endif
     LOOKUP_TABLE_COUNT
 

src/main/sensors/gyro.c

@@ -77,6 +77,9 @@
 FAST_RAM gyro_t gyro;
 static FAST_RAM uint8_t gyroDebugMode;
 
+#ifdef USE_GYRO_OVERFLOW_CHECK
+static FAST_RAM uint8_t overflowAxisMask;
+#endif
 static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
 static FAST_RAM float gyroPrevious[XYZ_AXIS_COUNT];
 static FAST_RAM timeUs_t accumulatedMeasurementTimeUs;
@@ -145,7 +148,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_soft_notch_cutoff_1 = 300,
     .gyro_soft_notch_hz_2 = 200,
     .gyro_soft_notch_cutoff_2 = 100,
-    .checkOverflow = true
+    .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES
 );
 
 
@@ -387,6 +390,16 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 
 bool gyroInit(void)
 {
+#ifdef USE_GYRO_OVERFLOW_CHECK
+    if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
+        overflowAxisMask = GYRO_OVERFLOW_Z;
+    } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
+        overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
+    } else {
+        overflowAxisMask = 0;
+    }
+#endif
+
     switch (debugMode) {
     case DEBUG_FFT:
     case DEBUG_GYRO_NOTCH:
@@ -627,6 +640,7 @@ FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 
 static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 {
+#ifdef USE_GYRO_OVERFLOW_CHECK
     // 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.
@@ -640,6 +654,7 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
               && 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) {
                 gyroSensor->overflowDetected = false;
             }
@@ -649,11 +664,16 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
         }
     }
 #ifndef SIMULATOR_BUILD
-    if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) != GYRO_OVERFLOW_NONE) {
+    // check for overflow in the axes set in overflowAxisMask
+    if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) {
         gyroSensor->overflowDetected = true;
         gyroSensor->overflowTimeUs = currentTimeUs;
     }
-#endif
+#endif // SIMULATOR_BUILD
+#else
+    UNUSED(gyroSensor);
+    UNUSED(currentTimeUs);
+#endif // USE_GYRO_OVERFLOW_CHECK
 }
 
 static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)

src/main/sensors/gyro.h

@@ -49,6 +49,12 @@ typedef struct gyro_s {
 
 extern gyro_t gyro;
 
+typedef enum {
+    GYRO_OVERFLOW_CHECK_NONE = 0,
+    GYRO_OVERFLOW_CHECK_YAW,
+    GYRO_OVERFLOW_CHECK_ALL_AXES
+} gyroOverflowCheck_e;
+
 typedef struct gyroConfig_s {
     sensor_align_e gyro_align;              // gyro alignment
     uint8_t  gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
@@ -63,8 +69,7 @@ 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;
+    gyroOverflowCheck_e checkOverflow;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/target/common_fc_pre.h

@@ -146,6 +146,7 @@
 #define VTX_SMARTAUDIO
 #define VTX_TRAMP
 #define USE_CAMERA_CONTROL
+#define USE_GYRO_OVERFLOW_CHECK
 #define USE_HUFFMAN
 #define USE_COPY_PROFILE_CMS_MENU
 #define USE_MSP_OVER_TELEMETRY