Dynamic filters (#5078)

* Init dynamic notches

* use gyro analyse

* Fix all compilation errors

* Disable dynamic filters on unit tests

* hopefully fix unit tests

* fix hanging FC when dynamic gyro used

* Make dynamic filters configurable as a feature

make/source.mk

@@ -92,6 +92,7 @@ COMMON_SRC = \
             flight/rth_estimator.c \
             flight/servos.c \
             flight/wind_estimator.c \
+            flight/gyroanalyse.c \
             io/beeper.c \
             io/lights.c \
             io/pwmdriver_i2c.c \
@@ -240,5 +241,22 @@ ifneq ($(filter VCP,$(FEATURES)),)
 TARGET_SRC += $(VCP_SRC)
 endif
 
+ifneq ($(DSP_LIB),)
+
+INCLUDE_DIRS += $(DSP_LIB)/Include
+
+TARGET_SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c
+
+TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
+TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c
+
+TARGET_SRC += $(wildcard $(DSP_LIB)/Source/*/*.S)
+endif
+
 # Search path and source files for the ST stdperiph library
 VPATH        := $(VPATH):$(STDPERIPH_DIR)/src

src/main/build/debug.h

@@ -71,5 +71,8 @@ typedef enum {
     DEBUG_ITERM_RELAX,
     DEBUG_D_BOOST,
     DEBUG_ANTIGRAVITY,
+    DEBUG_FFT,
+    DEBUG_FFT_TIME,
+    DEBUG_FFT_FREQ,
     DEBUG_COUNT
 } debugType_e;

src/main/common/filter.c

@@ -30,7 +30,6 @@
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
 // NULL filter
-
 float nullFilterApply(void *filter, float input)
 {
     UNUSED(filter);
@@ -148,7 +147,8 @@ void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t sam
     }
 
     // zero initial samples
-    filter->d1 = filter->d2 = 0;
+    filter->x1 = filter->x2 = 0;
+    filter->y1 = filter->y2 = 0;
 }
 
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType)
@@ -196,25 +196,59 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
     }
 
     // zero initial samples
-    filter->d1 = filter->d2 = 0;
+    filter->x1 = filter->x2 = 0;
+    filter->y1 = filter->y2 = 0;
+}
+
+FAST_CODE float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
+{
+    /* compute result */
+    const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
+
+    /* shift x1 to x2, input to x1 */
+    filter->x2 = filter->x1;
+    filter->x1 = input;
+
+    /* shift y1 to y2, result to y1 */
+    filter->y2 = filter->y1;
+    filter->y1 = result;
+
+    return result;
 }
 
 // Computes a biquad_t filter on a sample
 float FAST_CODE NOINLINE biquadFilterApply(biquadFilter_t *filter, float input)
 {
-    const float result = filter->b0 * input + filter->d1;
+    const float result = filter->b0 * input + filter->x1;
-    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
+    filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
-    filter->d2 = filter->b2 * input - filter->a2 * result;
+    filter->x2 = filter->b2 * input - filter->a2 * result;
     return result;
 }
 
 float biquadFilterReset(biquadFilter_t *filter, float value)
 {
-    filter->d1 = value - (value * filter->b0);
+    filter->x1 = value - (value * filter->b0);
-    filter->d2 = (filter->b2 - filter->a2) * value;
+    filter->x2 = (filter->b2 - filter->a2) * value;
     return value;
 }
 
+FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
+{
+    // backup state
+    float x1 = filter->x1;
+    float x2 = filter->x2;
+    float y1 = filter->y1;
+    float y2 = filter->y2;
+
+    biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
+
+    // restore state
+    filter->x1 = x1;
+    filter->x2 = x2;
+    filter->y1 = y1;
+    filter->y2 = y2;
+}
+
 /*
  * FIR filter
  */

src/main/common/filter.h

@@ -30,7 +30,7 @@ typedef struct pt1Filter_s {
 /* this holds the data required to update samples thru a filter */
 typedef struct biquadFilter_s {
     float b0, b1, b2, a1, a2;
-    float d1, d2;
+    float x1, x2, y1, y2;
 } biquadFilter_t;
 
 typedef union { 
@@ -79,10 +79,11 @@ void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t s
 void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samplingIntervalUs, float Q, biquadFilterType_e filterType);
 float biquadFilterApply(biquadFilter_t *filter, float sample);
 float biquadFilterReset(biquadFilter_t *filter, float value);
+float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
+void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
 void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
 void firFilterUpdate(firFilter_t *filter, float input);
-float firFilterApply(const firFilter_t *filter);
+float firFilterApply(const firFilter_t *filter);
-

src/main/config/feature.c

@@ -22,7 +22,7 @@
 
 #include "config/feature.h"
 
-static uint32_t activeFeaturesLatch = 0;
+static EXTENDED_FASTRAM uint32_t activeFeaturesLatch = 0;
 
 void latchActiveFeatures(void)
 {
@@ -34,7 +34,7 @@ bool featureConfigured(uint32_t mask)
     return featureConfig()->enabledFeatures & mask;
 }
 
-bool feature(uint32_t mask)
+bool FAST_CODE NOINLINE feature(uint32_t mask)
 {
     return activeFeaturesLatch & mask;
 }

src/main/fc/cli.c

@@ -144,7 +144,7 @@ static bool commandBatchError = false;
 // sync this with features_e
 static const char * const featureNames[] = {
     "THR_VBAT_COMP", "VBAT", "TX_PROF_SEL", "BAT_PROF_AUTOSWITCH", "MOTOR_STOP",
-    "", "SOFTSERIAL", "GPS", "",
+    "DYNAMIC_FILTERS", "SOFTSERIAL", "GPS", "",
     "", "TELEMETRY", "CURRENT_METER", "3D", "",
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",

src/main/fc/config.h

@@ -41,7 +41,7 @@ typedef enum {
     FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
     FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
     FEATURE_MOTOR_STOP = 1 << 4,
-    NOT_USED_10 = 1 << 5,               // was FEATURE_SERVO_TILT
+    FEATURE_DYNAMIC_FILTERS = 1 << 5,   // was FEATURE_SERVO_TILT
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,
     FEATURE_UNUSED_3 = 1 << 8,          // was FEATURE_FAILSAFE

src/main/fc/settings.yaml

@@ -78,7 +78,7 @@ tables:
     values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", 
-      "D_BOOST", "ANTIGRAVITY"]
+      "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -129,6 +129,9 @@ tables:
   - name: rssi_source
     values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
     enum: rssiSource_e
+  - name: dynamicFilterRangeTable
+    values: ["HIGH", "MEDIUM", "LOW"]
+    enum: dynamicFilterRange_e
 
 groups:
   - name: PG_GYRO_CONFIG
@@ -179,6 +182,25 @@ groups:
         condition: USE_GYRO_BIQUAD_RC_FIR2
         min: 0
         max: 500
+      - name: dyn_notch_width_percent
+        field: dyn_notch_width_percent
+        condition: USE_DYNAMIC_FILTERS
+        min: 0
+        max: 20
+      - name: dyn_notch_range
+        field: dyn_notch_range
+        condition: USE_DYNAMIC_FILTERS
+        table: dynamicFilterRangeTable
+      - name: dyn_notch_q
+        field: dyn_notch_q
+        condition: USE_DYNAMIC_FILTERS
+        min: 1
+        max: 1000
+      - name: dyn_notch_min_hz
+        field: dyn_notch_min_hz
+        condition: USE_DYNAMIC_FILTERS
+        min: 60
+        max: 1000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
         min: 0

src/main/flight/gyroanalyse.c

@@ -0,0 +1,374 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/* original work by Rav
+ * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
+ * coding assistance and advice from DieHertz, Rav, eTracer
+ * test pilots icr4sh, UAV Tech, Flint723
+ */
+#include <stdint.h>
+
+#include "platform.h"
+
+#ifdef USE_DYNAMIC_FILTERS
+
+#include "build/debug.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/time.h"
+#include "common/utils.h"
+
+#include "drivers/accgyro/accgyro.h"
+#include "drivers/time.h"
+
+#include "sensors/gyro.h"
+#include "fc/config.h"
+
+#include "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 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)
+// smoothing frequency for FFT centre frequency
+#define DYN_NOTCH_SMOOTH_FREQ_HZ  50
+// we need 4 steps for each axis
+#define DYN_NOTCH_CALC_TICKS      (XYZ_AXIS_COUNT * 4)
+
+#define DYN_NOTCH_OSD_MIN_THROTTLE 20
+
+static uint16_t EXTENDED_FASTRAM   fftSamplingRateHz;
+static float EXTENDED_FASTRAM      fftResolution;
+static uint8_t EXTENDED_FASTRAM    fftStartBin;
+static uint16_t EXTENDED_FASTRAM   dynNotchMaxCtrHz;
+static uint8_t dynamicFilterRange;
+static float EXTENDED_FASTRAM      dynNotchQ;
+static float EXTENDED_FASTRAM      dynNotch1Ctr;
+static float EXTENDED_FASTRAM      dynNotch2Ctr;
+static uint16_t EXTENDED_FASTRAM   dynNotchMinHz;
+static bool EXTENDED_FASTRAM dualNotch = true;
+static uint16_t EXTENDED_FASTRAM dynNotchMaxFFT;
+
+// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
+static EXTENDED_FASTRAM float hanningWindow[FFT_WINDOW_SIZE];
+
+void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
+{
+    dynamicFilterRange = gyroConfig()->dyn_notch_range;
+    fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
+    dynNotch1Ctr = 1 - gyroConfig()->dyn_notch_width_percent / 100.0f;
+    dynNotch2Ctr = 1 + gyroConfig()->dyn_notch_width_percent / 100.0f;
+    dynNotchQ = gyroConfig()->dyn_notch_q / 100.0f;
+    dynNotchMinHz = gyroConfig()->dyn_notch_min_hz;
+
+    if (gyroConfig()->dyn_notch_width_percent == 0) {
+        dualNotch = false;
+    }
+
+    if (dynamicFilterRange == DYN_NOTCH_RANGE_HIGH) {
+        fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
+    }
+    else if (dynamicFilterRange == DYN_NOTCH_RANGE_MEDIUM) {
+        fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
+    }
+
+    // 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)
+    const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
+    
+    fftSamplingRateHz = MIN((gyroLoopRateHz / 3), fftSamplingRateHz);
+
+    fftResolution = (float)fftSamplingRateHz / FFT_WINDOW_SIZE;
+
+    fftStartBin = dynNotchMinHz / lrintf(fftResolution);
+
+    dynNotchMaxCtrHz = fftSamplingRateHz / 2; //Nyquist
+
+    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)));
+    }
+}
+
+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;
+    state->maxSampleCount = samplingFrequency / fftSamplingRateHz;
+    state->maxSampleCountRcp = 1.f / state->maxSampleCount;
+
+    arm_rfft_fast_init_f32(&state->fftInstance, FFT_WINDOW_SIZE);
+
+//    recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
+//    at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
+//    for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
+    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] = dynNotchMaxCtrHz;
+        state->prevCenterFreq[axis] = dynNotchMaxCtrHz;
+        biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
+    }
+}
+
+void gyroDataAnalysePush(gyroAnalyseState_t *state, const int axis, const float sample)
+{
+    state->oversampledGyroAccumulator[axis] += sample;
+}
+
+static void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2);
+
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
+{
+    // samples should have been pushed by `gyroDataAnalysePush`
+    // if gyro sampling is > 1kHz, accumulate multiple samples
+    state->sampleCount++;
+
+    // this runs at 1kHz
+    if (state->sampleCount == state->maxSampleCount) {
+        state->sampleCount = 0;
+
+        // calculate mean value of accumulated samples
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
+            state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
+            if (axis == 0) {
+                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
+            }
+
+            state->oversampledGyroAccumulator[axis] = 0;
+        }
+
+        state->circularBufferIdx = (state->circularBufferIdx + 1) % FFT_WINDOW_SIZE;
+
+        // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
+        state->updateTicks = DYN_NOTCH_CALC_TICKS;
+    }
+
+    // calculate FFT and update filters
+    if (state->updateTicks > 0) {
+        gyroDataAnalyseUpdate(state, notchFilterDyn, notchFilterDyn2);
+        --state->updateTicks;
+    }
+}
+
+void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut);
+void arm_cfft_radix8by2_f32(arm_cfft_instance_f32 *S, float32_t *p1);
+void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
+void arm_radix8_butterfly_f32(float32_t *pSrc, uint16_t fftLen, const float32_t *pCoef, uint16_t twidCoefModifier);
+void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
+
+/*
+ * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
+ */
+static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn, biquadFilter_t *notchFilterDyn2)
+{
+    enum {
+        STEP_ARM_CFFT_F32,
+        STEP_BITREVERSAL,
+        STEP_STAGE_RFFT_F32,
+        STEP_ARM_CMPLX_MAG_F32,
+        STEP_CALC_FREQUENCIES,
+        STEP_UPDATE_FILTERS,
+        STEP_HANNING,
+        STEP_COUNT
+    };
+
+    arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
+
+    uint32_t startTime = 0;
+    if (debugMode == (DEBUG_FFT_TIME)) {
+        startTime = micros();
+    }
+
+    DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
+    switch (state->updateStep) {
+        case STEP_ARM_CFFT_F32:
+        {
+            switch (FFT_BIN_COUNT) {
+            case 16:
+                // 16us
+                arm_cfft_radix8by2_f32(Sint, state->fftData);
+                break;
+            case 32:
+                // 35us
+                arm_cfft_radix8by4_f32(Sint, state->fftData);
+                break;
+            case 64:
+                // 70us
+                arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
+                break;
+            }
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            break;
+        }
+        case STEP_BITREVERSAL:
+        {
+            // 6us
+            arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            state->updateStep++;
+            FALLTHROUGH;
+        }
+        case STEP_STAGE_RFFT_F32:
+        {
+            // 14us
+            // this does not work in place => fftData AND rfftData needed
+            stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            break;
+        }
+        case STEP_ARM_CMPLX_MAG_F32:
+        {
+            // 8us
+            arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
+            DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
+            state->updateStep++;
+            FALLTHROUGH;
+        }
+        case STEP_CALC_FREQUENCIES:
+        {
+            bool fftIncreased = false;
+            float dataMax = 0;
+            uint8_t binStart = 0;
+            uint8_t binMax = 0;
+            //for bins after initial decline, identify start bin and max bin 
+            for (int i = fftStartBin; i < FFT_BIN_COUNT; i++) {
+                if (fftIncreased || (state->fftData[i] > state->fftData[i - 1])) {
+                    if (!fftIncreased) {
+                        binStart = i; // first up-step bin
+                        fftIncreased = 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;
+                    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)
+            float centerFreq = dynNotchMaxCtrHz;
+            float fftMeanIndex = 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 = fftMeanIndex * fftResolution;
+            } else {
+                centerFreq = state->prevCenterFreq[state->updateAxis];
+            }
+            centerFreq = fmax(centerFreq, dynNotchMinHz);
+            centerFreq = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], centerFreq);
+            state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
+            state->centerFreq[state->updateAxis] = centerFreq;
+
+            dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
+
+            if (state->updateAxis == 0) {
+                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
+                DEBUG_SET(DEBUG_FFT_FREQ, 0, 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;
+        }
+        case STEP_UPDATE_FILTERS:
+        {
+            // 7us
+            // calculate cutoffFreq and notch Q, update notch filter  =1.8+((A2-150)*0.004)
+            if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
+                
+                DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
+
+                if (dualNotch) {
+                    biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
+                    biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
+                } else {
+                    biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
+                }
+            }
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
+            state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
+            state->updateStep++;
+            FALLTHROUGH;
+        }
+        case STEP_HANNING:
+        {
+            // 5us
+            // apply hanning window to gyro samples and store result in fftData
+            // hanning starts and ends with 0, could be skipped for minor speed improvement
+            const uint8_t ringBufIdx = FFT_WINDOW_SIZE - state->circularBufferIdx;
+            arm_mult_f32(&state->downsampledGyroData[state->updateAxis][state->circularBufferIdx], &hanningWindow[0], &state->fftData[0], ringBufIdx);
+            if (state->circularBufferIdx > 0) {
+                arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
+            }
+
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+        }
+    }
+
+    state->updateStep = (state->updateStep + 1) % STEP_COUNT;
+}
+
+
+uint16_t getMaxFFT(void) {
+    return dynNotchMaxFFT;
+}
+
+void resetMaxFFT(void) {
+    dynNotchMaxFFT = 0;
+}
+
+#endif // USE_DYNAMIC_FILTERS

