enable frequency analysis and automatic, dynamic changing of notch filter frequencies

change F3 from CM1 to CM4
add debug flags for FFT
add bandpass filter
add different filtering apply function
add feature DYNAMIC_FILTER
replace mode GTUNE with DYNAMIC FILTER
move gyro frequency analysis into gyro loop instead of own task

Makefile

@@ -276,14 +276,14 @@ STARTUP_SRC     = startup_stm32f30x_md_gcc.S
 STDPERIPH_SRC   := $(filter-out ${EXCLUDES}, $(STDPERIPH_SRC))
 DEVICE_STDPERIPH_SRC = $(STDPERIPH_SRC)
 
-VPATH           := $(VPATH):$(CMSIS_DIR)/CM1/CoreSupport:$(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x
+VPATH           := $(VPATH):$(CMSIS_DIR)/CM4/CoreSupport:$(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x
-CMSIS_SRC       = $(notdir $(wildcard $(CMSIS_DIR)/CM1/CoreSupport/*.c \
+CMSIS_SRC       = $(notdir $(wildcard $(CMSIS_DIR)/CM4/CoreSupport/*.c \
-                  $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x/*.c))
+                  $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x/*.c))
 
 INCLUDE_DIRS    := $(INCLUDE_DIRS) \
                    $(STDPERIPH_DIR)/inc \
-                   $(CMSIS_DIR)/CM1/CoreSupport \
+                   $(CMSIS_DIR)/CM4/CoreSupport \
-                   $(CMSIS_DIR)/CM1/DeviceSupport/ST/STM32F30x
+                   $(CMSIS_DIR)/CM4/DeviceSupport/ST/STM32F30x
 
 ifneq ($(filter VCP, $(FEATURES)),)
 INCLUDE_DIRS    := $(INCLUDE_DIRS) \
@@ -1023,12 +1023,13 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(SITL_TARGETS)))
 SRC := $(TARGET_SRC) $(SITL_SRC) $(VARIANT_SRC)
 endif
 
-ifneq ($(filter $(TARGET),$(F4_TARGETS) $(F7_TARGETS)),)
+ifneq ($(filter $(TARGET),$(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS)),)
 DSPLIB := $(ROOT)/lib/main/DSP_Lib
 DEVICE_FLAGS += -DARM_MATH_CM4 -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE
 
 INCLUDE_DIRS += $(DSPLIB)/Include
 
+SRC += $(DSPLIB)/Source/BasicMathFunctions/arm_mult_f32.c
 SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
 SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_f32.c
 SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
@@ -1039,6 +1040,7 @@ SRC += $(DSPLIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
 SRC += $(DSPLIB)/Source/StatisticsFunctions/arm_max_f32.c
 
 SRC += $(wildcard $(DSPLIB)/Source/*/*.S)
+
 endif
 
 

src/main/build/atomic.h

@@ -31,7 +31,7 @@ __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX_nb(uint3
    __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) );
 }
 
-#if !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
+#if !defined(STM32F3) && !defined(STM32F4) && !defined(STM32F7) /* already defined in /lib/main/CMSIS/CM4/CoreSupport/core_cmFunc.h for F4 targets */
 __attribute__( ( always_inline ) ) static inline void __set_BASEPRI_MAX(uint32_t basePri)
 {
     __ASM volatile ("\tMSR basepri_max, %0\n" : : "r" (basePri) : "memory" );

src/main/build/debug.h

@@ -65,5 +65,8 @@ typedef enum {
     DEBUG_ESC_SENSOR_RPM,
     DEBUG_ESC_SENSOR_TMP,
     DEBUG_ALTITUDE,
+    DEBUG_FFT,
+    DEBUG_FFT_TIME,
+    DEBUG_FFT_FREQ,
     DEBUG_COUNT
 } debugType_e;

src/main/common/filter.c

@@ -24,13 +24,6 @@
 #include "common/maths.h"
 #include "common/utils.h"
 
-#define M_LN2_FLOAT 0.69314718055994530942f
-#define M_PI_FLOAT  3.14159265358979323846f
-
-#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
-#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
-
-
 // NULL filter
 
 float nullFilterApply(void *filter, float input)
@@ -79,22 +72,22 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
 {
     biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
 }
+
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
 {
     // setup variables
-    const float sampleRate = 1 / ((float)refreshRate * 0.000001f);
+    const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
-    const float omega = 2 * M_PI_FLOAT * filterFreq / sampleRate;
     const float sn = sinf(omega);
     const float cs = cosf(omega);
-    const float alpha = sn / (2 * Q);
+    const float alpha = sn / (2.0f * Q);
 
     float b0 = 0, b1 = 0, b2 = 0, a0 = 0, a1 = 0, a2 = 0;
 
     switch (filterType) {
     case FILTER_LPF:
-        b0 = (1 - cs) / 2;
+        b0 = (1 - cs) * 0.5f;
         b1 = 1 - cs;
-        b2 = (1 - cs) / 2;
+        b2 = (1 - cs) * 0.5f;
         a0 = 1 + alpha;
         a1 = -2 * cs;
         a2 = 1 - alpha;
@@ -107,21 +100,70 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
         a1 = -2 * cs;
         a2 =  1 - alpha;
         break;
+    case FILTER_BPF:
+        b0 = alpha;
+        b1 = 0;
+        b2 = -alpha;
+        a0 = 1 + alpha;
+        a1 = -2 * cs;
+        a2 = 1 - alpha;
+        break;
     }
 
     // precompute the coefficients
-    filter->b0 = b0 / a0;
+    a0 = 1.0f / a0;
-    filter->b1 = b1 / a0;
+    filter->b0 = b0 * a0;
-    filter->b2 = b2 / a0;
+    filter->b1 = b1 * a0;
-    filter->a1 = a1 / a0;
+    filter->b2 = b2 * a0;
-    filter->a2 = a2 / a0;
+    filter->a1 = a1 * a0;
+    filter->a2 = a2 * a0;
 
     // zero initial samples
+    filter->x1 = filter->x2 = 0;
+    filter->y1 = filter->y2 = 0;
     filter->d1 = filter->d2 = 0;
 }
 
-/* Computes a biquadFilter_t filter on a sample */
+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;
+    float d1 = filter->d1;
+    float d2 = filter->d2;
+
+    biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
+
+    // restore state
+    filter->x1 = x1;
+    filter->x2 = x2;
+    filter->y1 = y1;
+    filter->y2 = y2;
+    filter->d1 = d1;
+    filter->d2 = d2;
+}
+
+/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
 float biquadFilterApply(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 biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
+float biquadFilterApplyDF2(biquadFilter_t *filter, float input)
 {
     const float result = filter->b0 * input + filter->d1;
     filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;

src/main/common/filter.h

@@ -23,6 +23,11 @@
 #define MAX_FIR_DENOISE_WINDOW_SIZE 120
 #endif
 
+#define M_LN2_FLOAT 0.69314718055994530942f
+#define M_PI_FLOAT  3.14159265358979323846f
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
+
 typedef struct pt1Filter_s {
     float state;
     float k;
@@ -33,6 +38,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 x1, x2, y1, y2;
     float d1, d2;
 } biquadFilter_t;
 
@@ -52,7 +58,8 @@ typedef enum {
 
 typedef enum {
     FILTER_LPF,
-    FILTER_NOTCH
+    FILTER_NOTCH,
+    FILTER_BPF,
 } biquadFilterType_e;
 
 typedef struct firFilter_s {
@@ -71,9 +78,14 @@ float nullFilterApply(void *filter, float input);
 
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
 float biquadFilterApply(biquadFilter_t *filter, float input);
+float biquadFilterApplyDF2(biquadFilter_t *filter, float input);
 float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
 
+// not exactly correct, but very very close and much much faster
+#define filterGetNotchQApprox(centerFreq, cutoff)   ((float)(cutoff * centerFreq) / ((float)(centerFreq - cutoff) * (float)(centerFreq + cutoff)))
+
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT);

src/main/fc/config.h

@@ -60,6 +60,7 @@ typedef enum {
     FEATURE_SOFTSPI = 1 << 26,
     FEATURE_ESC_SENSOR = 1 << 27,
     FEATURE_ANTI_GRAVITY = 1 << 28,
+    FEATURE_DYNAMIC_FILTER = 1 << 29,
 } features_e;
 
 #define MAX_NAME_LENGTH 16

src/main/fc/fc_msp.c

@@ -135,7 +135,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXGOV, "GOVERNOR", 18 },
     { BOXOSD, "OSD SW", 19 },
     { BOXTELEMETRY, "TELEMETRY", 20 },
-    { BOXGTUNE, "GTUNE", 21 },
+    { BOXDYNAMICFILTER, "DYNAMIC FILTER", 21 },
     { BOXSONAR, "SONAR", 22 },
     { BOXSERVO1, "SERVO1", 23 },
     { BOXSERVO2, "SERVO2", 24 },
@@ -306,6 +306,10 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY;
     }
 
+    if (!feature(FEATURE_DYNAMIC_FILTER)) {
+        activeBoxIds[activeBoxIdCount++] = BOXDYNAMICFILTER;
+    }
+	
     if (sensors(SENSOR_ACC)) {
         activeBoxIds[activeBoxIdCount++] = BOXANGLE;
         activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
@@ -418,7 +422,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGOV)) << BOXGOV |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
-        IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
+        IS_ENABLED(IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER)) << BOXDYNAMICFILTER |
         IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
         IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |

