Dynamic Filter Update

Improves on earlier 3.5RC1 dynamic filter with:
- better FFT tracking performance overall, even with a narrower default notch width
- can reach 850Hz for high kV 2.5-3" and 6S quads
- works better with 32k gyros
- can be applied pre- (location 1) and post- (location 2) static filters. Pre-static filter location works best, but post-static may work well in 32k modes or with PT1 option
- option to use a PT1 filter rather than the classical notch filter, perhaps useful for quads with a lot of noise above their peak.
- ability to totally bypass the pre-FFT bandpass filter, by setting Q=0, maximising the range of responsiveness
- "ignore" value, absolute FFT bin amplitude below which the bin will be excluded from consideration. May be tweaked to optimise peak detection; primarily for advanced tuners. If too high, only the very peaks will be included, if too low, noise may clutter peak detection.
- "threshold" value for peak detection, which, when divided by 10, is the multiple by which one bin must exceed the previous one for peak detection to commence. If too low, FFT detection becomes more random, if too high, no peaks are detected.

src/main/interface/settings.c

@@ -372,6 +372,12 @@ static const char * const lookupTableRcSmoothingDerivativeType[] = {
 };
 #endif // USE_RC_SMOOTHING_FILTER
 
+#ifdef USE_GYRO_DATA_ANALYSE
+static const char * const lookupTableDynamicFilterLocation[] = {
+    "BEFORE_STATIC_FILTERS", "AFTER_STATIC_FILTERS"
+};
+#endif // USE_GYRO_DATA_ANALYSE
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -461,6 +467,10 @@ 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(lookupTableDynamicFilterLocation),
+#endif // USE_GYRO_DATA_ANALYSE
+
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -506,8 +516,12 @@ 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_filter_type",            VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_type) },
-    { "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_notch_quality",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 70 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_quality) },
+    { "dyn_filter_location",        VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYNAMIC_FILTER_LOCATION }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_location) },
+    { "dyn_filter_threshold",       VAR_UINT8  | MASTER_VALUE, .config.minmax = { 10, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_threshold) },
+    { "dyn_filter_ignore",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 255 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_filter_ignore) },
 #endif
 
 // PG_ACCELEROMETER_CONFIG

src/main/interface/settings.h

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

src/main/sensors/gyro.c

@@ -132,8 +132,8 @@ typedef struct gyroSensor_s {
     filterApplyFnPtr notchFilter2ApplyFn;
     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 
-    filterApplyFnPtr notchFilterDynApplyFn;
+    filterApplyFnPtr dynFilterApplyFn;
-    biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
+    gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT];
 
     // overflow and recovery
     timeUs_t overflowTimeUs;
@@ -144,8 +144,12 @@ typedef struct gyroSensor_s {
 #endif // USE_YAW_SPIN_RECOVERY
 
 #ifdef USE_GYRO_DATA_ANALYSE
+#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200
+#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400
+#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350
     gyroAnalyseState_t gyroAnalyseState;
 #endif
+
 } gyroSensor_t;
 
 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@@ -203,8 +207,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_offset_yaw = 0,
     .yaw_spin_recovery = true,
     .yaw_spin_threshold = 1950,
-    .dyn_notch_quality = 70,
+    .dyn_filter_type = FILTER_BIQUAD,
-    .dyn_notch_width_percent = 50,
+    .dyn_filter_width_percent = 40,
+    .dyn_notch_quality = 20,
+    .dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS,
+    .dyn_filter_threshold = 30,
+    .dyn_filter_ignore = 20,
 );
 
 
@@ -510,6 +518,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 
 #ifdef USE_GYRO_DATA_ANALYSE
     gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
+    
 #endif
 
     return true;
@@ -662,7 +671,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) {
@@ -742,15 +751,35 @@ static bool isDynamicFilterActive(void)
     return feature(FEATURE_DYNAMIC_FILTER);
 }
 