src/main/flight/gyroanalyse.h

@@ -0,0 +1,63 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifdef USE_DYNAMIC_FILTERS
+
+#include "arm_math.h"
+#include "common/filter.h"
+
+// max for F3 targets
+#define FFT_WINDOW_SIZE 32
+
+typedef struct gyroAnalyseState_s {
+    // accumulator for oversampled data => no aliasing and less noise
+    uint8_t sampleCount;
+    uint8_t maxSampleCount;
+    float maxSampleCountRcp;
+    float oversampledGyroAccumulator[XYZ_AXIS_COUNT];
+
+    // downsampled gyro data circular buffer for frequency analysis
+    uint8_t circularBufferIdx;
+    float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
+
+    // update state machine step information
+    uint8_t updateTicks;
+    uint8_t updateStep;
+    uint8_t updateAxis;
+
+    arm_rfft_fast_instance_f32 fftInstance;
+    float fftData[FFT_WINDOW_SIZE];
+    float rfftData[FFT_WINDOW_SIZE];
+
+    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, biquadFilter_t *notchFilterDyn2);
+uint16_t getMaxFFT(void);
+void resetMaxFFT(void);
+#endif

src/main/sensors/gyro.c

@@ -34,6 +34,7 @@
 
 #include "config/parameter_group.h"
 #include "config/parameter_group_ids.h"