src/main/fc/fc_tasks.c

@@ -75,7 +75,6 @@
 #include "sensors/battery.h"
 #include "sensors/compass.h"
 #include "sensors/gyro.h"
-#include "sensors/gyroanalyse.h"
 #include "sensors/sonar.h"
 #include "sensors/esc_sensor.h"
 
@@ -352,9 +351,6 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_VTXCTRL, true);
 #endif
 #endif
-#ifdef USE_GYRO_DATA_ANALYSE
-    setTaskEnabled(TASK_GYRO_DATA_ANALYSE, true);
-#endif
 }
 #endif
 
@@ -597,14 +593,5 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .staticPriority = TASK_PRIORITY_IDLE,
     },
 #endif
-
-#ifdef USE_GYRO_DATA_ANALYSE
-    [TASK_GYRO_DATA_ANALYSE] = {
-        .taskName = "GYROFFT",
-        .taskFunc = gyroDataAnalyseUpdate,
-        .desiredPeriod = TASK_PERIOD_HZ(100),        // 100 Hz, 10ms
-        .staticPriority = TASK_PRIORITY_MEDIUM,
-    },
-#endif
 #endif
 };

src/main/fc/rc_controls.h

@@ -43,7 +43,7 @@ typedef enum {
     BOXGOV,
     BOXOSD,
     BOXTELEMETRY,
-    BOXGTUNE,
+    BOXDYNAMICFILTER,
     BOXSONAR,
     BOXSERVO1,
     BOXSERVO2,

src/main/flight/pid.c

@@ -171,7 +171,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
     if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) {
         dtermNotchFilterApplyFn = nullFilterApply;
     } else {
-        dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
         const float notchQ = filterGetNotchQ(pidProfile->dterm_notch_hz, pidProfile->dterm_notch_cutoff);
         for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
             dtermFilterNotch[axis] = &biquadFilterNotch[axis];
@@ -194,7 +194,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
             }
             break;
         case FILTER_BIQUAD:
