Merge pull request #7597 from iNavFlight/dzikuvx-matrix-filter-rework

Matrix filter rework

src/main/flight/dynamic_gyro_notch.c

@@ -26,6 +26,8 @@
 
 #ifdef USE_DYNAMIC_FILTERS
 
+FILE_COMPILE_FOR_SPEED
+
 #include <stdint.h>
 #include "dynamic_gyro_notch.h"
 #include "fc/config.h"
@@ -33,7 +35,12 @@
 #include "sensors/gyro.h"
 
 void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
-    state->filtersApplyFn = nullFilterApply;
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
+            state->filtersApplyFn[axis][i] = nullFilterApply;
+        }
+    }
 
     state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f;
     state->enabled = gyroConfig()->dynamicGyroNotchEnabled;
@@ -45,27 +52,32 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) {
          */
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
             //Any initial notch Q is valid sice it will be updated immediately after
-            biquadFilterInit(&state->filters[axis][0], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+            for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
-            biquadFilterInit(&state->filters[axis][1], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+                biquadFilterInit(&state->filters[axis][i], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
-            biquadFilterInit(&state->filters[axis][2], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+                state->filtersApplyFn[axis][i] = (filterApplyFnPtr)biquadFilterApplyDF1;
+            }
+        
         }
 
-        state->filtersApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1;
     }
 }
 
-void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency) {
+void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]) {
 
-    state->frequency[axis] = frequency;
+    if (axis == FD_ROLL) {
-
+        for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
-    DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, axis, frequency);
+            DEBUG_SET(DEBUG_DYNAMIC_FILTER_FREQUENCY, i, frequency[i]);
-
+        }
-    if (state->enabled) {
-        biquadFilterUpdate(&state->filters[0][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
-        biquadFilterUpdate(&state->filters[1][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
-        biquadFilterUpdate(&state->filters[2][axis], frequency, state->looptime, state->dynNotchQ, FILTER_NOTCH);
     }
 
+    if (state->enabled) {
+        for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
+            // Filter update happens only if peak was detected 
+            if (frequency[i] > 0.0f) {
+                biquadFilterUpdate(&state->filters[axis][i], frequency[i], state->looptime, state->dynNotchQ, FILTER_NOTCH);
+            }
+        }
+    }
 }
 
 float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input) {
@@ -75,9 +87,9 @@ float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, flo
      * We always apply all filters. If a filter dimension is disabled, one of
      * the function pointers will be a null apply function
      */
-    output = state->filtersApplyFn((filter_t *)&state->filters[axis][0], output);
+    for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
-    output = state->filtersApplyFn((filter_t *)&state->filters[axis][1], output);
+        output = state->filtersApplyFn[axis][i]((filter_t *)&state->filters[axis][i], output);
-    output = state->filtersApplyFn((filter_t *)&state->filters[axis][2], output);
+    }
 
     return output;
 }

src/main/flight/dynamic_gyro_notch.h

@@ -27,23 +27,22 @@
 #include <stdint.h>
 #include "common/axis.h"
 #include "common/filter.h"
+#include "sensors/gyro.h"
 
 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
 
 typedef struct dynamicGyroNotchState_s {
-    uint16_t frequency[XYZ_AXIS_COUNT];
+    // uint16_t frequency[XYZ_AXIS_COUNT];
     float dynNotchQ;
     float dynNotch1Ctr;
     float dynNotch2Ctr;
     uint32_t looptime;
     uint8_t enabled;
-    /*
+    
-     * Dynamic gyro filter can be 3x1, 3x2 or 3x3 depending on filter type
+    biquadFilter_t filters[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
-     */
+    filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
-    biquadFilter_t filters[XYZ_AXIS_COUNT][XYZ_AXIS_COUNT];
-    filterApplyFnPtr filtersApplyFn;
 } dynamicGyroNotchState_t;
 
 void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state);
-void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, uint16_t frequency);
+void dynamicGyroNotchFiltersUpdate(dynamicGyroNotchState_t *state, int axis, float frequency[]);
 float dynamicGyroNotchFiltersApply(dynamicGyroNotchState_t *state, int axis, float input);

src/main/flight/gyroanalyse.c