-static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
+static void gyroInitFilterDynamic(gyroSensor_t *gyroSensor)
 {
-    gyroSensor->notchFilterDynApplyFn = nullFilterApply;
+    gyroSensor->dynFilterApplyFn = nullFilterApply;
 
     if (isDynamicFilterActive()) {
-        gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+        switch (gyroConfig()->dyn_filter_type) {
-        const float notchQ = filterGetNotchQ(400, 390); //just any init value
+        case FILTER_PT1: {
+            const float gyroDt = gyro.targetLooptime * 1e-6f;
+            const float gain = pt1FilterGain(DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ, gyroDt);
+
+            gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
+
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+                pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain);
+            }
+
+            break;
+        }
+        case FILTER_BIQUAD: {
+            const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ);
+
+            gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
+
+            for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                biquadFilterInit(&gyroSensor->dynFilter[axis].biquadFilterState, DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+            }
+
+            break;
+        }
         }
     }
 }
@@ -780,7 +809,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
     gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
     gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 #ifdef USE_GYRO_DATA_ANALYSE
-    gyroInitFilterDynamicNotch(gyroSensor);
+    gyroInitFilterDynamic(gyroSensor);
 #endif
 }
 
@@ -1075,7 +1104,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
 
 #ifdef USE_GYRO_DATA_ANALYSE
     if (isDynamicFilterActive()) {
-        gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
+        gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->dynFilter);
     }
 #endif
 }

src/main/sensors/gyro.h

@@ -58,6 +58,11 @@ typedef enum {
     GYRO_OVERFLOW_CHECK_ALL_AXES
 } gyroOverflowCheck_e;
 
+enum {
+    DYN_FILTER_BEFORE_STATIC_FILTERS = 0,
+    DYN_FILTER_AFTER_STATIC_FILTERS
+} ;
+
 #define GYRO_CONFIG_USE_GYRO_1      0
 #define GYRO_CONFIG_USE_GYRO_2      1
 #define GYRO_CONFIG_USE_GYRO_BOTH   2
@@ -96,8 +101,12 @@ typedef struct gyroConfig_s {
     int16_t  yaw_spin_threshold;
 
     uint16_t gyroCalibrationDuration;  // Gyro calibration duration in 1/100 second
+    uint8_t dyn_filter_type;
+    uint8_t dyn_filter_width_percent;
     uint8_t dyn_notch_quality; // bandpass quality factor, 100 for steep sided bandpass
-    uint8_t dyn_notch_width_percent;
+    uint8_t dyn_filter_location; // before or after static filters
+    uint8_t dyn_filter_threshold; // divided by 10 then difference needed to detect peak
+    uint8_t dyn_filter_ignore; // ignore any FFT bin below this threshold
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/sensors/gyro_filter_impl.h

@@ -11,7 +11,15 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
         if (isDynamicFilterActive()) {
             if (axis == X) {
                 GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
-                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data
+                GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf)); // store raw data in 3
+            }
+            if (gyroConfig()->dyn_filter_location == DYN_FILTER_BEFORE_STATIC_FILTERS) {
+                gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
+                gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
+                if (axis == X) {
+                   GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
+                   GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-notch data in 2
+               }
             }
         }
 #endif
@@ -24,10 +32,13 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor, timeDe
 
 #ifdef USE_GYRO_DATA_ANALYSE
         if (isDynamicFilterActive()) {
-            gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
+            if (gyroConfig()->dyn_filter_location == DYN_FILTER_AFTER_STATIC_FILTERS){
-            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
+                    GYRO_FILTER_DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf)); // store post-static data in debug 2
+                    GYRO_FILTER_DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after statics
+                }
+                gyroDataAnalysePush(&gyroSensor->gyroAnalyseState, axis, gyroADCf);
+                gyroADCf = gyroSensor->dynFilterApplyFn((filter_t *)&gyroSensor->dynFilter[axis], gyroADCf);
             }
         }
 #endif

src/main/sensors/gyroanalyse.c

@@ -45,19 +45,26 @@
 // 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
 // Eg [0,31), [31,62), [62, 93) etc
 
-#define FFT_BIN_COUNT             (FFT_WINDOW_SIZE / 2)
+// for gyro loop >= 4KHz, analyse up to 1000Hz, 16 bins each 62.5 Hz wide
-// for gyro loop >= 4KHz, analyse up to 666Hz, 16 bins each 41.625 Hz wide
+#define FFT_SAMPLING_RATE_HZ      2000
-#define FFT_SAMPLING_RATE_HZ      1333
+#define FFT_RESOLUTION            ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE)
 // following bin must be at least 2 times previous to indicate start of peak
