Skip to content

Commit fd9a261

Browse files
pjpeipjpei
committed
Updated lulu filter to improve high throttle handling
1 parent dac34ae commit fd9a261

File tree

1 file changed

+105
-155
lines changed

1 file changed

+105
-155
lines changed

‎src/main/common/lulu.c

Lines changed: 105 additions & 155 deletions
Original file line numberDiff line numberDiff line change
@@ -16,164 +16,114 @@
1616
#endif /* __ARM_ACLE */
1717
#include <fenv.h>
1818

19-
void luluFilterInit(luluFilter_t *filter, int N)
20-
{
21-
filter->N = constrain(N, 1, 15);
22-
filter->windowSize = filter->N * 2 + 1;
23-
filter->windowBufIndex = 0;
24-
25-
memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
26-
memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
19+
void luluFilterInit(luluFilter_t *filter, int N) {
20+
if (N > 15) {
21+
N = 15;
22+
}
23+
if (N < 1) {
24+
N = 1;
25+
}
26+
filter->N = N;
27+
filter->windowSize = filter->N * 2 + 1;
28+
filter->windowBufIndex = 0;
29+
30+
memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
31+
memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
2732
}
2833

29-
FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
30-
{
31-
float curVal = 0;
32-
float curValB = 0;
33-
for (int N = 1; N <= filterN; N++)
34-
{
35-
int indexNeg = (index + windowSize - 2 * N) % windowSize;
36-
int curIndex = (indexNeg + 1) % windowSize;
37-
float prevVal = series[indexNeg];
38-
float prevValB = seriesB[indexNeg];
39-
int indexPos = (curIndex + N) % windowSize;
40-
41-
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
42-
{
43-
if (indexPos >= windowSize)
44-
{
45-
indexPos = 0;
46-
}
47-
if (curIndex >= windowSize)
48-
{
49-
curIndex = 0;
50-
}
51-
// curIndex = (2 - 1) % 3 = 1
52-
curVal = series[curIndex];
53-
curValB = seriesB[curIndex];
54-
float nextVal = series[indexPos];
55-
float nextValB = seriesB[indexPos];
56-
// onbump (s, 1, 1, 3)
57-
// if(onBump(series, curIndex, N, windowSize))
58-
if (prevVal < curVal && curVal > nextVal)
59-
{
60-
float maxValue = MAX(prevVal, nextVal);
61-
62-
series[curIndex] = maxValue;
63-
int k = curIndex;
64-
for (int j = 1; j < N; j++)
65-
{
66-
if (++k >= windowSize)
67-
{
68-
k = 0;
69-
}
70-
series[k] = maxValue;
71-
}
72-
}
73-
74-
if (prevValB < curValB && curValB > nextValB)
75-
{
76-
float maxValue = MAX(prevValB, nextValB);
77-
78-
curVal = maxValue;
79-
seriesB[curIndex] = maxValue;
80-
int k = curIndex;
81-
for (int j = 1; j < N; j++)
82-
{
83-
if (++k >= windowSize)
84-
{
85-
k = 0;
86-
}
87-
seriesB[k] = maxValue;
88-
}
89-
}
90-
prevVal = curVal;
91-
prevValB = curValB;
92-
curIndex++;
93-
indexPos++;
94-
}
95-
96-
curIndex = (indexNeg + 1) % windowSize;
97-
prevVal = series[indexNeg];
98-
prevValB = seriesB[indexNeg];
99-
indexPos = (curIndex + N) % windowSize;
100-
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
101-
{
102-
if (indexPos >= windowSize)
103-
{
104-
indexPos = 0;
105-
}
106-
if (curIndex >= windowSize)
107-
{
108-
curIndex = 0;
109-
}
110-
// curIndex = (2 - 1) % 3 = 1
111-
curVal = series[curIndex];
112-
curValB = seriesB[curIndex];
113-
float nextVal = series[indexPos];
114-
float nextValB = seriesB[indexPos];
115-
116-
if (prevVal > curVal && curVal < nextVal)
117-
{
118-
float minValue = MIN(prevVal, nextVal);
119-
120-
curVal = minValue;
121-
series[curIndex] = minValue;
122-
int k = curIndex;
123-
for (int j = 1; j < N; j++)
124-
{
125-
if (++k >= windowSize)
126-
{
127-
k = 0;
128-
}
129-
series[k] = minValue;
130-
}
131-
}
132-
133-
if (prevValB > curValB && curValB < nextValB)
134-
{
135-
float minValue = MIN(prevValB, nextValB);
136-
curValB = minValue;
137-
seriesB[curIndex] = minValue;
138-
int k = curIndex;
139-
for (int j = 1; j < N; j++)
140-
{
141-
if (++k >= windowSize)
142-
{
143-
k = 0;
144-
}
145-
seriesB[k] = minValue;
146-
}
147-
}
148-
prevVal = curVal;
149-
prevValB = curValB;
150-
curIndex++;
151-
indexPos++;
152-
}
153-
}
154-
return (curVal - curValB) / 2;
34+
FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize) {
35+
register float curVal = 0;
36+
register float curValB = 0;
37+
for (int N = 1; N <= filterN; N++) {
38+
int indexNeg = (index + windowSize - 2 * N) % windowSize;
39+
register int curIndex = (indexNeg + 1) % windowSize;
40+
register float prevVal = series[indexNeg];
41+
register float prevValB = seriesB[indexNeg];
42+
register int indexPos = (curIndex + N) % windowSize;
43+
for (int i = windowSize - 2 * N; i < windowSize - N; i++) {
44+
if (indexPos >= windowSize) {
45+
indexPos = 0;
46+
}
47+
if (curIndex >= windowSize) {
48+
curIndex = 0;
49+
}
50+
51+
curVal = series[curIndex];
52+
curValB = seriesB[curIndex];
53+
register float nextVal = series[indexPos];
54+
register float nextValB = seriesB[indexPos];
55+
56+
if (prevVal < curVal && curVal > nextVal) {
57+
float maxValue = MAX(prevVal, nextVal);
58+
series[curIndex] = maxValue;
59+
}
60+
61+
if (prevValB < curValB && curValB > nextValB) {
62+
float maxValue = MAX(prevValB, nextValB);
63+
seriesB[curIndex] = maxValue;
64+
}
65+
prevVal = curVal;
66+
prevValB = curValB;
67+
curIndex++;
68+
indexPos++;
69+
}
70+
71+
curIndex = (indexNeg + 1) % windowSize;
72+
prevVal = series[indexNeg];
73+
prevValB = seriesB[indexNeg];
74+
indexPos = (curIndex + N) % windowSize;
75+
for (int i = windowSize - 2 * N; i < windowSize - N; i++) {
76+
if (indexPos >= windowSize) {
77+
indexPos = 0;
78+
}
79+
if (curIndex >= windowSize) {
80+
curIndex = 0;
81+
}
82+
83+
curVal = series[curIndex];
84+
curValB = seriesB[curIndex];
85+
register float nextVal = series[indexPos];
86+
register float nextValB = seriesB[indexPos];
87+
88+
if (prevVal > curVal && curVal < nextVal) {
89+
float minValue = MIN(prevVal, nextVal);
90+
series[curIndex] = minValue;
91+
}
92+
93+
if (prevValB > curValB && curValB < nextValB) {
94+
float minValue = MIN(prevValB, nextValB);
95+
seriesB[curIndex] = minValue;
96+
}
97+
prevVal = curVal;
98+
prevValB = curValB;
99+
curIndex++;
100+
indexPos++;
101+
}
102+
}
103+
int finalIndex = (index + windowSize - filterN) % windowSize;
104+
curVal = series[finalIndex];
105+
curValB = seriesB[finalIndex];
106+
return (curVal - curValB) / 2;
155107
}
156108

157-
FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
158-
{
159-
// This is the value N of the LULU filter.
160-
int filterN = filter->N;
161-
// This is the total window size for the rolling buffer
162-
int filterWindow = filter->windowSize;
163-
164-
int windowIndex = filter->windowBufIndex;
165-
float inputVal = input;
166-
int newIndex = (windowIndex + 1) % filterWindow;
167-
filter->windowBufIndex = newIndex;
168-
filter->luluInterim[windowIndex] = inputVal;
169-
filter->luluInterimB[windowIndex] = -inputVal;
170-
return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
109+
FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input) {
110+
// This is the value N of the LULU filter.
111+
register int filterN = filter->N;
112+
// This is the total window size for the rolling buffer
113+
register int filterWindow = filter->windowSize;
114+
115+
register int windowIndex = filter->windowBufIndex;
116+
register float inputVal = input;
117+
register int newIndex = (windowIndex + 1) % filterWindow;
118+
filter->windowBufIndex = newIndex;
119+
filter->luluInterim[windowIndex] = inputVal;
120+
filter->luluInterimB[windowIndex] = -inputVal;
121+
return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
171122
}
172123

173-
FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
174-
{
175-
// This is the UL filter
176-
float resultA = luluFilterPartialApply(filter, input);
177-
// We use the median interpretation of this filter to remove bias in the output
178-
return resultA;
179-
}
124+
FAST_CODE float luluFilterApply(luluFilter_t *filter, float input) {
125+
// This is the UL filter
126+
float resultA = luluFilterPartialApply(filter, input);
127+
// We use the median interpretation of this filter to remove bias in the output
128+
return resultA;
129+
}

0 commit comments

Comments
 (0)