Merge pull request #8253 from iNavFlight/dzikuvx-secondary-gyro-notch

Update Matrix Filter to 3D Matrix Filter

docs/Settings.md

@@ -752,6 +752,16 @@ Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`,
 
 ---
 
+### dynamic_gyro_notch_3d_q
+
+Q factor for 3D dynamic notches
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 200 | 1 | 1000 |
+
+---
+
 ### dynamic_gyro_notch_enabled
 
 Enable/disable dynamic gyro notch also known as Matrix Filter
@@ -772,6 +782,16 @@ Minimum frequency for dynamic notches. Default value of `150` works best with 5"
 
 ---
 
+### dynamic_gyro_notch_mode
+
+Gyro dynamic notch type
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2D |  |  |
+
+---
+
 ### dynamic_gyro_notch_q
 
 Q factor for dynamic notches

src/main/CMakeLists.txt

@@ -331,6 +331,8 @@ main_sources(COMMON_SRC
     flight/rpm_filter.h
     flight/dynamic_gyro_notch.c
     flight/dynamic_gyro_notch.h
+    flight/secondary_dynamic_gyro_notch.c
+    flight/secondary_dynamic_gyro_notch.h
     flight/dynamic_lpf.c
     flight/dynamic_lpf.h
     flight/secondary_imu.c

src/main/fc/settings.yaml

@@ -187,6 +187,9 @@ tables:
   - name: rth_trackback_mode
     values: ["OFF", "ON", "FS"]
     enum: rthTrackbackMode_e
+  - name: dynamic_gyro_notch_mode
+    values: ["2D", "3D_R", "3D_P", "3D_Y", "3D_RP", "3D_RY", "3D_PY", "3D"]
+    enum: dynamicGyroNotchMode_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -285,6 +288,19 @@ groups:
         condition: USE_DYNAMIC_FILTERS
         min: 30
         max: 250
+      - name: dynamic_gyro_notch_mode
+        description: "Gyro dynamic notch type"
+        default_value: "2D"
+        table: dynamic_gyro_notch_mode
+        field: dynamicGyroNotchMode
+        condition: USE_DYNAMIC_FILTERS
+      - name: dynamic_gyro_notch_3d_q
+        description: "Q factor for 3D dynamic notches"
+        default_value: 200
+        field: dynamicGyroNotch3dQ
+        condition: USE_DYNAMIC_FILTERS
+        min: 1
+        max: 1000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
         min: 0

src/main/flight/dynamic_gyro_notch.h

@@ -31,14 +31,12 @@
 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
 
 /*
- * Number of peaks to detect with Dynamic Notch Filter aka Matrixc Filter. This is equal to the number of dynamic notch filters
+ * Number of peaks to detect with Dynamic Notch Filter aka Matrix Filter. This is equal to the number of dynamic notch filters
  */
 #define DYN_NOTCH_PEAK_COUNT 3
 typedef struct dynamicGyroNotchState_s {
     uint16_t frequency[XYZ_AXIS_COUNT][DYN_NOTCH_PEAK_COUNT];
     float dynNotchQ;
-    float dynNotch1Ctr;
-    float dynNotch2Ctr;
     uint32_t looptime;
     uint8_t enabled;
     

src/main/flight/secondary_dynamic_gyro_notch.c

@@ -0,0 +1,112 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#include "platform.h"
+
+#ifdef USE_DYNAMIC_FILTERS
+
+FILE_COMPILE_FOR_SPEED
+
+#include <stdint.h>
+#include "secondary_dynamic_gyro_notch.h"
+#include "fc/config.h"
+#include "build/debug.h"
+#include "sensors/gyro.h"
+
+#define SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 150
+
+void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state) {
+
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        state->filtersApplyFn[axis] = nullFilterApply;
+    }
+
+    state->dynNotchQ = gyroConfig()->dynamicGyroNotch3dQ / 100.0f;
+    state->enabled = gyroConfig()->dynamicGyroNotchMode != DYNAMIC_NOTCH_MODE_2D;
+    state->looptime = getLooptime();
+
+    if (
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_R ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
+    ) {
+        /* 
+         * Enable ROLL filter
+         */
+        biquadFilterInit(&state->filters[FD_ROLL], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+        state->filtersApplyFn[FD_ROLL] = (filterApplyFnPtr)biquadFilterApplyDF1;
+    }
+
+    if (
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_P ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RP ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
+    ) {
+        /* 
+         * Enable PITCH filter
+         */
+        biquadFilterInit(&state->filters[FD_PITCH], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+        state->filtersApplyFn[FD_PITCH] = (filterApplyFnPtr)biquadFilterApplyDF1;
+    }
+
+    if (
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_Y ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_PY ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_RY ||
+        gyroConfig()->dynamicGyroNotchMode == DYNAMIC_NOTCH_MODE_3D
+    ) {
+        /* 
+         * Enable YAW filter
+         */
+        biquadFilterInit(&state->filters[FD_YAW], SECONDARY_DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, state->looptime, 1.0f, FILTER_NOTCH);
+        state->filtersApplyFn[FD_YAW] = (filterApplyFnPtr)biquadFilterApplyDF1;
+    }
+
+    
+}
+
+void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]) {
+
+    if (state->enabled) {
+
+        /*
+         * The secondary dynamic notch uses only the first detected frequency
+         * The rest of peaks are ignored
+         */
+        state->frequency[axis] = frequency[0];
+
+        // Filter update happens only if peak was detected 
+        if (frequency[0] > 0.0f) {
+            biquadFilterUpdate(&state->filters[axis], state->frequency[axis], state->looptime, state->dynNotchQ, FILTER_NOTCH);
+        }
+    }
+}
+
+float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input) {
+    return state->filtersApplyFn[axis]((filter_t *)&state->filters[axis], input);
+}
+
+#endif

src/main/flight/secondary_dynamic_gyro_notch.h

@@ -0,0 +1,43 @@
+/*
+ * This file is part of INAV Project.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute 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.
+ *
+ * This file 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 this program. If not, see http://www.gnu.org/licenses/.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include "common/axis.h"
+#include "common/filter.h"
+
+typedef struct secondaryDynamicGyroNotchState_s {
+    uint16_t frequency[XYZ_AXIS_COUNT];
+    float dynNotchQ;
+    uint32_t looptime;
+    uint8_t enabled;
+    
+    biquadFilter_t filters[XYZ_AXIS_COUNT];
+    filterApplyFnPtr filtersApplyFn[XYZ_AXIS_COUNT];
+} secondaryDynamicGyroNotchState_t;
+
+void secondaryDynamicGyroNotchFiltersInit(secondaryDynamicGyroNotchState_t *state);
+void secondaryDynamicGyroNotchFiltersUpdate(secondaryDynamicGyroNotchState_t *state, int axis, float frequency[]);
+float secondaryDynamicGyroNotchFiltersApply(secondaryDynamicGyroNotchState_t *state, int axis, float input);

src/main/sensors/gyro.c

@@ -92,10 +92,11 @@ STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
 
 EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
 EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
+EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 5);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@@ -116,6 +117,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
+    .dynamicGyroNotchMode = SETTING_DYNAMIC_GYRO_NOTCH_MODE_DEFAULT,
+    .dynamicGyroNotch3dQ = SETTING_DYNAMIC_GYRO_NOTCH_3D_Q_DEFAULT,
 #endif
 #ifdef USE_GYRO_KALMAN
     .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
@@ -298,6 +301,9 @@ bool gyroInit(void)
 #ifdef USE_DYNAMIC_FILTERS
     // Dynamic notch running at PID frequency
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
+
+    secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState);
+
     gyroDataAnalyseStateInit(
         &gyroAnalyseState,
         gyroConfig()->dynamicGyroNotchMinHz,
@@ -449,6 +455,14 @@ void FAST_CODE NOINLINE gyroFilter()
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
             gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
         }
+
+        /**
+         * Secondary dynamic notch filter. 
+         * In some cases, noise amplitude is high enough not to be filtered by the primary filter.
+         * This happens on the first frequency with the biggest aplitude
+         */
+        gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf);
+
 #endif
 
 #ifdef USE_GYRO_KALMAN
@@ -470,6 +484,13 @@ void FAST_CODE NOINLINE gyroFilter()
                 gyroAnalyseState.filterUpdateAxis,
                 gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
             );
+
+            secondaryDynamicGyroNotchFiltersUpdate(
+                &secondaryDynamicGyroNotchState,
+                gyroAnalyseState.filterUpdateAxis,
+                gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
+            );
+
         }
     }
 #endif

src/main/sensors/gyro.h

@@ -24,6 +24,7 @@
 #include "config/parameter_group.h"
 #include "drivers/sensor.h"
 #include "flight/dynamic_gyro_notch.h"
+#include "flight/secondary_dynamic_gyro_notch.h"
 
 typedef enum {
     GYRO_NONE = 0,
@@ -39,6 +40,17 @@ typedef enum {
     GYRO_FAKE
 } gyroSensor_e;
 
+typedef enum {
+    DYNAMIC_NOTCH_MODE_2D = 0,
+    DYNAMIC_NOTCH_MODE_R,
+    DYNAMIC_NOTCH_MODE_P,
+    DYNAMIC_NOTCH_MODE_Y,
+    DYNAMIC_NOTCH_MODE_RP,
+    DYNAMIC_NOTCH_MODE_RY,
+    DYNAMIC_NOTCH_MODE_PY,
+    DYNAMIC_NOTCH_MODE_3D
+} dynamicGyroNotchMode_e;
+
 typedef struct gyro_s {
     bool initialized;
     uint32_t targetLooptime;
@@ -68,6 +80,8 @@ typedef struct gyroConfig_s {
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;
+    uint8_t dynamicGyroNotchMode;
+    uint16_t dynamicGyroNotch3dQ;
 #endif
 #ifdef USE_GYRO_KALMAN
     uint16_t kalman_q;