Merge pull request #10177 from iNavFlight/dzikuvx-lulu-improvements

Gyro LULU smoother as gyro filter alternative

docs/Settings.md

@@ -1782,6 +1782,16 @@ Specifies the type of the software LPF of the gyro signals.
 
 ---
 
+### gyro_lulu_sample_count
+
+Gyro lulu sample count, in number of samples.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 3 |  | 15 |
+
+---
+
 ### gyro_main_lpf_hz
 
 Software based gyro main lowpass filter. Value is cutoff frequency (Hz)
@@ -2318,7 +2328,7 @@ This is the main loop time (in us). Changing this affects PID effect with some P
 
 | Default | Min | Max |
 | --- | --- | --- |
-| 1000 |  | 9000 |
+| 500 |  | 9000 |
 
 ---
 

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
   - name: headtracker_dev_type
     values: ["NONE", "SERIAL", "MSP"]
@@ -219,13 +219,18 @@ 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)"
         default_value: 60