-            dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            dtermLpfApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
             for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
                 dtermFilterLpf[axis] = &biquadFilter[axis];
                 biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);

src/main/flight/servos.c

@@ -514,7 +514,7 @@ static void filterServos(void)
                 servoFilterIsSet = true;
             }
 
-            servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx]));
+            servo[servoIdx] = lrintf(biquadFilterApplyDF2(&servoFilter[servoIdx], (float)servo[servoIdx]));
             // Sanity check
             servo[servoIdx] = constrain(servo[servoIdx], servoParams(servoIdx)->min, servoParams(servoIdx)->max);
         }

src/main/scheduler/scheduler.h

@@ -110,9 +110,6 @@ typedef enum {
 #ifdef VTX_CONTROL
     TASK_VTXCTRL,
 #endif
-#ifdef USE_GYRO_DATA_ANALYSE
-    TASK_GYRO_DATA_ANALYSE,
-#endif
 
     /* Count of real tasks */
     TASK_COUNT,

src/main/sensors/acceleration.c

@@ -465,7 +465,7 @@ void accUpdate(rollAndPitchTrims_t *rollAndPitchTrims)
 
     if (accLpfCutHz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-            acc.accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)acc.accSmooth[axis]));
+            acc.accSmooth[axis] = lrintf(biquadFilterApplyDF2(&accFilter[axis], (float)acc.accSmooth[axis]));
         }
     }
 

