Merge pull request #6517 from ctzsnooze/DynFiltV5

Dynamic Filter Update

src/main/interface/settings.c

@@ -372,6 +372,15 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
 };
 #endif // USE_RC_SMOOTHING_FILTER
 
+#ifdef USE_GYRO_DATA_ANALYSE
+static const char * const lookupTableDynamicFftLocation[] = {
+    "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
+};
+static const char * const lookupTableDynamicFilterRange[] = {
+    "HIGH", "MEDIUM", "LOW"
+};
+#endif // USE_GYRO_DATA_ANALYSE
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -461,6 +470,11 @@ const lookupTableEntry_t lookupTables[] = {
     LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
     LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
 #endif // USE_RC_SMOOTHING_FILTER
+#ifdef USE_GYRO_DATA_ANALYSE
+    LOOKUP_TABLE_ENTRY(lookupTableDynamicFftLocation),
+    LOOKUP_TABLE_ENTRY(lookupTableDynamicFilterRange),
+#endif // USE_GYRO_DATA_ANALYSE
+
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -506,8 +520,9 @@ const clivalue_t valueTable[] = {
     { "gyro_to_use",                VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
 #endif
 #if defined(USE_GYRO_DATA_ANALYSE)
-    { "dyn_notch_quality",          VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) },
+    { "dyn_fft_location",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FFT_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_fft_location) },
-    { "dyn_notch_width_percent",    VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_width_percent) },
+    { "dyn_filter_width_percent",   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 99 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_width_percent) },
+    { "dyn_filter_range",           VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_RANGE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_range) },
 #endif
 
 // PG_ACCELEROMETER_CONFIG

src/main/interface/settings.h

@@ -112,6 +112,11 @@ typedef enum {
     TABLE_RC_SMOOTHING_INPUT_TYPE,
     TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
 #endif // USE_RC_SMOOTHING_FILTER
+#ifdef USE_GYRO_DATA_ANALYSE
+    TABLE_DYNAMIC_FFT_LOCATION,
+    TABLE_DYNAMIC_FILTER_RANGE,
+#endif // USE_GYRO_DATA_ANALYSE
+
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;
 

src/main/sensors/gyro.c

@@ -144,8 +144,11 @@ typedef struct gyroSensor_s {
 #endif // USE_YAW_SPIN_RECOVERY
 
 #ifdef USE_GYRO_DATA_ANALYSE
+#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
+#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
     gyroAnalyseState_t gyroAnalyseState;
 #endif
+
 } gyroSensor_t;
 
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@@ -203,8 +206,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_offset_yaw = 0,
     .yaw_spin_recovery = true,
     .yaw_spin_threshold = 1950,
-    .dyn_notch_quality = 70,
+    .dyn_filter_width_percent = 40,
-    .dyn_notch_width_percent = 50,
+    .dyn_fft_location = DYN_FFT_AFTER_STATIC_FILTERS,
+    .dyn_filter_range = DYN_FILTER_RANGE_MEDIUM,
 );
 
 
@@ -510,6 +514,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 
 #ifdef USE_GYRO_DATA_ANALYSE
     gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
+    
 #endif
 
     return true;
@@ -662,7 +667,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
 
     // Dereference the pointer to null before checking valid cutoff and filter
     // type. It will be overridden for positive cases.
-    *lowpassFilterApplyFn = &nullFilterApply;
+    *lowpassFilterApplyFn = nullFilterApply;
 
     // If lowpass cutoff has been specified and is less than the Nyquist frequency
     if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
@@ -748,9 +753,9 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
 
     if (isDynamicFilterActive()) {
         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
-        const float notchQ = filterGetNotchQ(400, 390); //just any init value
+        const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+            biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
         }
     }
 }
@@ -1013,7 +1018,7 @@ static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t current
 #endif // USE_YAW_SPIN_RECOVERY
 
 #define GYRO_FILTER_FUNCTION_NAME filterGyro
-#define GYRO_FILTER_DEBUG_SET(...)
+#define GYRO_FILTER_DEBUG_SET(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
 #include "gyro_filter_impl.h"
 #undef GYRO_FILTER_FUNCTION_NAME
 #undef GYRO_FILTER_DEBUG_SET

src/main/sensors/gyro.h

@@ -21,11 +21,14 @@
 #pragma once
 
 #include "common/axis.h"
+#include "common/filter.h"
 #include "common/time.h"
