Merge pull request #3074 from rav-rav/dynamic_notch_pr

Dynamic notch

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

@@ -26,11 +26,9 @@
 
 #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 +77,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,6 +105,14 @@ 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
@@ -117,10 +123,50 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
     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 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 biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
 float biquadFilterApply(biquadFilter_t *filter, float input)
 {
     const float result = filter->b0 * input + filter->d1;

src/main/common/filter.h

@@ -33,6 +33,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 +53,8 @@ typedef enum {
 
 typedef enum {
     FILTER_LPF,
-    FILTER_NOTCH
+    FILTER_NOTCH,
+    FILTER_BPF,
 } biquadFilterType_e;
 
 typedef struct firFilter_s {
@@ -71,9 +73,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 biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float biquadFilterApply(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/cli.c

@@ -150,7 +150,7 @@ static const char * const featureNames[] = {
     "SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
     "RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
     "UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
-    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
+    "SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
 };
 
 // sync this with rxFailsafeChannelMode_e

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_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/settings.c

@@ -204,7 +204,10 @@ static const char * const lookupTableDebug[DEBUG_COUNT] = {
     "STACK",
     "ESC_SENSOR_RPM",
     "ESC_SENSOR_TMP",
-    "ALTITUDE"
+    "ALTITUDE",
+    "FFT",
+    "FFT_TIME",
+    "FFT_FREQ"
 };
 
 #ifdef OSD

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/gyro.c

@@ -96,6 +96,8 @@ typedef struct gyroSensor_s {
     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
     filterApplyFnPtr notchFilter2ApplyFn;
     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
+    filterApplyFnPtr notchFilterDynApplyFn;
+    biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
 } gyroSensor_t;
 
 static gyroSensor_t gyroSensor0;
@@ -407,7 +409,6 @@ void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
 
 void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 {
-
     gyroSensor->notchFilter2ApplyFn = nullFilterApply;
     const uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
     if (notchHz && notchHz <= gyroFrequencyNyquist) {
@@ -419,11 +420,21 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
     }
 }
 
+void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
+{
+    gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
+    const float notchQ = filterGetNotchQ(400, 390); //just any init value
+    for (int axis = 0; axis < 3; axis++) {
+        biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
+    }
+}
+
 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 {
     gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
     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);
+    gyroInitFilterDynamicNotch(gyroSensor);
 }
 
 void gyroInitFilters(void)
@@ -506,6 +517,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;
@@ -527,24 +539,37 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
         return;
     }
 
+#ifdef USE_GYRO_DATA_ANALYSE
+    gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
+#endif
+
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         // 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 = gyroSensor->notchFilterDynApplyFn(&gyroSensor->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;
     }
-
-#ifdef USE_GYRO_DATA_ANALYSE
-    gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
-#endif
 }
 
 void gyroUpdate(void)

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,323 @@
+/*
+ * 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           60  // 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)
+
+#define BIQUAD_Q 1.0f / sqrtf(2.0f)         // quality factor - butterworth
+
+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()
 {
-    UNUSED(gyroDev);
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-    UNUSED(gyro);
+        for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
+            gyroData[axis][i] = 0;
+        }
+    }
 }
 
-void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
+static inline int fftFreqToBin(int freq)
 {
-    UNUSED(currentTimeUs);
+    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 < XYZ_AXIS_COUNT; axis++) {
+        fftResult[axis].centerFreq = 200; // any init value
+        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 feature(FEATURE_DYNAMIC_FILTER);
+}
+
+/*
+ * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
+ */
+void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
+{
+    if (!isDynamicFilterActive()) {
+        return;
+    }
+
+    // if gyro sampling is > 1kHz, accumulate multiple samples
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        fftAcc[axis] += gyroDev->gyroADC[axis];
+    }
+    fftAccCount++;
+
+    // this runs at 1kHz
+    if (fftAccCount == fftSamplingScale) {
+        fftAccCount = 0;
+
+        //calculate mean value of accumulated samples
+        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            float sample = fftAcc[axis] / fftSamplingScale;
+            sample = biquadFilterApply(&fftGyroFilter[axis], sample);
+            gyroData[axis][fftIdx] = sample;
+            if (axis == 0)
+                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
+            fftAcc[axis] = 0;
+        }
+
+        fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
+    }
+
+    // calculate FFT and update filters
+    gyroDataAnalyseUpdate(notchFilterDyn);
+}
+
+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);
+
+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(biquadFilter_t *notchFilterDyn)
+{
+    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++;
+            // fall through
+        }
+        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++;
+            // fall through
+        }
+        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 = biquadFilterApply(&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));
+                }
+            }
+
+            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++;
+            // fall through
+        }
+        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,17 @@
 #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 {
+    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, biquadFilter_t *notchFilterDyn);
-void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
+void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn);
-void gyroDataAnalyseUpdate(timeUs_t currentTimeUs);
+bool isDynamicFilterActive();

src/main/target/OMNIBUS/target.h

@@ -20,6 +20,7 @@
 #undef TELEMETRY_IBUS   //no space left
 #undef TELEMETRY_HOTT   //no space left
 #undef TELEMETRY_JETIEXBUS
+#undef USE_GYRO_DATA_ANALYSE
 
 #define TARGET_BOARD_IDENTIFIER "OMNI" // https://en.wikipedia.org/wiki/Omnibus
 

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
@@ -48,6 +49,7 @@
 #define USE_ESC_SENSOR
 #define I2C3_OVERCLOCK true
 #define TELEMETRY_IBUS
+#define USE_GYRO_DATA_ANALYSE
 #endif
 
 #ifdef STM32F7
@@ -56,6 +58,7 @@
 #define I2C3_OVERCLOCK true
 #define I2C4_OVERCLOCK true
 #define TELEMETRY_IBUS
+#define USE_GYRO_DATA_ANALYSE
 #endif
 
 #if defined(STM32F4) || defined(STM32F7)