fix build errors

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

@@ -96,15 +96,14 @@ 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;
 
 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 
-static filterApplyFnPtr notchFilterDynApplyFn;
-biquadFilter_t *notchFilterDyn[3];
-
 #define DEBUG_GYRO_CALIBRATION 3
 
 #ifdef STM32F10X
@@ -410,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) {
@@ -422,30 +420,26 @@ void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t n
     }
 }
 
+void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
+{
+    gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApply; // 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 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)
@@ -545,6 +539,10 @@ 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;
@@ -555,7 +553,7 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
             DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
 
         if (isDynamicFilterActive())
-            gyroADCf = notchFilterDynApplyFn(notchFilterDyn[axis], gyroADCf);
+            gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf);
 
         if (axis == 0)
             DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
@@ -572,10 +570,6 @@ void gyroUpdateSensor(gyroSensor_t *gyroSensor)
 
         gyro.gyroADCf[axis] = gyroADCf;
     }
-
-#ifdef USE_GYRO_DATA_ANALYSE
-    gyroDataAnalyse(&gyroSensor->gyroDev, &gyro);
-#endif
 }
 
 void gyroUpdate(void)

src/main/sensors/gyroanalyse.c

@@ -53,7 +53,7 @@
 #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_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)
 
@@ -118,7 +118,7 @@ void gyroDataAnalyseInit(uint32_t targetLooptimeUs) {
     // 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;
+        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);
     }
@@ -136,15 +136,14 @@ bool isDynamicFilterActive(void) {
 /*
  * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
  */
-void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
+void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) {
     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;
+        fftAcc[axis] += gyroDev->gyroADC[axis];
     }
     fftAccCount++;
 
@@ -158,7 +157,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
             sample = biquadFilterApplyDF2(&fftGyroFilter[axis], sample);
             gyroData[axis][fftIdx] = sample;
             if (axis == 0)
-                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
+                DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
             fftAcc[axis] = 0;
         }
 
@@ -166,7 +165,7 @@ void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro) {
     }
 
     // calculate FFT and update filters
-    gyroDataAnalyseUpdate();
+    gyroDataAnalyseUpdate(notchFilterDyn);
 }
 
 void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut);
@@ -189,7 +188,7 @@ typedef enum {
 /*
  * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
  */
-void gyroDataAnalyseUpdate() {
+void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) {
     static int axis = 0;
     static int step = 0;
     arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
@@ -295,7 +294,7 @@ void gyroDataAnalyseUpdate() {
             // 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);
+            biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 
             axis = (axis + 1) % 3;

src/main/sensors/gyroanalyse.h

@@ -30,9 +30,6 @@ typedef struct gyroFftData_s {
 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();
 bool isDynamicFilterActive();
-
-extern biquadFilter_t *notchFilterDyn[3];