src/main/sensors/current.c

@@ -135,7 +135,7 @@ void currentMeterADCRefresh(int32_t lastUpdateAt)
 {
     const uint16_t iBatSample = adcGetChannel(ADC_CURRENT);
     currentMeterADCState.amperageLatest = currentMeterADCToCentiamps(iBatSample);
-    currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApply(&adciBatFilter, iBatSample));
+    currentMeterADCState.amperage = currentMeterADCToCentiamps(biquadFilterApplyDF2(&adciBatFilter, iBatSample));
 
     updateCurrentmAhDrawnState(&currentMeterADCState.mahDrawnState, currentMeterADCState.amperageLatest, lastUpdateAt);
 }

src/main/sensors/gyro.c

@@ -102,6 +102,9 @@ static gyroSensor_t gyroSensor0;
 
 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 
+static filterApplyFnPtr notchFilterDynApplyFn;
+biquadFilter_t *notchFilterDyn[3];
+
 #define DEBUG_GYRO_CALIBRATION 3
 
 #ifdef STM32F10X
@@ -367,7 +370,7 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
     if (lpfHz && lpfHz <= gyroFrequencyNyquist) {  // Initialisation needs to happen once samplingrate is known
         switch (gyroConfig()->gyro_soft_lpf_type) {
         case FILTER_BIQUAD:
-            gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
             for (int axis = 0; axis < 3; axis++) {
                 gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis];
                 biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime);
@@ -397,7 +400,7 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
     gyroSensor->notchFilter1ApplyFn = nullFilterApply;
     const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
     if (notchHz && notchHz <= gyroFrequencyNyquist) {
-        gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
         for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@@ -411,7 +414,7 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
     gyroSensor->notchFilter2ApplyFn = nullFilterApply;
     const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
     if (notchHz && notchHz <= gyroFrequencyNyquist) {
-        gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApplyDF2;
         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
         for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
@@ -426,9 +429,23 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
     gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 }
 
+void gyroInitFilterDynamicNotch()
+{
+    static biquadFilter_t gyroFilterNotch[XYZ_AXIS_COUNT];
+
+    notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; //must be this function, do not change
+
+    const float notchQ = filterGetNotchQ(400, 390); //just any init value
+    for (int axis = 0; axis < 3; axis++) {
+        notchFilterDyn[axis] = &gyroFilterNotch[axis];
+        biquadFilterInit(notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+    }
+}
+
 void gyroInitFilters(void)
 {
     gyroInitSensorFilters(&gyroSensor0);
+    gyroInitFilterDynamicNotch();
 }
 
 bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
@@ -506,6 +523,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
 void gyroUpdateSensor(gyroSensor_t *gyroSensor)
 {
     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
+
         return;
     }
     gyroSensor->gyroDev.dataReady = false;
@@ -531,14 +549,27 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
         // scale gyro output to degrees per second
         float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
 
+#ifdef USE_GYRO_DATA_ANALYSE
+        // Apply Dynamic Notch filtering
+        if (axis == 0)
+            DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
+
+        if (isDynamicFilterActive())
+            gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf);
+
+        if (axis == 0)
+            DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
+#endif
+
+        // Apply Static Notch filtering
+        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
+        gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
+        gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
+
         // Apply LPF
         DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
         gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf);
 
-        // Apply Notch filtering
-        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
-        gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf);
-        gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf);
         gyro.gyroADCf[axis] = gyroADCf;
     }
 

