From 6f40120a24ec9d7192c40d59f64a91cefef166d0 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 29 Jun 2024 21:16:26 +0200
Subject: [PATCH] Make gyro LULU independent from the gyro LPF

---
 src/main/build/debug.h    |  1 +
 src/main/fc/settings.yaml |  8 +++++++-
 src/main/sensors/gyro.c   | 33 ++++++++++++++++++++++++---------
 src/main/sensors/gyro.h   |  1 +
 4 files changed, 33 insertions(+), 10 deletions(-)

diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index fea424b900..ef32bf8873 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -76,6 +76,7 @@ typedef enum {
     DEBUG_POS_EST,
     DEBUG_ADAPTIVE_FILTER,
     DEBUG_HEADTRACKING,
+    DEBUG_LULU,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index ff42fb18fa..1631d44b43 100644
--- a/src/main/fc/settings.yaml
+++ b/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", "ADAPTIVE_FILTER", "HEADTRACKER" ]
+      "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER", "HEADTRACKER", "LULU" ]
   - name: aux_operator
     values: ["OR", "AND"]
     enum: modeActivationOperator_e
@@ -226,10 +226,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)"
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 780bada378..9fa0a7c0e4 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -135,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)
@@ -248,9 +249,9 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t
 {
     *applyFn = nullFilterApply;
     if (cutoff > 0) {
+        *applyFn = (filterApplyFnPtr)pt1FilterApply;
         for (int axis = 0; axis < 3; axis++) {
             pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
-            *applyFn = (filterApplyFnPtr)pt1FilterApply;
         }
     }
 }
@@ -260,14 +261,17 @@ static void gyroInitFilters(void)
     //First gyro LPF running at full gyro frequency 8kHz
     initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
 
-    /*
-    luluFilterInit(&state[axis].lulu, cutoff);
-    *applyFn = (filterApplyFnPtr)luluFilterApply;
-    
-    initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyroLuluSampleCount, getLooptime(), FILTER_LULU);
-    */
+    if (gyroConfig()->gyroLuluEnabled && gyroConfig()->gyroLuluSampleCount > 0) {
+        gyroLuluApplyFn = (filterApplyFnPtr)luluFilterApply;
 
-    if(gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_OFF) {
+        for (int axis = 0; axis < 3; axis++) {
+            luluFilterInit(&gyroLuluState[axis].lulu, gyroConfig()->gyroLuluSampleCount);
+        }
+    } else {
+        gyroLuluApplyFn = nullFilterApply;
+    }
+
+    if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_OFF) {
         initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
     } else {
         gyroLpf2ApplyFn = nullFilterApply;
@@ -467,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
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index c6f60b45bb..de78b2f81c 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -105,6 +105,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroFilterMode;
 
     uint8_t gyroLuluSampleCount;
+    bool gyroLuluEnabled;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);