From f411b82eb6e58cfc3cb731eb465e289b190c138f Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 29 Jul 2018 11:21:12 -0400 Subject: [PATCH 1/3] Disable DYNAMIC_FILTER feature if gyro loop is less than 2KHz The dynamic filter is not usefull with gyro loop times less than 2KHz so disable the feature and prevent the code from running in this circumstance. In `isDynamicFilterActive()` we can't just rely on the feature because the feature will be disabled after the gyro has initialized and is running - possibly running into the underflow/wedging problem. And unfortunately we can't disable the feature earlier because we need to know the `targetLooptime` which is determined during gyro init. --- src/main/fc/config.c | 5 +++++ src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 ++ 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 28475b3f4..f431fbb1b 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -410,6 +410,11 @@ static void validateAndFixConfig(void) #ifndef USE_OSD_SLAVE void validateAndFixGyroConfig(void) { + // Disable dynamic filter if gyro loop is less than 2KHz + if (gyro.targetLooptime > GYRO_LOOPTIME_2KHZ) { + featureClear(FEATURE_DYNAMIC_FILTER); + } + // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 068d16f7a..9fece1636 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -743,7 +743,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin #ifdef USE_GYRO_DATA_ANALYSE static bool isDynamicFilterActive(void) { - return feature(FEATURE_DYNAMIC_FILTER); + return feature(FEATURE_DYNAMIC_FILTER) && (gyro.targetLooptime <= GYRO_LOOPTIME_2KHZ); } static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b72662034..58fc17540 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -26,6 +26,8 @@ #include "drivers/bus.h" #include "drivers/sensor.h" +#define GYRO_LOOPTIME_2KHZ 500 + typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, From 25c6b038d9ce2c7991711786cc8993da5d85aff6 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Sun, 29 Jul 2018 17:16:01 -0400 Subject: [PATCH 2/3] Changes from review Used HZ_TO_INTERVAL_US() instead of hardcoded 500us. Moved tests into a dedicated function. --- src/main/fc/config.c | 5 ++++- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 -- src/main/sensors/gyroanalyse.c | 3 +++ src/main/sensors/gyroanalyse.h | 1 + 5 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index f431fbb1b..0e34b5744 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -60,6 +60,7 @@ #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/gyro.h" +#include "sensors/gyroanalyse.h" #ifndef USE_OSD_SLAVE pidProfile_t *currentPidProfile; @@ -410,10 +411,12 @@ static void validateAndFixConfig(void) #ifndef USE_OSD_SLAVE void validateAndFixGyroConfig(void) { +#ifdef USE_GYRO_DATA_ANALYSE // Disable dynamic filter if gyro loop is less than 2KHz - if (gyro.targetLooptime > GYRO_LOOPTIME_2KHZ) { + if (!dynamicFilterAllowed()) { featureClear(FEATURE_DYNAMIC_FILTER); } +#endif // Prevent invalid notch cutoff if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) { diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9fece1636..1e505651e 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -743,7 +743,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin #ifdef USE_GYRO_DATA_ANALYSE static bool isDynamicFilterActive(void) { - return feature(FEATURE_DYNAMIC_FILTER) && (gyro.targetLooptime <= GYRO_LOOPTIME_2KHZ); + return feature(FEATURE_DYNAMIC_FILTER) && dynamicFilterAllowed(); } static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 58fc17540..b72662034 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -26,8 +26,6 @@ #include "drivers/bus.h" #include "drivers/sensor.h" -#define GYRO_LOOPTIME_2KHZ 500 - typedef enum { GYRO_NONE = 0, GYRO_DEFAULT, diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 236126739..c69e731f4 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -304,4 +304,7 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, state->updateStep = (state->updateStep + 1) % STEP_COUNT; } +bool dynamicFilterAllowed(void) { + return (gyro.targetLooptime <= HZ_TO_INTERVAL_US(2000)); +} #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 89d4401f3..58a3449c6 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -61,3 +61,4 @@ void gyroDataAnalyseInit(void); void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); +bool dynamicFilterAllowed(void); From 382ad2ad8fa8a1414f27929f506ef91b938146e3 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 31 Jul 2018 10:33:35 -0400 Subject: [PATCH 3/3] More changes from review & rebase Simplified disable logic to only use the feature() check in the gyro loop. Changed 2000hz loop time constant to a define. --- src/main/fc/config.c | 5 +++-- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyroanalyse.c | 4 ---- src/main/sensors/gyroanalyse.h | 1 - 4 files changed, 4 insertions(+), 8 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 0e34b5744..9259e5595 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -60,7 +60,6 @@ #include "sensors/acceleration.h" #include "sensors/battery.h" #include "sensors/gyro.h" -#include "sensors/gyroanalyse.h" #ifndef USE_OSD_SLAVE pidProfile_t *currentPidProfile; @@ -70,6 +69,8 @@ pidProfile_t *currentPidProfile; #define RX_SPI_DEFAULT_PROTOCOL 0 #endif +#define DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME HZ_TO_INTERVAL_US(2000) + PG_REGISTER_WITH_RESET_TEMPLATE(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_RESET_TEMPLATE(pilotConfig_t, pilotConfig, @@ -413,7 +414,7 @@ void validateAndFixGyroConfig(void) { #ifdef USE_GYRO_DATA_ANALYSE // Disable dynamic filter if gyro loop is less than 2KHz - if (!dynamicFilterAllowed()) { + if (gyro.targetLooptime > DYNAMIC_FILTER_MAX_SUPPORTED_LOOP_TIME) { featureClear(FEATURE_DYNAMIC_FILTER); } #endif diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 1e505651e..068d16f7a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -743,7 +743,7 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin #ifdef USE_GYRO_DATA_ANALYSE static bool isDynamicFilterActive(void) { - return feature(FEATURE_DYNAMIC_FILTER) && dynamicFilterAllowed(); + return feature(FEATURE_DYNAMIC_FILTER); } static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index c69e731f4..45d5a49b7 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -303,8 +303,4 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, state->updateStep = (state->updateStep + 1) % STEP_COUNT; } - -bool dynamicFilterAllowed(void) { - return (gyro.targetLooptime <= HZ_TO_INTERVAL_US(2000)); -} #endif // USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index 58a3449c6..89d4401f3 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -61,4 +61,3 @@ void gyroDataAnalyseInit(void); void gyroDataAnalyseStateInit(gyroAnalyseState_t *gyroAnalyse, uint32_t targetLooptime); void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample); void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, biquadFilter_t *notchFilterDyn); -bool dynamicFilterAllowed(void);