LULU filter port to iNav as a single replacement for most filters in iNav, simplifying filter configuration drastically

src/main/CMakeLists.txt

@@ -31,6 +31,8 @@ main_sources(COMMON_SRC
     common/gps_conversion.h
     common/log.c
     common/log.h
+    common/lulu.c
+    common/lulu.h
     common/maths.c
     common/maths.h
     common/memory.c

src/main/common/filter.c

@@ -23,10 +23,10 @@
 #include "platform.h"
 
 #include "common/filter.h"
+#include "common/lulu.h"
 #include "common/maths.h"
 #include "common/utils.h"
 #include "common/time.h"
-
 // NULL filter
 float nullFilterApply(void *filter, float input)
 {
@@ -326,6 +326,8 @@ void initFilter(const uint8_t filterType, filter_t *filter, const float cutoffFr
             pt2FilterInit(&filter->pt2, pt2FilterGain(cutoffFrequency, dT));
         } if (filterType == FILTER_PT3) {
             pt3FilterInit(&filter->pt3, pt3FilterGain(cutoffFrequency, dT));
+        } if (filterType == FILTER_LULU) {
+            luluFilterInit(&filter->lulu, cutoffFrequency);
         } else {
             biquadFilterInitLPF(&filter->biquad, cutoffFrequency, refreshRate);
         }
@@ -341,8 +343,10 @@ void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyF
             *applyFn = (filterApplyFnPtr) pt2FilterApply;
         } if (filterType == FILTER_PT3) {
             *applyFn = (filterApplyFnPtr) pt3FilterApply;
+        } if (filterType == FILTER_LULU) {
+            *applyFn = (filterApplyFnPtr) luluFilterApply;
         } else {
             *applyFn = (filterApplyFnPtr) biquadFilterApply;
         }
     }
-}
+}

src/main/common/filter.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "lulu.h"
+
 typedef struct rateLimitFilter_s {
     float state;
 } rateLimitFilter_t;
@@ -50,13 +52,15 @@ typedef union {
     pt1Filter_t pt1;
     pt2Filter_t pt2;
     pt3Filter_t pt3;
+    luluFilter_t lulu;
 } filter_t;
 
 typedef enum {
     FILTER_PT1 = 0,
     FILTER_BIQUAD,
     FILTER_PT2,
-    FILTER_PT3
+    FILTER_PT3,
+    FILTER_LULU
 } filterType_e;
 
 typedef enum {
@@ -134,4 +138,4 @@ void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float
 float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);
 
 void initFilter(uint8_t filterType, filter_t *filter, float cutoffFrequency, uint32_t refreshRate);
-void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);
+void assignFilterApplyFn(uint8_t filterType, float cutoffFrequency, filterApplyFnPtr *applyFn);

src/main/common/lulu.c

