Merge pull request #7521 from iNavFlight/dzikuvx-simplify-dynamic-notch

Simplify dynamic notch setup and set ON by default

Author: DzikuVx

docs/Settings.md

@@ -788,17 +788,17 @@ Enable/disable dynamic gyro notch also known as Matrix Filter
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF |  |  |
+| ON |  |  |
 
 ---
 
 ### dynamic_gyro_notch_min_hz
 
-Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
+Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70`
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 150 | 30 | 1000 |
+| 50 | 30 | 1000 |
 
 ---
 
@@ -812,16 +812,6 @@ Q factor for dynamic notches
 
 ---
 
-### dynamic_gyro_notch_range
-
-Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers
-
-| Default | Min | Max |
-| --- | --- | --- |
-| MEDIUM |  |  |
-
----
-
 ### eleres_freq
 
 _// TODO_

src/main/blackbox/blackbox.c

@@ -1809,7 +1809,6 @@ static bool blackboxWriteSysinfo(void)
         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d",                     gyroConfig()->gyro_main_lpf_hz);
         BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d",                   gyroConfig()->gyro_main_lpf_type);
 #ifdef USE_DYNAMIC_FILTERS
-        BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d",           gyroConfig()->dynamicGyroNotchRange);
         BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d",               gyroConfig()->dynamicGyroNotchQ);
         BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d",           gyroConfig()->dynamicGyroNotchMinHz);
 #endif

src/main/fc/settings.yaml

@@ -144,9 +144,6 @@ tables:
   - name: rssi_source
     values: ["NONE", "AUTO", "ADC", "CHANNEL", "PROTOCOL", "MSP"]
     enum: rssiSource_e
-  - name: dynamicFilterRangeTable
-    values: ["HIGH", "MEDIUM", "LOW"]
-    enum: dynamicFilterRange_e
   - name: pidTypeTable
     values: ["NONE", "PID", "PIFF", "AUTO"]
     enum: pidType_e
@@ -280,16 +277,10 @@ groups:
         max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
-        default_value: OFF
+        default_value: ON
         field: dynamicGyroNotchEnabled
         condition: USE_DYNAMIC_FILTERS
         type: bool
-      - name: dynamic_gyro_notch_range
-        description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers"
-        default_value: "MEDIUM"
-        field: dynamicGyroNotchRange
-        condition: USE_DYNAMIC_FILTERS
-        table: dynamicFilterRangeTable
       - name: dynamic_gyro_notch_q
         description: "Q factor for dynamic notches"
         default_value: 120
@@ -298,8 +289,8 @@ groups:
         min: 1
         max: 1000
       - name: dynamic_gyro_notch_min_hz
-        description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`"
-        default_value: 150
+        description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirotors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`"
+        default_value: 50
         field: dynamicGyroNotchMinHz
         condition: USE_DYNAMIC_FILTERS
         min: 30

src/main/flight/gyroanalyse.c

@@ -60,25 +60,11 @@ FILE_COMPILE_FOR_SPEED
 void gyroDataAnalyseStateInit(
     gyroAnalyseState_t *state, 
     uint16_t minFrequency,
-    uint8_t range,
     uint32_t targetLooptimeUs
 ) {
-    state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_LOW;
     state->minFrequency = minFrequency;
 
-    if (range == DYN_NOTCH_RANGE_HIGH) {
-        state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_HIGH;
-    }
-    else if (range == DYN_NOTCH_RANGE_MEDIUM) {
-        state->fftSamplingRateHz = DYN_NOTCH_RANGE_HZ_MEDIUM;
-    }
-
-    // If we get at least 3 samples then use the default FFT sample frequency
-    // otherwise we need to calculate a FFT sample frequency to ensure we get 3 samples (gyro loops < 4K)
-    const int gyroLoopRateHz = lrintf((1.0f / targetLooptimeUs) * 1e6f);
-    
-    state->fftSamplingRateHz = MIN((gyroLoopRateHz / 3), state->fftSamplingRateHz);
-
+    state->fftSamplingRateHz = lrintf(1e6f / targetLooptimeUs / 3); // Looptime divided by 3
     state->fftResolution = (float)state->fftSamplingRateHz / FFT_WINDOW_SIZE;
 
     state->fftStartBin = state->minFrequency / lrintf(state->fftResolution);

src/main/flight/gyroanalyse.h

@@ -68,7 +68,6 @@ STATIC_ASSERT(FFT_WINDOW_SIZE <= (uint8_t) -1, window_size_greater_than_underlyi
 void gyroDataAnalyseStateInit(
     gyroAnalyseState_t *state, 
     uint16_t minFrequency,
-    uint8_t range,
     uint32_t targetLooptimeUs
 );
 void gyroDataAnalysePush(gyroAnalyseState_t *gyroAnalyse, int axis, float sample);

src/main/sensors/gyro.c

@@ -124,7 +124,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
     .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
 #ifdef USE_DYNAMIC_FILTERS
-    .dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT,
     .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
@@ -355,7 +354,6 @@ bool gyroInit(void)
     gyroDataAnalyseStateInit(
         &gyroAnalyseState, 
         gyroConfig()->dynamicGyroNotchMinHz,
-        gyroConfig()->dynamicGyroNotchRange,
         getLooptime()
     );
 #endif

src/main/sensors/gyro.h

@@ -41,16 +41,6 @@ typedef enum {
     GYRO_FAKE
 } gyroSensor_e;
 
-typedef enum {
-    DYN_NOTCH_RANGE_HIGH = 0,
-    DYN_NOTCH_RANGE_MEDIUM,
-    DYN_NOTCH_RANGE_LOW
-} dynamicFilterRange_e;
-
-#define DYN_NOTCH_RANGE_HZ_HIGH 2000
-#define DYN_NOTCH_RANGE_HZ_MEDIUM 1333
-#define DYN_NOTCH_RANGE_HZ_LOW 1000
-
 typedef struct gyro_s {
     bool initialized;
     uint32_t targetLooptime;
@@ -78,7 +68,6 @@ typedef struct gyroConfig_s {
     uint16_t gyroDynamicLpfMaxHz;
     uint8_t gyroDynamicLpfCurveExpo;
 #ifdef USE_DYNAMIC_FILTERS
-    uint8_t dynamicGyroNotchRange;
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;