From fd9a261c8e50345bb60179379362d023033b0663 Mon Sep 17 00:00:00 2001 From: Peter Peiser Date: Mon, 14 Oct 2024 21:03:42 +0200 Subject: [PATCH] Updated lulu filter to improve high throttle handling --- src/main/common/lulu.c | 260 +++++++++++++++++------------------------ 1 file changed, 105 insertions(+), 155 deletions(-) diff --git a/src/main/common/lulu.c b/src/main/common/lulu.c index 8ab7803ed1b..d3b26b5f0b0 100644 --- a/src/main/common/lulu.c +++ b/src/main/common/lulu.c @@ -16,164 +16,114 @@ #endif /* __ARM_ACLE */ #include -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)); +void luluFilterInit(luluFilter_t *filter, int N) { + if (N > 15) { + N = 15; + } + if (N < 1) { + N = 1; + } + filter->N = N; + 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) -{ - float curVal = 0; - float curValB = 0; - for (int N = 1; N <= filterN; N++) - { - int indexNeg = (index + windowSize - 2 * N) % windowSize; - int curIndex = (indexNeg + 1) % windowSize; - float prevVal = series[indexNeg]; - float prevValB = seriesB[indexNeg]; - 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]; - float nextVal = series[indexPos]; - 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; - 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; - 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]; - float nextVal = series[indexPos]; - float nextValB = seriesB[indexPos]; - - if (prevVal > curVal && curVal < nextVal) - { - float minValue = MIN(prevVal, nextVal); - - curVal = minValue; - series[curIndex] = minValue; - 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; - 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 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; + } + + curVal = series[curIndex]; + curValB = seriesB[curIndex]; + register float nextVal = series[indexPos]; + register float nextValB = seriesB[indexPos]; + + if (prevVal < curVal && curVal > nextVal) { + float maxValue = MAX(prevVal, nextVal); + series[curIndex] = maxValue; + } + + if (prevValB < curValB && curValB > nextValB) { + float maxValue = MAX(prevValB, nextValB); + seriesB[curIndex] = 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; + } + + 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); + series[curIndex] = minValue; + } + + if (prevValB > curValB && curValB < nextValB) { + float minValue = MIN(prevValB, nextValB); + seriesB[curIndex] = minValue; + } + prevVal = curVal; + prevValB = curValB; + curIndex++; + indexPos++; + } + } + int finalIndex = (index + windowSize - filterN) % windowSize; + curVal = series[finalIndex]; + curValB = seriesB[finalIndex]; + return (curVal - curValB) / 2; } -FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input) -{ - // This is the value N of the LULU filter. - int filterN = filter->N; - // This is the total window size for the rolling buffer - int filterWindow = filter->windowSize; - - int windowIndex = filter->windowBufIndex; - float inputVal = input; - 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 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; -} +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; +} \ No newline at end of file