-#define FFT_MIN_BIN_RISE          2
+#define FFT_BIN_COUNT             (FFT_WINDOW_SIZE / 2)
 // the desired approimate lower frequency when calculating bin offset
 #define FFT_BIN_OFFSET_DESIRED_HZ 90
-// lowpass frequency for smoothing notch centre point
+// centre frequency of bandpass that constrains input to FFT
-#define DYN_NOTCH_SMOOTH_FREQ_HZ  60
+#if FFT_SAMPLING_RATE_HZ == 2000
+#define FFT_BPF_HZ                350
+#elif FFT_SAMPLING_RATE_HZ < 2000
+#define FFT_BPF_HZ                (FFT_SAMPLING_RATE_HZ / 4)
+#endif
+#define DYN_NOTCH_SMOOTH_FREQ_HZ  50
 // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
-#define DYN_NOTCH_MIN_CENTRE_HZ   125
+#define DYN_NOTCH_MIN_CENTRE_HZ   140
+// maximum notch centre frequency limited by Nyquist
+#define DYN_NOTCH_MAX_CENTRE_HZ   (FFT_SAMPLING_RATE_HZ / 2)
 // lowest allowed notch cutoff frequency
-#define DYN_NOTCH_MIN_CUTOFF_HZ   105
+#define DYN_NOTCH_MIN_CUTOFF_HZ   110
 // we need 4 steps for each axis
 #define DYN_NOTCH_CALC_TICKS      (XYZ_AXIS_COUNT * 4)
 
@@ -66,13 +73,14 @@ static uint16_t FAST_RAM_ZERO_INIT fftSamplingRateHz;
 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;
 
 // 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;
+static FAST_RAM_ZERO_INIT float dynamicFilterCutoffFactor;
+static FAST_RAM_ZERO_INIT uint8_t dynamicFilterType;
+static FAST_RAM_ZERO_INIT float dynamicFilterThreshold;
+static FAST_RAM_ZERO_INIT float dynamicFilterIgnore;
 
 void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
 {
@@ -92,7 +100,6 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
 
     fftBpfHz = fftSamplingRateHz / 4;
     fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
-    dynNotchMaxCentreHz = fftSamplingRateHz / 2;
 
     // Calculate the FFT bin offset to try and get the lowest bin used
     // in the center calc close to 90hz
@@ -103,7 +110,10 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
         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;
+    dynamicFilterCutoffFactor = (100.0f - gyroConfig()->dyn_filter_width_percent) / 100;
+    dynamicFilterType = gyroConfig()->dyn_filter_type;
+    dynamicFilterThreshold = gyroConfig()->dyn_filter_threshold / 10;
+    dynamicFilterIgnore = gyroConfig()->dyn_filter_ignore / 10;
 }
 
 void gyroDataAnalyseStateInit(gyroAnalyseState_t *state, uint32_t targetLooptimeUs)
@@ -123,7 +133,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] = DYN_NOTCH_MAX_CENTRE_HZ;
+        state->prevCenterFreq[axis] = DYN_NOTCH_MAX_CENTRE_HZ;
         biquadFilterInit(&state->gyroBandpassFilter[axis], fftBpfHz, 1000000 / fftSamplingRateHz, 0.01f * gyroConfig()->dyn_notch_quality, FILTER_BPF);
         biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
     }
@@ -134,12 +145,12 @@ void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float
     state->oversampledGyroAccumulator[axis] += sample;
 }
 
-static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn);
+static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter);
 
 /*
  * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
  */