src/main/sensors/gyro.h

@@ -66,6 +66,7 @@ typedef struct gyroConfig_s {
 PG_DECLARE(gyroConfig_t, gyroConfig);
 
 bool gyroInit(void);
+
 void gyroInitFilters(void);
 void gyroUpdate(void);
 const busDevice_t *gyroSensorBus(void);

src/main/sensors/gyroanalyse.c

@@ -1,35 +1,321 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 is distributed in the hope that it 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 Cleanflight.	If not, see <http://www.gnu.org/licenses/>.
+ */
+
 #include <stdint.h>
 
 #include "platform.h"
 
 #ifdef USE_GYRO_DATA_ANALYSE
-
 #include "arm_math.h"
 
 #include "build/debug.h"
 
+#include "common/filter.h"
 #include "common/maths.h"
 #include "common/time.h"
 #include "common/utils.h"
 
+#include "config/feature.h"
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
 #include "drivers/accgyro/accgyro.h"
+#include "drivers/system.h"
+
+#include "fc/config.h"
+#include "fc/rc_controls.h"
 
 #include "sensors/gyro.h"
 #include "sensors/gyroanalyse.h"
 
+#include "common/filter.h"
 
-void gyroDataAnalyseInit(void)
+// 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
+// Eg [0,31), [31,62), [62, 93) etc
+
+#define FFT_WINDOW_SIZE                32  // max for f3 targets
+#define FFT_MIN_FREQ                  100  // not interested in filtering frequencies below 100Hz
+#define FFT_SAMPLING_RATE            1000  // allows analysis up to 500Hz which is more than motors create
+#define FFT_BPF_HZ                    200  // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
+#define DYN_NOTCH_WIDTH               100  // just an orientation and start value
+#define DYN_NOTCH_CHANGERATE           40  // lower cut does not improve the performance much, higher cut makes it worse...
+#define DYN_NOTCH_MIN_CUTOFF          120  // don't cut too deep into low frequencies
+#define DYN_NOTCH_MAX_CUTOFF          200  // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
+
+static uint16_t samplingFrequency;          // gyro rate
+static uint8_t fftBinCount;
+static float fftResolution;                 // hz per bin
+static float gyroData[3][FFT_WINDOW_SIZE];  // gyro data used for frequency analysis
+
+static arm_rfft_fast_instance_f32 fftInstance;
+static float fftData[FFT_WINDOW_SIZE];
+static float rfftData[FFT_WINDOW_SIZE];
+static gyroFftData_t fftResult[3];
+static uint16_t fftMaxFreq = 0;             // nyquist rate
+static uint16_t fftIdx = 0;                 // use a circular buffer for the last FFT_WINDOW_SIZE samples
+
+
+// accumulator for oversampled data => no aliasing and less noise
+static float fftAcc[3] = {0, 0, 0};
+static int fftAccCount = 0;
+static int fftSamplingScale;
+
+// bandpass filter gyro data
+static biquadFilter_t fftGyroFilter[3];
+
+// filter for smoothing frequency estimation
+static biquadFilter_t fftFreqFilter[3];
+
+// Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
+static float hanningWindow[FFT_WINDOW_SIZE];
+
+void initHanning() {
+    for (int i = 0; i < FFT_WINDOW_SIZE; ++i) {
+        hanningWindow[i] = (0.5 - 0.5 * cosf(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
+    }
 }
 
-void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro)
+void initGyroData() {
-{
+    for (int axis = 0; axis < 3; ++axis) {
-    UNUSED(gyroDev);
+        for (int i = 0; i < FFT_WINDOW_SIZE; ++i) {
+            gyroData[axis][i] = 0;
+        }
+    }
+}
+
+static inline int fftFreqToBin(int freq) {
+    return ((FFT_WINDOW_SIZE / 2 - 1) * freq) / (fftMaxFreq);
+}
+
+void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
+    // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
+    samplingFrequency = 1000000 / targetLooptimeUs;
+    fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
+    fftMaxFreq = FFT_SAMPLING_RATE / 2;
+    fftBinCount = fftFreqToBin(fftMaxFreq) + 1;
+    fftResolution = FFT_SAMPLING_RATE / FFT_WINDOW_SIZE;
+    arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
+
+    initGyroData();
+    initHanning();
+
+    // recalculation of filters takes 4 calls per axis => each filter gets updated every 3 * 4 = 12 calls
+    // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
+    float looptime = targetLooptimeUs * 4 * 3;
+    for (int axis = 0; axis < 3; ++axis) {
+        fftResult[axis].centerFreq = 200;
+        biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
+        biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
+    }
+}
+
+// used in OSD
+const gyroFftData_t *gyroFftData(int axis) {
+    return &fftResult[axis];
+}
+
+bool isDynamicFilterActive(void) {
+    return (IS_RC_MODE_ACTIVE(BOXDYNAMICFILTER) || feature(FEATURE_DYNAMIC_FILTER));
+}
+
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
+    if (!isDynamicFilterActive()) {
+        return;
+    }
     UNUSED(gyro);
+
+    // if gyro sampling is > 1kHz, accumulate multiple samples
+    for (int axis = 0; axis < 3; ++axis) {
+        fftAcc[axis] += gyroDev->gyroADC[axis] * gyroDev->scale;
+    }
+    fftAccCount++;
+
+    // this runs at 1kHz
+    if (fftAccCount == fftSamplingScale) {
+        fftAccCount = 0;
+
+        //calculate mean value of accumulated samples
+        for (int axis = 0; axis < 3; ++axis) {
+            float sample = fftAcc[axis] / fftSamplingScale;
+            sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample);
+            gyroData[axis][fftIdx] = sample;
+            if (axis == 0)
+                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
+            fftAcc[axis] = 0;
+        }
+
+        fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
+    }
+
+    // calculate FFT and update filters
+    gyroDataAnalyseUpdate();
 }
 