+#include "config/feature.h"
 
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/accgyro/accgyro_mpu.h"
@@ -66,6 +67,8 @@
 #include "sensors/gyro.h"
 #include "sensors/sensors.h"
 
+#include "flight/gyroanalyse.h"
+
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
 #endif
@@ -96,7 +99,19 @@ STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
 STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
+#ifdef USE_DYNAMIC_FILTERS
+
+#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
+#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
+
+static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn;
+static EXTENDED_FASTRAM filterApplyFnPtr notchFilterDynApplyFn2;
+static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
+static EXTENDED_FASTRAM biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
+EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 6);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
@@ -111,7 +126,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_soft_notch_cutoff_1 = 1,
     .gyro_soft_notch_hz_2 = 0,
     .gyro_soft_notch_cutoff_2 = 1,
-    .gyro_stage2_lowpass_hz = 0
+    .gyro_stage2_lowpass_hz = 0,
+    .dyn_notch_width_percent = 8,
+    .dyn_notch_range = DYN_NOTCH_RANGE_MEDIUM,
+    .dyn_notch_q = 120,
+    .dyn_notch_min_hz = 150,
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -251,6 +270,33 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     return gyroHardware;
 }
 
+#ifdef USE_DYNAMIC_FILTERS
+bool isDynamicFilterActive(void)
+{
+    return feature(FEATURE_DYNAMIC_FILTERS);
+}
+
+static void gyroInitFilterDynamicNotch(void)
+{
+
+    notchFilterDynApplyFn = nullFilterApply;
+    notchFilterDynApplyFn2 = nullFilterApply;
+
+    if (isDynamicFilterActive()) {
+        notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+        if(gyroConfig()->dyn_notch_width_percent != 0) {
+            notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+        }
+        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(&notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
+            biquadFilterInit(&notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, getLooptime(), notchQ, FILTER_NOTCH);
+        }
+    }
+
+}
+#endif
+
 bool gyroInit(void)
 {
     memset(&gyro, 0, sizeof(gyro));
@@ -280,6 +326,10 @@ bool gyroInit(void)
     }
 
     gyroInitFilters();