-#include "pg/pg.h"
+
 #include "drivers/bus.h"
 #include "drivers/sensor.h"
 
+#include "pg/pg.h"
+
 typedef enum {
     GYRO_NONE = 0,
     GYRO_DEFAULT,
@@ -58,6 +61,17 @@ typedef enum {
     GYRO_OVERFLOW_CHECK_ALL_AXES
 } gyroOverflowCheck_e;
 
+enum {
+    DYN_FFT_BEFORE_STATIC_FILTERS = 0,
+    DYN_FFT_AFTER_STATIC_FILTERS
+} ;
+
+enum {
+    DYN_FILTER_RANGE_HIGH = 0,
+    DYN_FILTER_RANGE_MEDIUM,
+    DYN_FILTER_RANGE_LOW
+} ;
+
 #define GYRO_CONFIG_USE_GYRO_1      0
 #define GYRO_CONFIG_USE_GYRO_2      1
 #define GYRO_CONFIG_USE_GYRO_BOTH   2
@@ -96,8 +110,10 @@ typedef struct gyroConfig_s {
     int16_t  yaw_spin_threshold;
 
     uint16_t gyroCalibrationDuration;  // Gyro calibration duration in 1/100 second
-    uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass
+    
-    uint8_t dyn_notch_width_percent;
+    uint8_t dyn_filter_width_percent;
+    uint8_t dyn_fft_location; // before or after static filters
+    uint8_t dyn_filter_range; // ignore any FFT bin below this threshold
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/gyro_filter_impl.h

@@ -10,10 +10,12 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
 #ifdef USE_GYRO_DATA_ANALYSE
         if (isDynamicFilterActive()) {
             if (axis == X) {
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
             }
         }
+
+        float gyroDataForAnalysis = gyroADCf;
 #endif
 
         // apply static notch filters and software lowpass filters
@@ -24,11 +26,17 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
 
 #ifdef USE_GYRO_DATA_ANALYSE
         if (isDynamicFilterActive()) {
+            if (gyroConfig()->dyn_fft_location == DYN_FFT_AFTER_STATIC_FILTERS) {
+                gyroDataForAnalysis = gyroADCf;
+            }
+
+            if (axis == X) {
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroDataForAnalysis));
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroDataForAnalysis));
+            }
+
             gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
             gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
-            if (axis == X) {
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
-            }
         }
 #endif
 

src/main/sensors/gyroanalyse.c

@@ -42,37 +42,35 @@
 #include "sensors/gyroanalyse.h"
 
 // The FFT splits the frequency domain into an number of bins
-// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
+// A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each 31.25Hz wide
 // Eg [0,31), [31,62), [62, 93) etc
-
+// for gyro loop >= 4KHz, sample rate 2000 defines FFT range to 1000Hz, 16 bins each 62.5 Hz wide
+// NB  FFT_WINDOW_SIZE is set to 32 in gyroanalyse.h
 #define FFT_BIN_COUNT             (FFT_WINDOW_SIZE / 2)
-// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide
+// start to compare 3rd bin to 2nd bin, ie start comparing from 77Hz, 100Hz, or 150Hz centres
-#define FFT_SAMPLING_RATE_HZ      1333
+#define FFT_BIN_OFFSET            2
-// following bin must be at least 2 times previous to indicate start of peak
+// smoothing frequency for FFT centre frequency
-#define FFT_MIN_BIN_RISE          2
+#define DYN_NOTCH_SMOOTH_FREQ_HZ  50
-// the desired approimate lower frequency when calculating bin offset
+// notch centre point will not go below sample rate divided by these dividers, resulting in range limits:
-#define FFT_BIN_OFFSET_DESIRED_HZ 90
+// HIGH : 133/166-1000Hz, MEDIUM -> 89/111-666Hz, LOW -> 67/83-500Hz
-// lowpass frequency for smoothing notch centre point
+#define DYN_NOTCH_MIN_CENTRE_DIV  12
-#define DYN_NOTCH_SMOOTH_FREQ_HZ  60
+// divider to get lowest allowed notch cutoff frequency
-// notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
+// otherwise cutoff is user configured percentage below centre frequency
-#define DYN_NOTCH_MIN_CENTRE_HZ   125
+#define DYN_NOTCH_MIN_CUTOFF_DIV  15
-// lowest allowed notch cutoff frequency
-#define DYN_NOTCH_MIN_CUTOFF_HZ   105
 // we need 4 steps for each axis
 #define DYN_NOTCH_CALC_TICKS      (XYZ_AXIS_COUNT * 4)
 
 static uint16_t FAST_RAM_ZERO_INIT   fftSamplingRateHz;