-void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
+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);
-    UNUSED(currentTimeUs);
+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);
+
+typedef 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
+} UpdateStep_e;
+
+/*
+ * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
+ */
+void gyroDataAnalyseUpdate() {
+    static int axis = 0;
+    static int step = 0;
+    arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
+
+    uint32_t startTime = 0;
+    if (debugMode == (DEBUG_FFT_TIME))
+        startTime = micros();
+
+    DEBUG_SET(DEBUG_FFT_TIME, 0, step);
+    switch (step) {
+        case STEP_ARM_CFFT_F32:
+        {
+            switch (FFT_WINDOW_SIZE / 2) {
+            case 16:
+                // 16us
+                arm_cfft_radix8by2_f32(Sint, fftData);
+                break;
+            case 32:
+                // 35us
+                arm_cfft_radix8by4_f32(Sint, fftData);
+                break;
+            case 64:
+                // 70us
+                arm_radix8_butterfly_f32(fftData, FFT_WINDOW_SIZE / 2, Sint->pTwiddle, 1);
+                break;
+            }
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            break;
+        }
+        case STEP_BITREVERSAL:
+        {
+            // 6us
+            arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            step++;
+            //break;
+        }
+        case STEP_STAGE_RFFT_F32:
+        {
+            // 14us
+            // this does not work in place => fftData AND rfftData needed
+            stage_rfft_f32(&fftInstance, fftData, rfftData);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            break;
+        }
+        case STEP_ARM_CMPLX_MAG_F32:
+        {
+            // 8us
+            arm_cmplx_mag_f32(rfftData, fftData, fftBinCount);
+            DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
+            step++;
+            //break;
+        }
+        case STEP_CALC_FREQUENCIES:
+        {
+            // 13us
+            float fftSum = 0;
+            float fftWeightedSum = 0;
+
+            fftResult[axis].maxVal = 0;
+            // iterate over fft data and calculate weighted indexes
+            float squaredData;
+            for (int i = 0; i < fftBinCount; ++i) {
+                squaredData = fftData[i] * fftData[i];  //more weight on higher peaks
+                fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
+                fftSum += squaredData;
+                fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0
+            }
+
+            // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
+            if (fftSum > 0) {
+                // idx was shifted by 1 to start at 1, not 0
+                float fftMeanIndex = (fftWeightedSum / fftSum) - 1;
+                // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
+                // fftMeanIndex += 0.5;
+
+                // don't go below the minimal cutoff frequency + 10 and don't jump around too much
+                float centerFreq;
+                centerFreq = constrain(fftMeanIndex * fftResolution, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
+                centerFreq = biquadFilterApplyDF2(&fftFreqFilter[axis], centerFreq);
+                centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, fftMaxFreq);
+                fftResult[axis].centerFreq = centerFreq;
+                if (axis == 0) {
+                    DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
+                }
+            }
+
+            // copy data for display in OSD
+            const float scaleFactor = 255.0 / MIN(1, fftResult[axis].maxVal);
+            const int count = MIN(GYRO_FFT_BIN_COUNT, fftBinCount);
+            for (int ii = 0; ii < count; ++ii) {
+                fftResult[axis].bins[ii] = fftData[ii] * scaleFactor;
+            }
+
+
+            DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+            break;
+        }
+        case STEP_UPDATE_FILTERS:
+        {
+            // 7us
+            // calculate new filter coefficients
+            float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
+            float notchQ = filterGetNotchQApprox(fftResult[axis].centerFreq, cutoffFreq);
+            biquadFilterUpdate(notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+
+            axis = (axis + 1) % 3;
+            step++;
+        }
+        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
+            uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx;
+            arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx);
+            if (fftIdx > 0)
+                arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx);
+
+            DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
+        }
+    }
+
+    step = (step + 1) % STEP_COUNT;
 }