@@ -0,0 +1,178 @@
+#include "lulu.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#ifdef __ARM_ACLE
+#include <arm_acle.h>
+#endif /* __ARM_ACLE */
+#include <fenv.h>
+
+void luluFilterInit(luluFilter_t *filter, int N)
+{
+	filter->N = constrain(N, 1, 15);
+	filter->windowSize = filter->N * 2 + 1;
+	filter->windowBufIndex = 0;
+
+	memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
+	memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
+}
+
+FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
+{
+	register float curVal = 0;
+	register float curValB = 0;
+	for (int N = 1; N <= filterN; N++)
+	{
+		int indexNeg = (index + windowSize - 2 * N) % windowSize;
+		register int curIndex = (indexNeg + 1) % windowSize;
+		register float prevVal = series[indexNeg];
+		register float prevValB = seriesB[indexNeg];
+		register int indexPos = (curIndex + N) % windowSize;
+		for (int i = windowSize - 2 * N; i < windowSize - N; i++)
+		{
+			if (indexPos >= windowSize)
+			{
+				indexPos = 0;
+			}
+			if (curIndex >= windowSize)
+			{
+				curIndex = 0;
+			}
+			// curIndex = (2 - 1) % 3 = 1
+			curVal = series[curIndex];
+			curValB = seriesB[curIndex];
+			register float nextVal = series[indexPos];
+			register float nextValB = seriesB[indexPos];
+			// onbump (s, 1, 1, 3)
+			// if(onBump(series, curIndex, N, windowSize))
+			if (prevVal < curVal && curVal > nextVal)
+			{
+				float maxValue = MAX(prevVal, nextVal);
+
+				series[curIndex] = maxValue;
+				register int k = curIndex;
+				for (int j = 1; j < N; j++)
+				{
+					if (++k >= windowSize)
+					{
+						k = 0;
+					}
+					series[k] = maxValue;
+				}
+			}
+
+			if (prevValB < curValB && curValB > nextValB)
+			{
+				float maxValue = MAX(prevValB, nextValB);
+
+				curVal = maxValue;
+				seriesB[curIndex] = maxValue;
+				register int k = curIndex;
+				for (int j = 1; j < N; j++)
+				{
+					if (++k >= windowSize)
+					{
+						k = 0;
+					}
+					seriesB[k] = maxValue;
+				}
+			}
+			prevVal = curVal;
+			prevValB = curValB;
+			curIndex++;
+			indexPos++;
+		}
+
+		curIndex = (indexNeg + 1) % windowSize;
+		prevVal = series[indexNeg];
+		prevValB = seriesB[indexNeg];
+		indexPos = (curIndex + N) % windowSize;
+		for (int i = windowSize - 2 * N; i < windowSize - N; i++)
+		{
+			if (indexPos >= windowSize)
+			{
+				indexPos = 0;
+			}
+			if (curIndex >= windowSize)
+			{
+				curIndex = 0;
+			}
+			// curIndex = (2 - 1) % 3 = 1
+			curVal = series[curIndex];
+			curValB = seriesB[curIndex];
+			register float nextVal = series[indexPos];
+			register float nextValB = seriesB[indexPos];
+
+			if (prevVal > curVal && curVal < nextVal)
+			{
+				float minValue = MIN(prevVal, nextVal);
+
+				curVal = minValue;
+				series[curIndex] = minValue;
+				register int k = curIndex;
+				for (int j = 1; j < N; j++)
+				{
+					if (++k >= windowSize)
+					{
+						k = 0;
+					}
+					series[k] = minValue;
+				}
+			}
+
+			if (prevValB > curValB && curValB < nextValB)
+			{
+				float minValue = MIN(prevValB, nextValB);
+				curValB = minValue;
+				seriesB[curIndex] = minValue;
+				register int k = curIndex;
+				for (int j = 1; j < N; j++)
+				{
+					if (++k >= windowSize)
+					{
+						k = 0;
+					}
+					seriesB[k] = minValue;
+				}
+			}
+			prevVal = curVal;
+			prevValB = curValB; 
+			curIndex++;
+			indexPos++;
+		}
+	}
+	return (curVal - curValB) / 2;
+}
+
+FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
+{
+	// This is the value N of the LULU filter.
+	register int filterN = filter->N;
+	// This is the total window size for the rolling buffer
+	register int filterWindow = filter->windowSize;
+
+	register int windowIndex = filter->windowBufIndex;
+	register float inputVal = input;
+	register int newIndex = (windowIndex + 1) % filterWindow;
+	filter->windowBufIndex = newIndex;
+	filter->luluInterim[windowIndex] = inputVal;
+	filter->luluInterimB[windowIndex] = -inputVal;
+	return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
+}
+
+FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
+{
+	// This is the UL filter
+	float resultA = luluFilterPartialApply(filter, input);
+	// We use the median interpretation of this filter to remove bias in the output
+	return resultA;
+}

src/main/common/lulu.h