+#ifdef USE_DYNAMIC_FILTERS
+    gyroInitFilterDynamicNotch();
+    gyroDataAnalyseStateInit(&gyroAnalyseState, getLooptime());
+#endif
     return true;
 }
 
@@ -429,6 +479,15 @@ void FAST_CODE NOINLINE gyroUpdate()
 
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
 
+#ifdef USE_DYNAMIC_FILTERS
+        if (isDynamicFilterActive()) {
+            if (axis == FD_ROLL) {
+                DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
+                DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
+            }
+        }
+#endif
+
         if (axis < 2) {
             DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
         }
@@ -452,7 +511,25 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
 #endif
         gyro.gyroADCf[axis] = gyroADCf;
+
+#ifdef USE_DYNAMIC_FILTERS
+        if (isDynamicFilterActive()) {
+            if (axis == FD_ROLL) {
+                DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
+                DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
+            }
+            gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
+            gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
+            gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);
+        }
+#endif
     }
+
+#ifdef USE_DYNAMIC_FILTERS
+    if (isDynamicFilterActive()) {
+        gyroDataAnalyse(&gyroAnalyseState, notchFilterDyn, notchFilterDyn2);
+    }
+#endif
 }
 
 bool gyroReadTemperature(void)

src/main/sensors/gyro.h