+
 #endif // USE_GYRO_DATA_ANALYSE

src/main/sensors/gyroanalyse.h

@@ -18,9 +18,21 @@
 #pragma once
 
 #include "common/time.h"
+#include "common/filter.h"
 
-void gyroDataAnalyseInit(void);
+#define GYRO_FFT_BIN_COUNT      16 // FFT_WINDOW_SIZE / 2
+typedef struct gyroFftData_s {
+    uint8_t bins[GYRO_FFT_BIN_COUNT];
+    float maxVal;
+    uint16_t centerFreq;
+} gyroFftData_t;
+
+void gyroDataAnalyseInit(uint32_t targetLooptime);
+const gyroFftData_t *gyroFftData(int axis);
 struct gyroDev_s;
 struct gyro_s;
 void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
-void gyroDataAnalyseUpdate(timeUs_t currentTimeUs);
+void gyroDataAnalyseUpdate();
+bool isDynamicFilterActive();
+
+extern biquadFilter_t *notchFilterDyn[3];

src/main/sensors/voltage.c

@@ -153,7 +153,7 @@ void voltageMeterADCRefresh(void)
         uint8_t channel = voltageMeterAdcChannelMap[i];
         uint16_t rawSample = adcGetChannel(channel);
 
-        uint16_t filteredSample = biquadFilterApply(&state->filter, rawSample);
+        uint16_t filteredSample = biquadFilterApplyDF2(&state->filter, rawSample);
 
         // always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
         state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
@@ -210,7 +210,7 @@ void voltageMeterESCRefresh(void)
 #ifdef USE_ESC_SENSOR
     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
     voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage / 10 : 0;
-    voltageMeterESCState.voltageFiltered = biquadFilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
+    voltageMeterESCState.voltageFiltered = biquadFilterApplyDF2(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
 #endif
 }
 

src/main/target/common_fc_pre.h

@@ -41,6 +41,7 @@
 #ifdef STM32F3
 #define MINIMAL_CLI
 #define USE_DSHOT
+#define USE_GYRO_DATA_ANALYSE
 #endif
 
 #ifdef STM32F4