diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index b389756254..34f91328d2 100644 --- a/src/main/interface/settings.c +++ b/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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index 11106ed4c1..0a80061e0d 100644 --- a/src/main/interface/settings.h +++ b/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 diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 17c8320ceb..90a2a4f1e7 100644 --- a/src/main/sensors/gyro.c +++ b/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) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index f44418c91c..6ac9377300 100644 --- a/src/main/sensors/gyro.h +++ b/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); diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 640c1ec3aa..d96eea06bd 100644 --- a/src/main/target/common_fc_pre.h +++ b/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