-void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
+void gyroDataAnalyse(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter)
 {
     // samples should have been pushed by `gyroDataAnalysePush`
     // if gyro sampling is > 1kHz, accumulate multiple samples
@@ -152,8 +163,9 @@ 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;
+            if (gyroConfig()->dyn_notch_quality > 4){
                 sample = biquadFilterApply(&state->gyroBandpassFilter[axis], sample);
-
+            }
             state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
             if (axis == 0) {
                 DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
@@ -170,7 +182,7 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
 
     // calculate FFT and update filters
     if (state->updateTicks > 0) {
-        gyroDataAnalyseUpdate(state, notchFilterDyn);
+        gyroDataAnalyseUpdate(state, dynFilter);
         --state->updateTicks;
     }
 }
@@ -184,7 +196,7 @@ void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t
 /*
  * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
  */
-static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn)
+static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, gyroDynamicFilter_t *dynFilter)
 {
     enum {
         STEP_ARM_CFFT_F32,
@@ -255,59 +267,88 @@ static FAST_CODE_NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state,
             // calculate FFT centreFreq
             float fftSum = 0;
             float fftWeightedSum = 0;
+            float dataAvg = 0;
             bool fftIncreasing = false;
 
+            //get simple average of bin amplitudes
+            for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
+                dataAvg += state->fftData[i];
+            }
+            dataAvg = dataAvg / FFT_BIN_COUNT;
+
             // iterate over fft data and calculate weighted indices
             for (int i = 1 + fftBinOffset; i < FFT_BIN_COUNT; i++) {
                 const float data = state->fftData[i];
                 const float prevData = state->fftData[i - 1];
-
+                // only consider bins above ignore multiple of average size
-                if (fftIncreasing || data > prevData * FFT_MIN_BIN_RISE) {
+                if (data > dynamicFilterIgnore * dataAvg) {
+                // only consider bins after > threshold step up from previous
+                    if (fftIncreasing || data > prevData * dynamicFilterThreshold) {
                         float cubedData = data * data * data;
-
                         // add previous bin before first rise
                         if (!fftIncreasing) {
                             cubedData += prevData * prevData * prevData;
-
                             fftIncreasing = true;
                         }
-
                         fftSum += cubedData;
                         // calculate weighted index starting at 1, not 0
                         fftWeightedSum += cubedData * (i + 1);
                     }
                 }
+            }
 
             // 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 = dynNotchMaxCentreHz;
+            float centerFreq = DYN_NOTCH_MAX_CENTRE_HZ;
             float fftMeanIndex = 0;
-
             if (fftSum > 0) {
                 // idx was shifted by 1 to start at 1, not 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 * FFT_RESOLUTION;
+            } else {
+                centerFreq = state->prevCenterFreq[state->updateAxis];
             }
-
+            state->prevCenterFreq[state->updateAxis] = centerFreq;
+            centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
             centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
-            centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, dynNotchMaxCentreHz);
+            centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
             state->centerFreq[state->updateAxis] = centerFreq;
 
             if (state->updateAxis == 0) {
                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
             }
+
+            if (state->updateAxis == 0 || state->updateAxis == 1) {
                 DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis, state->centerFreq[state->updateAxis]);
+            }
             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
             break;
         }
         case STEP_UPDATE_FILTERS:
         {
             // 7us
+            switch (dynamicFilterType) {
+            case FILTER_PT1: {
+                const int cutoffFreq = state->centerFreq[state->updateAxis] * dynamicFilterCutoffFactor;
+                const float gyroDt = gyro.targetLooptime * 1e-6f;
+                const float gain = pt1FilterGain(cutoffFreq, gyroDt);
+
+                pt1FilterUpdateCutoff(&dynFilter[state->updateAxis].pt1FilterState, gain);
+
+                break;
+            }
+            case FILTER_BIQUAD: {
                 // 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] * dynamicFilterCutoffFactor, DYN_NOTCH_MIN_CUTOFF_HZ);
                 const float notchQ = filterGetNotchQ(state->centerFreq[state->updateAxis], cutoffFreq);
-            biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
+                biquadFilterUpdate(&dynFilter[state->updateAxis].biquadFilterState, state->centerFreq[state->updateAxis], gyro.targetLooptime, notchQ, FILTER_NOTCH);
+
+            break;
+            }
+            }
+
             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 
             state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;

src/main/sensors/gyroanalyse.h

@@ -28,6 +28,11 @@
 // max for F3 targets
 #define FFT_WINDOW_SIZE 32
 
+typedef union gyroDynamicFilter_u {
+    pt1Filter_t pt1FilterState;
+    biquadFilter_t biquadFilterState;
+} gyroDynamicFilter_t;
+
 typedef struct gyroAnalyseState_s {
     // accumulator for oversampled data => no aliasing and less noise
     uint8_t sampleCount;
@@ -53,10 +58,11 @@ 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);
 
 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);
+void gyroDataAnalyse(gyroAnalyseState_t *gyroAnalyse, gyroDynamicFilter_t *dynFilter);