-// centre frequency of bandpass that constrains input to FFT
-static uint16_t FAST_RAM_ZERO_INIT fftBpfHz;
-// Hz per bin
 static float FAST_RAM_ZERO_INIT      fftResolution;
-// maximum notch centre frequency limited by Nyquist
-static uint16_t FAST_RAM_ZERO_INIT dynNotchMaxCentreHz;
 static uint8_t FAST_RAM_ZERO_INIT    fftBinOffset;
+static uint16_t FAST_RAM_ZERO_INIT   dynamicNotchMinCenterHz;
+static uint16_t FAST_RAM_ZERO_INIT   dynamicNotchMaxCenterHz;
+static uint16_t FAST_RAM_ZERO_INIT   dynamicNotchMinCutoffHz;
+static float FAST_RAM_ZERO_INIT      dynamicFilterWidthFactor;
+static uint8_t dynamicFilterRange;
 
 // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
 static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
-static FAST_RAM_ZERO_INIT float dynamicNotchCutoff;
 
 void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
 {
@@ -84,31 +82,39 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
     gyroAnalyseInitialized = true;
 #endif
 
-    const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
+    dynamicFilterRange = gyroConfig()->dyn_filter_range;
     
+    fftSamplingRateHz = 1000;
+    if (dynamicFilterRange == DYN_FILTER_RANGE_HIGH) {
+        fftSamplingRateHz = 2000;
+    }
+    else if (dynamicFilterRange == DYN_FILTER_RANGE_MEDIUM) {
+        fftSamplingRateHz = 1333;
+    }
     // If we get at least 3 samples then use the default FFT sample frequency
     // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
-    fftSamplingRateHz = MIN((gyroLoopRateHz / 3), FFT_SAMPLING_RATE_HZ);
+    const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
+    
+    fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
 
-    fftBpfHz = fftSamplingRateHz / 4;
     fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
-    dynNotchMaxCentreHz = fftSamplingRateHz / 2;
+    fftBinOffset = FFT_BIN_OFFSET;
+
+    dynamicNotchMaxCenterHz = fftSamplingRateHz / 2; //Nyquist
+    dynamicNotchMinCenterHz = fftSamplingRateHz / DYN_NOTCH_MIN_CENTRE_DIV;
+    dynamicNotchMinCutoffHz = fftSamplingRateHz / DYN_NOTCH_MIN_CUTOFF_DIV;
+    dynamicFilterWidthFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
 
-    // Calculate the FFT bin offset to try and get the lowest bin used
-    // in the center calc close to 90hz
-    // > 1333hz = 1, 889hz (2.67K) = 2, 666hz (2K) = 3
-    fftBinOffset = MAX(1, lrintf(FFT_BIN_OFFSET_DESIRED_HZ / fftResolution - 1.5f));
 
     for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
         hanningWindow[i] = (0.5f - 0.5f * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
     }
-
-    dynamicNotchCutoff = (100.0f - gyroConfig()->dyn_notch_width_percent) / 100;
 }
 
 void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
 {
     // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
+    // *** can this next line be removed ??? ***
     gyroDataAnalyseInit(targetLooptimeUs);
 
     const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
@@ -123,8 +129,8 @@ void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptime
     const float looptime = MAX(1000000u / fftSamplingRateHz, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         // any init value
-        state->centerFreq[axis] = 200;
+        state->centerFreq[axis] = dynamicNotchMaxCenterHz;
-        biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF);
+        state->prevCenterFreq[axis] = dynamicNotchMaxCenterHz;
         biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
     }
 }
