Merge pull request #9879 from iNavFlight/dzikuvx-adaptive-filter

Adaptive gyro filter based on signal statistics experiment

Author: DzikuVx

.vscode/c_cpp_properties.json

@@ -3,24 +3,19 @@
         {
             "name": "Linux",
             "includePath": [
-                "${workspaceRoot}/src/main/**",
-                "${workspaceRoot}/lib/main/**",
-                "/usr/include/**"
+                "${workspaceRoot}",
+                "${workspaceRoot}/src/main/**"
             ],
             "browse": {
                 "limitSymbolsToIncludedHeaders": false,
                 "path": [
-                    "${workspaceRoot}/src/main/**",
-                    "${workspaceRoot}/lib/main/**"
+                    "${workspaceRoot}/**"
                 ]
             },
-            "intelliSenseMode": "linux-gcc-arm",
+            "intelliSenseMode": "msvc-x64",
             "cStandard": "c11",
             "cppStandard": "c++17",
             "defines": [
-                "MCU_FLASH_SIZE 512",
-                "USE_NAV",
-                "NAV_FIXED_WING_LANDING",
                 "USE_OSD",
                 "USE_GYRO_NOTCH_1",
                 "USE_GYRO_NOTCH_2",
@@ -34,30 +29,13 @@
                 "USE_RPM_FILTER",
                 "USE_GLOBAL_FUNCTIONS",
                 "USE_DYNAMIC_FILTERS",
-                "USE_IMU_BNO055",
-                "USE_SECONDARY_IMU",
                 "USE_DSHOT",
                 "FLASH_SIZE 480",
                 "USE_I2C_IO_EXPANDER",
                 "USE_PCF8574",
                 "USE_ESC_SENSOR",
-                "USE_PROGRAMMING_FRAMEWORK",
-                "USE_SERIALRX_GHST",
-                "USE_TELEMETRY_GHST",
-                "USE_CMS",
-                "USE_DJI_HD_OSD",
-                "USE_GYRO_KALMAN",
-                "USE_RANGEFINDER",
-                "USE_RATE_DYNAMICS",
-                "USE_SMITH_PREDICTOR",
-                "USE_ALPHA_BETA_GAMMA_FILTER",
-                "USE_MAG_VCM5883",
-                "USE_TELEMETRY_JETIEXBUS",
-                "USE_NAV",
-                "USE_SDCARD_SDIO",
-                "USE_SDCARD",
-                "USE_Q_TUNE",
-                "USE_GYRO_FFT_FILTER"
+                "USE_ADAPTIVE_FILTER",
+                "MCU_FLASH_SIZE 1024",
             ],
             "configurationProvider": "ms-vscode.cmake-tools"
         }

cmake/at32.cmake

@@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -26,6 +27,9 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_std_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

cmake/stm32.cmake

@@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC
     BasicMathFunctions/arm_scale_f32.c
     BasicMathFunctions/arm_sub_f32.c
     BasicMathFunctions/arm_mult_f32.c
+    BasicMathFunctions/arm_offset_f32.c
     TransformFunctions/arm_rfft_fast_f32.c
     TransformFunctions/arm_cfft_f32.c
     TransformFunctions/arm_rfft_fast_init_f32.c
@@ -27,6 +28,9 @@ set(CMSIS_DSP_SRC
     CommonTables/arm_common_tables.c
     ComplexMathFunctions/arm_cmplx_mag_f32.c
     StatisticsFunctions/arm_max_f32.c
+    StatisticsFunctions/arm_rms_f32.c
+    StatisticsFunctions/arm_std_f32.c
+    StatisticsFunctions/arm_mean_f32.c
 )
 list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/")
 

docs/Settings.md

@@ -1582,6 +1582,76 @@ For developer ground test use. Disables motors, sets heading status = Trusted on
 
 ---
 
+### gyro_adaptive_filter_hpf_hz
+
+High pass filter cutoff frequency
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 1 | 50 |
+
+---
+
+### gyro_adaptive_filter_integrator_threshold_high
+
+High threshold for adaptive filter integrator
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 4 | 1 | 10 |
+
+---
+
+### gyro_adaptive_filter_integrator_threshold_low
+
+Low threshold for adaptive filter integrator
+
+| Default | Min | Max |
+| --- | --- | --- |
+| -2 | -10 | 0 |
+
+---
+
+### gyro_adaptive_filter_max_hz
+
+Maximum frequency for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 150 | 100 | 500 |
+
+---
+
+### gyro_adaptive_filter_min_hz
+
+Minimum frequency for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 50 | 30 | 250 |
+
+---
+
+### gyro_adaptive_filter_std_lpf_hz
+
+Standard deviation low pass filter cutoff frequency
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2 | 0 | 10 |
+
+---
+
+### gyro_adaptive_filter_target
+
+Target value for adaptive filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3.5 | 1 | 6 |
+
+---
+
 ### gyro_anti_aliasing_lpf_hz
 
 Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz
@@ -1622,33 +1692,33 @@ Minimum frequency of the gyro Dynamic LPF
 
 ---
 
-### gyro_main_lpf_hz
+### gyro_filter_mode
 
-Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
+Specifies the type of the software LPF of the gyro signals.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 60 | 0 | 500 |
+| STATIC |  |  |
 
 ---
 