@@ -39,6 +39,16 @@ typedef enum {
     GYRO_FAKE
 } gyroSensor_e;
 
+typedef enum {
+    DYN_NOTCH_RANGE_HIGH = 0,
+    DYN_NOTCH_RANGE_MEDIUM,
+    DYN_NOTCH_RANGE_LOW
+} dynamicFilterRange_e;
+
+#define DYN_NOTCH_RANGE_HZ_HIGH 2000
+#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
+#define DYN_NOTCH_RANGE_HZ_LOW 1000
+
 typedef struct gyro_s {
     uint32_t targetLooptime;
     float gyroADCf[XYZ_AXIS_COUNT];
@@ -60,6 +70,10 @@ typedef struct gyroConfig_s {
     uint16_t gyro_soft_notch_hz_2;
     uint16_t gyro_soft_notch_cutoff_2;
     uint16_t gyro_stage2_lowpass_hz;
+    uint8_t dyn_notch_width_percent;
+    uint8_t dyn_notch_range;
+    uint16_t dyn_notch_q;
+    uint16_t dyn_notch_min_hz;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/target/common.h

@@ -64,6 +64,7 @@
 #endif
 
 #if (FLASH_SIZE > 256)
+#define USE_DYNAMIC_FILTERS
 #define USE_EXTENDED_CMS_MENUS
 #define USE_UAV_INTERCONNECT
 #define USE_RX_UIB

src/main/target/common_post.h

@@ -50,4 +50,9 @@
 #undef USE_SERIALRX_SUMH
 #undef USE_SERIALRX_XBUS
 #undef USE_SERIALRX_JETIEXBUS
+#endif
+
+#if defined(SIMULATOR_BUILD) || defined(UNIT_TEST)
+// This feature uses 'arm_math.h', which does not exist for x86.
+#undef USE_DYNAMIC_FILTERS
 #endif