Merge pull request #10186 from iNavFlight/dzikuvx-lulu-flow-changes

Make LULU smoother independent from the gyro LPF

docs/Settings.md

@@ -1782,13 +1782,23 @@ Specifies the type of the software LPF of the gyro signals.
 
 ---
 
+### gyro_lulu_enabled
+
+Enable/disable gyro LULU filter
+
+| Default | Min | Max |
+| --- | --- | --- |
+| OFF | OFF | ON |
+
+---
+
 ### gyro_lulu_sample_count
 
 Gyro lulu sample count, in number of samples.
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 3 |  | 15 |
+| 3 | 1 | 15 |
 
 ---
 

src/main/build/debug.h

@@ -77,6 +77,7 @@ typedef enum {
     DEBUG_ADAPTIVE_FILTER,
     DEBUG_HEADTRACKING,
     DEBUG_GPS,
+    DEBUG_LULU,
     DEBUG_COUNT // also update debugModeNames in cli.c
 } debugType_e;
 

src/main/fc/settings.yaml

@@ -84,7 +84,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE",
       "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", 
-      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS" ]
+      "ADAPTIVE_FILTER", "HEADTRACKER", "GPS", "LULU"]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
@@ -193,7 +193,7 @@ tables:
     values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
     enum: led_pin_pwm_mode_e
   - name: gyro_filter_mode
-    values: ["STATIC", "DYNAMIC", "ADAPTIVE", "LULU"]
+    values: ["OFF", "STATIC", "DYNAMIC", "ADAPTIVE"]
     enum: gyroFilterType_e
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
@@ -227,10 +227,16 @@ groups:
         default_value: 250
         field: gyro_anti_aliasing_lpf_hz
         max: 1000
+      - name: gyro_lulu_enabled
+        description: "Enable/disable gyro LULU filter"
+        default_value: OFF
+        field: gyroLuluEnabled
+        type: bool
       - name: gyro_lulu_sample_count
         description: "Gyro lulu sample count, in number of samples."
         default_value: 3
         field: gyroLuluSampleCount
+        min: 1
         max: 15
       - name: gyro_main_lpf_hz
         description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"

src/main/sensors/gyro.c

@@ -88,6 +88,9 @@ STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
 STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn;
 STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
 
+STATIC_FASTRAM filterApplyFnPtr gyroLuluApplyFn;
+STATIC_FASTRAM filter_t gyroLuluState[XYZ_AXIS_COUNT];
+
 #ifdef USE_DYNAMIC_FILTERS
 
 EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
@@ -96,7 +99,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
@@ -132,7 +135,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .adaptiveFilterIntegratorThresholdLow  = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT,
 #endif
     .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT,
-    .gyroLuluSampleCount = SETTING_GYRO_LULU_SAMPLE_COUNT_DEFAULT
+    .gyroLuluSampleCount = SETTING_GYRO_LULU_SAMPLE_COUNT_DEFAULT,
+    .gyroLuluEnabled = SETTING_GYRO_LULU_ENABLED_DEFAULT
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -241,32 +245,36 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
     return gyroHardware;
 }
 
-static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime, filterType_e filterType)
+static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime)
 {
     *applyFn = nullFilterApply;
     if (cutoff > 0) {
+        *applyFn = (filterApplyFnPtr)pt1FilterApply;
         for (int axis = 0; axis < 3; axis++) {
-            if(filterType == FILTER_LULU) {
-                luluFilterInit(&state[axis].lulu, cutoff);
-                *applyFn = (filterApplyFnPtr)luluFilterApply;
-            } else {
-                pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
-                *applyFn = (filterApplyFnPtr)pt1FilterApply;
-            }
+            pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
         }
     }
 }
 
 static void gyroInitFilters(void)
 {
     //First gyro LPF running at full gyro frequency 8kHz
-    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime(), FILTER_PT1);
+    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
+
+    if (gyroConfig()->gyroLuluEnabled && gyroConfig()->gyroLuluSampleCount > 0) {
+        gyroLuluApplyFn = (filterApplyFnPtr)luluFilterApply;
+
+        for (int axis = 0; axis < 3; axis++) {
+            luluFilterInit(&gyroLuluState[axis].lulu, gyroConfig()->gyroLuluSampleCount);
+        }
+    } else {
+        gyroLuluApplyFn = nullFilterApply;
+    }
 
-    if(gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_LULU) {
-        initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyroLuluSampleCount, getLooptime(), FILTER_LULU);
+    if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_OFF) {
+        initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
     } else {
-        //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON
-        initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime(), FILTER_PT1);
+        gyroLpf2ApplyFn = nullFilterApply;
     }
 
 #ifdef USE_ADAPTIVE_FILTER
@@ -463,6 +471,17 @@ void FAST_CODE NOINLINE gyroFilter(void)
         gyroADCf = rpmFilterGyroApply(axis, gyroADCf);
 #endif
 
+        // LULU gyro filter
+        DEBUG_SET(DEBUG_LULU, axis, gyroADCf); //Pre LULU debug
+        float preLulu = gyroADCf;
+        gyroADCf = gyroLuluApplyFn((filter_t *) &gyroLuluState[axis], gyroADCf);
+        DEBUG_SET(DEBUG_LULU, axis + 3, gyroADCf); //Post LULU debug
+
+        if (axis == ROLL) {
+            DEBUG_SET(DEBUG_LULU, 6, gyroADCf - preLulu); //LULU delta debug
+        }
+
+        // Gyro Main LPF
         gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
 
 #ifdef USE_ADAPTIVE_FILTER

src/main/sensors/gyro.h

@@ -53,10 +53,10 @@ typedef enum {
 } dynamicGyroNotchMode_e;
 
 typedef enum {
-    GYRO_FILTER_MODE_STATIC = 0,
-    GYRO_FILTER_MODE_DYNAMIC = 1,
-    GYRO_FILTER_MODE_ADAPTIVE = 2,
-    GYRO_FILTER_MODE_LULU = 3
+    GYRO_FILTER_MODE_OFF = 0,
+    GYRO_FILTER_MODE_STATIC = 1,
+    GYRO_FILTER_MODE_DYNAMIC = 2,
+    GYRO_FILTER_MODE_ADAPTIVE = 3
 } gyroFilterMode_e;
 
 typedef struct gyro_s {
@@ -105,6 +105,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroFilterMode;
 
     uint8_t gyroLuluSampleCount;
+    bool gyroLuluEnabled;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);