-### gyro_to_use
+### gyro_main_lpf_hz
 
-On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro
+Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 0 | 0 | 2 |
+| 60 | 0 | 500 |
 
 ---
 
-### gyro_use_dyn_lpf
+### gyro_to_use
 
-Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position.
+On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro
 
 | Default | Min | Max |
 | --- | --- | --- |
-| OFF | OFF | ON |
+| 0 | 0 | 2 |
 
 ---
 

src/main/CMakeLists.txt

@@ -346,6 +346,8 @@ main_sources(COMMON_SRC
     flight/dynamic_lpf.h
     flight/ez_tune.c
     flight/ez_tune.h
+    flight/adaptive_filter.c
+    flight/adaptive_filter.h
 
     io/adsb.c
     io/beeper.c

src/main/build/debug.h

@@ -72,5 +72,6 @@ typedef enum {
     DEBUG_RATE_DYNAMICS,
     DEBUG_LANDING,
     DEBUG_POS_EST,
+    DEBUG_ADAPTIVE_FILTER,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/config.c

@@ -190,6 +190,18 @@ uint32_t getGyroLooptime(void)
 
 void validateAndFixConfig(void)
 {
+
+#ifdef USE_ADAPTIVE_FILTER
+    // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz
+    if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) {
+        gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5;
+    }
+    //gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz
+    if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) {
+        gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5;
+    }
+#endif
+
     if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
         accelerometerConfigMutable()->acc_notch_hz = 0;
     }

src/main/fc/fc_tasks.c

@@ -51,6 +51,7 @@
 #include "flight/rpm_filter.h"
 #include "flight/servos.h"
 #include "flight/wind_estimator.h"
+#include "flight/adaptive_filter.h"
 
 #include "navigation/navigation.h"
 
@@ -426,6 +427,10 @@ void fcTasksInit(void)
     setTaskEnabled(TASK_SMARTPORT_MASTER, true);
 #endif
 
+#ifdef USE_ADAPTIVE_FILTER
+    setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE));
+#endif
+
 #if defined(SITL_BUILD)
     serialProxyStart();
 #endif
@@ -680,4 +685,12 @@ cfTask_t cfTasks[TASK_COUNT] = {
         .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ),          // 100Hz @10ms
         .staticPriority = TASK_PRIORITY_HIGH,
     },
+#ifdef USE_ADAPTIVE_FILTER
+    [TASK_ADAPTIVE_FILTER] = {
+        .taskName = "ADAPTIVE_FILTER",
+        .taskFunc = adaptiveFilterTask,
+        .desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ),          // 100Hz @10ms
+        .staticPriority = TASK_PRIORITY_LOW,
+    },
+#endif
 };

src/main/fc/settings.yaml

@@ -83,7 +83,7 @@ tables:
     values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
-      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
@@ -191,6 +191,9 @@ tables:
   - name: led_pin_pwm_mode
     values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
     enum: led_pin_pwm_mode_e
+  - name: gyro_filter_mode
+    values: ["STATIC", "DYNAMIC", "ADAPTIVE"]
+    enum: gyroFilterType_e
 
 constants:
   RPYL_PID_MIN: 0
@@ -226,11 +229,11 @@ groups:
         field: gyro_main_lpf_hz
         min: 0
         max: 500
-      - name: gyro_use_dyn_lpf
-        description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position."
-        default_value: OFF
-        field: useDynamicLpf
-        type: bool
+      - name: gyro_filter_mode
+        description: "Specifies the type of the software LPF of the gyro signals."
+        default_value: "STATIC"
+        field: gyroFilterMode
+        table: gyro_filter_mode
       - name: gyro_dyn_lpf_min_hz
         description: "Minimum frequency of the gyro Dynamic LPF"
         default_value: 200
@@ -330,6 +333,55 @@ groups:
         field: gravity_cmss_cal
         min: 0
         max: 2000
+      - name: gyro_adaptive_filter_target
+        description: "Target value for adaptive filter"
+        default_value: 3.5
+        field: adaptiveFilterTarget
+        min: 1
+        max: 6
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_min_hz
+        description: "Minimum frequency for adaptive filter"
+        default_value: 50
+        field: adaptiveFilterMinHz
+        min: 30
+        max: 250
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_max_hz
+        description: "Maximum frequency for adaptive filter"
+        default_value: 150
+        field: adaptiveFilterMaxHz
+        min: 100
+        max: 500
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_std_lpf_hz
+        description: "Standard deviation low pass filter cutoff frequency"
+        default_value: 2
+        field: adaptiveFilterStdLpfHz
+        min: 0
+        max: 10
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_hpf_hz
+        description: "High pass filter cutoff frequency"
+        default_value: 10
+        field: adaptiveFilterHpfHz
+        min: 1
+        max: 50
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_integrator_threshold_high
+        description: "High threshold for adaptive filter integrator"
+        default_value: 4
+        field: adaptiveFilterIntegratorThresholdHigh
+        min: 1
+        max: 10
+        condition: USE_ADAPTIVE_FILTER
+      - name: gyro_adaptive_filter_integrator_threshold_low
+        description: "Low threshold for adaptive filter integrator"
+        default_value: -2
+        field: adaptiveFilterIntegratorThresholdLow
+        min: -10
+        max: 0
+        condition: USE_ADAPTIVE_FILTER
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t