@@ -0,0 +1,14 @@
+#pragma once
+
+// Max N = 15
+typedef struct
+{
+    int windowSize;
+    int windowBufIndex;
+    int N;
+    float luluInterim[32] __attribute__((aligned(128)));
+    float luluInterimB[32];
+} luluFilter_t;
+
+void luluFilterInit(luluFilter_t *filter, int N);
+float luluFilterApply(luluFilter_t *filter, float input);

src/main/fc/config.c

@@ -192,8 +192,8 @@ 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) {
+//     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

src/main/fc/settings.yaml

@@ -119,7 +119,7 @@ tables:
   - name: filter_type
     values: ["PT1", "BIQUAD"]
   - name: filter_type_full
-    values: ["PT1", "BIQUAD", "PT2", "PT3"]
+    values: ["PT1", "BIQUAD", "PT2", "PT3", "LULU"]
   - name: log_level
     values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
   - name: iterm_relax
@@ -192,7 +192,7 @@ tables:
     values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"]
     enum: led_pin_pwm_mode_e
   - name: gyro_filter_mode
-    values: ["STATIC", "DYNAMIC", "ADAPTIVE"]
+    values: ["STATIC", "DYNAMIC", "ADAPTIVE", "LULU"]
     enum: gyroFilterType_e
 
 constants:
@@ -216,22 +216,27 @@ groups:
     members:
       - name: looptime
         description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible."
-        default_value: 1000
+        default_value: 500
         max: 9000
       - name: gyro_anti_aliasing_lpf_hz
         description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz"
         default_value: 250
         field: gyro_anti_aliasing_lpf_hz
         max: 1000
+      - name: gyro_lulu_sample_count
+        description: "Gyro lulu sample count, in number of samples."
+        default_value: 3
+        field: gyroLuluSampleCount
+        max: 15
       - name: gyro_main_lpf_hz
-        description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)"
+        description: "Software based gyro main lowpass filter. Value is Hz"
         default_value: 60
         field: gyro_main_lpf_hz
         min: 0
         max: 500
       - name: gyro_filter_mode
         description: "Specifies the type of the software LPF of the gyro signals."
-        default_value: "STATIC"
+        default_value: "LULU"
         field: gyroFilterMode
         table: gyro_filter_mode
       - name: gyro_dyn_lpf_min_hz
@@ -254,7 +259,7 @@ groups:
         max: 10
       - name: dynamic_gyro_notch_enabled
         description: "Enable/disable dynamic gyro notch also known as Matrix Filter"
-        default_value: ON
+        default_value: OFF
         field: dynamicGyroNotchEnabled
         condition: USE_DYNAMIC_FILTERS
         type: bool
@@ -293,7 +298,7 @@ groups:
         default_value: 0
       - name: setpoint_kalman_enabled
         description: "Enable Kalman filter on the gyro data"
-        default_value: ON
+        default_value: OFF
         condition: USE_GYRO_KALMAN
         field: kalmanEnabled
         type: bool

src/main/sensors/gyro.c

@@ -240,24 +240,33 @@ 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)
+static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime, filterType_e filterType)
 {
     *applyFn = nullFilterApply;
     if (cutoff > 0) {
-        *applyFn = (filterApplyFnPtr)pt1FilterApply;
         for (int axis = 0; axis < 3; axis++) {
-            pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
+            if(filterType == FILTER_LULU) {
+                luluFilterInit(&state[axis].lulu, cutoff);
+                *applyFn = (filterApplyFnPtr)luluFilterApply;
+            } else {
+                pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
+                *applyFn = (filterApplyFnPtr)pt1FilterApply;
+            }
         }
     }
 }
 
 static void gyroInitFilters(void)
 {
     //First gyro LPF running at full gyro frequency 8kHz
-    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
+    initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime(), FILTER_PT1);
 
-    //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());
+    if(gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_LULU) {
+        initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyroLuluSampleCount, getLooptime(), FILTER_LULU);
+    } 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);
+    }
 
 #ifdef USE_ADAPTIVE_FILTER
     if (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE) {