@@ -61,7 +61,7 @@ enum {
 // 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
+#define DYN_NOTCH_SMOOTH_FREQ_HZ  25
 
 /*
  * Slow down gyro sample acquisition. This lowers the max frequency but increases the resolution.
@@ -93,11 +93,12 @@ void gyroDataAnalyseStateInit(
     const uint32_t filterUpdateUs = targetLooptimeUs * STEP_COUNT * XYZ_AXIS_COUNT;
 
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        // any init value
+        
-        state->centerFreq[axis] = state->maxFrequency;
+        for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
-        state->prevCenterFreq[axis] = state->maxFrequency;
+            state->centerFrequency[axis][i] = state->maxFrequency;
+            pt1FilterInit(&state->detectedFrequencyFilter[axis][i], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs * 1e-6f);
+        }
 
-        biquadFilterInitLPF(&state->detectedFrequencyFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, filterUpdateUs);
     }
 }
 
@@ -135,18 +136,6 @@ void stage_rfft_f32(arm_rfft_fast_instance_f32 *S, float32_t *p, float32_t *pOut
 void arm_cfft_radix8by4_f32(arm_cfft_instance_f32 *S, float32_t *p1);
 void arm_bitreversal_32(uint32_t *pSrc, const uint16_t bitRevLen, const uint16_t *pBitRevTable);
 
-static uint8_t findPeakBinIndex(gyroAnalyseState_t *state) {
-    uint8_t peakBinIndex = state->fftStartBin;
-    float peakValue = 0;
-    for (int i = state->fftStartBin; i < FFT_BIN_COUNT; i++) {
-        if (state->fftData[i] > peakValue) {
-            peakValue = state->fftData[i];
-            peakBinIndex = i;
-        }
-    }
-    return peakBinIndex;
-}
-
 static float computeParabolaMean(gyroAnalyseState_t *state, uint8_t peakBinIndex) {
     float preciseBin = peakBinIndex;
 
@@ -190,38 +179,78 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state)
             // 8us
             arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
 
-            //Find peak frequency           
+            //Zero the data structure
-            uint8_t peakBin = findPeakBinIndex(state);
+            for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
+                state->peaks[i].bin = 0;
+                state->peaks[i].value = 0.0f;
+            }
 
-            // Failsafe to ensure the last bin is not a peak bin
+            // Find peaks
-            peakBin = constrain(peakBin, state->fftStartBin, FFT_BIN_COUNT - 1);
+            for (int bin = (state->fftStartBin + 1); bin < FFT_BIN_COUNT - 1; bin++) {
+                /*
+                 * Peak is defined if the current bin is greater than the previous bin and the next bin
+                 */
+                if (
+                    state->fftData[bin] > state->fftData[bin - 1] && 
+                    state->fftData[bin] > state->fftData[bin + 1]
+                ) {
+                    /*
+                     * We are only interested in N biggest peaks
+                     * Check previously found peaks and update the structure if necessary
+                     */
+                    for (int p = 0; p < DYN_NOTCH_PEAK_COUNT; p++) {
+                        if (state->fftData[bin] > state->peaks[p].value) {
+                            for (int k = DYN_NOTCH_PEAK_COUNT - 1; k > p; k--) {
+                                state->peaks[k] = state->peaks[k - 1];
+                            }
+                            state->peaks[p].bin = bin;
+                            state->peaks[p].value = state->fftData[bin];
+                            break;
+                        }
+                    }
+                    bin++; // If bin is peak, next bin can't be peak => jump it
+                }
+            }
 
-            /*
+            // Sort N biggest peaks in ascending bin order (example: 3, 8, 25, 0, 0, ..., 0)
-             * Calculate center frequency using the parabola method
+            for (int p = DYN_NOTCH_PEAK_COUNT - 1; p > 0; p--) {
-             */
+                for (int k = 0; k < p; k++) {
-            float preciseBin = computeParabolaMean(state, peakBin);
+                    // Swap peaks but ignore swapping void peaks (bin = 0). This leaves
-            float peakFrequency = preciseBin * state->fftResolution;
+                    // void peaks at the end of peaks array without moving them
+                    if (state->peaks[k].bin > state->peaks[k + 1].bin && state->peaks[k + 1].bin != 0) {
+                        peak_t temp = state->peaks[k];
+                        state->peaks[k] = state->peaks[k + 1];
+                        state->peaks[k + 1] = temp;
+                    }
+                }
+            }
 
