Skip to content

Commit

Permalink
Updated lulu filter to improve high throttle handling
Browse files Browse the repository at this point in the history
  • Loading branch information
@pjpei
pjpei committed Oct 14, 2024
1 parent dac34ae commit fd9a261
Showing 1 changed file with 105 additions and 155 deletions.
260 changes: 105 additions & 155 deletions src/main/common/lulu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,164 +16,114 @@
#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));
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;
}

1 comment on commit fd9a261

@pjpei
Copy link
Contributor Author

@pjpei pjpei commented on fd9a261 Oct 14, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Corrected indentation to 4 spaces instead of tabs and to K&R style instead of Allman style
Removed crushing old values
Read the correct value out of the buffer instead of assuming the inequality operations will sort it out

Please sign in to comment.