@@ -152,8 +158,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
         // calculate mean value of accumulated samples
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
-            sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample);
-
             state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
             if (axis == 0) {
                 DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
@@ -251,53 +255,72 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
         }
         case STEP_CALC_FREQUENCIES:
         {
-            // 13us
+            bool fftIncreased = false;
-            // calculate FFT centreFreq
+            float dataMax = 0;
-            float fftSum = 0;
+            uint8_t binStart = 0;
-            float fftWeightedSum = 0;
+            uint8_t binMax = 0;
-            bool fftIncreasing = false;
+            //for bins after initial decline, identify start bin and max bin 
-
-            // iterate over fft data and calculate weighted indices
             for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
-                const float data = state->fftData[i];
+                if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
-                const float prevData = state->fftData[i - 1];
+                    if (!fftIncreased) {
-
+                        binStart = i; // first up-step bin
-                if (fftIncreasing || data > prevData * FFT_MIN_BIN_RISE) {
+                        fftIncreased = true;
-                    float cubedData = data * data * data;
-
-                    // add previous bin before first rise
-                    if (!fftIncreasing) {
-                        cubedData += prevData * prevData * prevData;
-
-                        fftIncreasing = true;
                     }
-
+                    if (state->fftData[i] > dataMax) {
+                        dataMax = state->fftData[i];
+                        binMax = i;  // tallest bin
+                    }
+                }
+            }
+            // accumulate fftSum and fftWeightedSum from peak bin, and shoulder bins either side of peak
+            float cubedData = state->fftData[binMax] * state->fftData[binMax] * state->fftData[binMax];
+            float fftSum = cubedData;
+            float fftWeightedSum = cubedData * (binMax + 1);
+            // accumulate upper shoulder
+            for (int i = binMax; i < FFT_BIN_COUNT - 1; i++) {
+                if (state->fftData[i] > state->fftData[i + 1]) {
+                    cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
                     fftSum += cubedData;
-                    // calculate weighted index starting at 1, not 0
                     fftWeightedSum += cubedData * (i + 1);
+                } else {
+                break;
+                }
+            }
+            // accumulate lower shoulder
+            for (int i = binMax; i > binStart + 1; i--) {
+                if (state->fftData[i] > state->fftData[i - 1]) {
+                    cubedData = state->fftData[i] * state->fftData[i] * state->fftData[i];
+                    fftSum += cubedData;
+                    fftWeightedSum += cubedData * (i + 1);
+                } else {
+                break;
                 }
             }
-
             // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
-            // if no peak, go to highest point to minimise delay
+            float centerFreq = dynamicNotchMaxCenterHz;
-            float centerFreq = dynNotchMaxCentreHz;
             float fftMeanIndex = 0;
-
-            if (fftSum > 0) {
              // idx was shifted by 1 to start at 1, not 0
+            if (fftSum > 0) {
                 fftMeanIndex = (fftWeightedSum / fftSum) - 1;
                 // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
-                centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
+                centerFreq = fftMeanIndex * fftResolution;
+            } else {
+                centerFreq = state->prevCenterFreq[state->updateAxis];
             }
-
+            // constrain and low-pass smooth centre frequency
+            centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
             centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
-            centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
+            centerFreq = constrain(centerFreq, dynamicNotchMinCenterHz, dynamicNotchMaxCenterHz);
             state->centerFreq[state->updateAxis] = centerFreq;
 
             if (state->updateAxis == 0) {
                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
+               DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
             }
-            DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]);
+            if (state->updateAxis == 1) {
+                DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
+            }
+            // Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
             break;
         }
@@ -305,9 +328,10 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
         {
             // 7us
             // calculate cutoffFreq and notch Q, update notch filter
-            const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicNotchCutoff, DYN_NOTCH_MIN_CUTOFF_HZ);
+            const float cutoffFreq = fmax(state->centerFreq[state->updateAxis] * dynamicFilterWidthFactor, dynamicNotchMinCutoffHz);
             const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
             biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
+
             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 
             state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;

src/main/sensors/gyroanalyse.h

@@ -22,9 +22,10 @@
 
 #include "arm_math.h"
 
-#include "common/time.h"
 #include "common/filter.h"
 
+#include "sensors/gyro.h"
+
 // max for F3 targets
 #define FFT_WINDOW_SIZE 32
 
@@ -35,9 +36,6 @@ typedef struct gyroAnalyseState_s {
     float maxSampleCountRcp;
     float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
 
-    // filter for downsampled accumulated gyro
-    biquadFilter_t gyroBandpassFilter[XYZ_AXIS_COUNT];
-
     // downsampled gyro data circular buffer for frequency analysis
     uint8_t circularBufferIdx;
     float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
@@ -53,6 +51,7 @@ typedef struct gyroAnalyseState_s {
 
     biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
     uint16_t centerFreq[XYZ_AXIS_COUNT];
+    uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
 } gyroAnalyseState_t;
 
 STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlying_type);