-            peakFrequency = biquadFilterApply(&state->detectedFrequencyFilter[state->updateAxis], peakFrequency);
-            peakFrequency = constrainf(peakFrequency, state->minFrequency, state->maxFrequency);
-
-            state->prevCenterFreq[state->updateAxis] = state->centerFreq[state->updateAxis];
-            state->centerFreq[state->updateAxis] = peakFrequency;
             break;
         }
         case STEP_UPDATE_FILTERS_AND_HANNING:
         {
-            // 7us
+
-            // calculate cutoffFreq and notch Q, update notch filter  =1.8+((A2-150)*0.004)
+            /*
-            if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
+             * Update frequencies
-                /*
+             */
-                 * Filters will be updated inside dynamicGyroNotchFiltersUpdate()
+            for (int i = 0; i < DYN_NOTCH_PEAK_COUNT; i++) {
-                 */
+
-                state->filterUpdateExecute = true;
+                if (state->peaks[i].bin > 0) {
-                state->filterUpdateAxis = state->updateAxis;
+                    const int bin = constrain(state->peaks[i].bin, state->fftStartBin, FFT_BIN_COUNT - 1);
-                state->filterUpdateFrequency = state->centerFreq[state->updateAxis];
+                    float frequency = computeParabolaMean(state, bin) * state->fftResolution;
+
+                    state->centerFrequency[state->updateAxis][i] = pt1FilterApply(&state->detectedFrequencyFilter[state->updateAxis][i], frequency);
+                } else {
+                    state->centerFrequency[state->updateAxis][i] = 0.0f;
+                }
             }
 
+            /*
+             * Filters will be updated inside dynamicGyroNotchFiltersUpdate()
+             */
+            state->filterUpdateExecute = true;
+            state->filterUpdateAxis = state->updateAxis;
+
             //Switch to the next axis
             state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
             

src/main/flight/gyroanalyse.h

@@ -31,6 +31,11 @@
  */
 #define FFT_WINDOW_SIZE 64
 
+typedef struct peak_s {
+    int bin;
+    float value;
+} peak_t;
+
 typedef struct gyroAnalyseState_s {
     // accumulator for oversampled data => no aliasing and less noise
     float currentSample[XYZ_AXIS_COUNT];
@@ -40,7 +45,6 @@ typedef struct gyroAnalyseState_s {
     float downsampledGyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
 
     // update state machine step information
-    uint8_t updateTicks;
     uint8_t updateStep;
     uint8_t updateAxis;
 
@@ -48,12 +52,15 @@ typedef struct gyroAnalyseState_s {
     float fftData[FFT_WINDOW_SIZE];
     float rfftData[FFT_WINDOW_SIZE];
 
-    biquadFilter_t detectedFrequencyFilter[XYZ_AXIS_COUNT];
+    pt1Filter_t detectedFrequencyFilter[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
-    uint16_t centerFreq[XYZ_AXIS_COUNT];
+    float centerFrequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
-    uint16_t prevCenterFreq[XYZ_AXIS_COUNT];
+    
+    peak_t peaks[DYN_NOTCH_PEAK_COUNT];
+
     bool filterUpdateExecute;
     uint8_t filterUpdateAxis;
     uint16_t filterUpdateFrequency;
+
     uint16_t fftSamplingRateHz;
     uint8_t fftStartBin;
     float fftResolution;

src/main/sensors/gyro.c

@@ -458,8 +458,8 @@ void FAST_CODE NOINLINE gyroFilter()
         if (gyroAnalyseState.filterUpdateExecute) {
             dynamicGyroNotchFiltersUpdate(
                 &dynamicGyroNotchState, 
-                gyroAnalyseState.filterUpdateAxis, 
+                gyroAnalyseState.filterUpdateAxis,
-                gyroAnalyseState.filterUpdateFrequency
+                gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
             );
         }
     }

src/main/sensors/gyro.h

@@ -24,6 +24,11 @@
 #include "config/parameter_group.h"
 #include "drivers/sensor.h"
 
+/*
+ * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters
+ */
+#define DYN_NOTCH_PEAK_COUNT 3
+
 typedef enum {
     GYRO_NONE = 0,
     GYRO_AUTODETECT,