Skip to content

Commit

Permalink
Updated LULU to latest version
Browse files Browse the repository at this point in the history
  • Loading branch information
@pjpei
pjpei committed Oct 9, 2024
1 parent 57ab476 commit 7fa7b30
Showing 1 changed file with 64 additions and 67 deletions.
131 changes: 64 additions & 67 deletions src/main/common/lulu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,152 +11,149 @@
#include "common/maths.h"
#include "common/utils.h"

#include "fc/fc_rc.h"


#ifdef __ARM_ACLE
#include <arm_acle.h>
#endif /* __ARM_ACLE */
#include <fenv.h>

void luluFilterInit(luluFilter_t *filter, int N)
{
if(N > 15)
if (N > 15)
{
N = 15;
}
if(N < 1)
if (N < 1)
{
N = 1;
}
filter->N = N;
filter->windowSize = filter->N * 2 + 1;
filter->windowBufIndex = 0;
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)
{
{
register float curVal = 0;
register float curValB = 0;
for(int N = 1; N <= filterN; N++)
for (int N = 1; N <= filterN; N++)
{
int indexNeg = (index + windowSize - 2*N) % windowSize;
register int curIndex = (indexNeg + 1);
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++)
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
{
if(indexPos >= windowSize)
if (indexPos >= windowSize)
{
indexPos = 0;
}
if(curIndex >= windowSize)
if (curIndex >= windowSize)
{
curIndex = 0;
}
//curIndex = (2 - 1) % 3 = 1
// 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)
// 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++)
series[curIndex] = maxValue;
/* register int k = curIndex;
for (int j = 1; j < N; j++)
{
if(++k >= windowSize)
{
k = 0;
}
if (++k >= windowSize)
{
k = 0;
}
series[k] = maxValue;
}
}*/
}

if(prevValB < curValB && curValB > nextValB)
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++)
curVal = maxValue;
seriesB[curIndex] = maxValue;
/* register int k = curIndex;
for (int j = 1; j < N; j++)
{
if(++k >= windowSize)
{
k = 0;
}
if (++k >= windowSize)
{
k = 0;
}
seriesB[k] = maxValue;
}
}*/
}
prevVal = curVal;
prevValB = curValB;
curIndex++;
indexPos++;
}

curIndex = (indexNeg + 1);
curIndex = (indexNeg + 1) % windowSize;
prevVal = series[indexNeg];
prevValB = seriesB[indexNeg];
indexPos = (curIndex + N) % windowSize;
for(int i = windowSize - 2*N; i < windowSize - N; i++)
for (int i = windowSize - 2 * N; i < windowSize - N; i++)
{
if(indexPos >= windowSize)
if (indexPos >= windowSize)
{
indexPos = 0;
}
if(curIndex >= windowSize)
if (curIndex >= windowSize)
{
curIndex = 0;
}
//curIndex = (2 - 1) % 3 = 1
// 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)
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++)
curVal = minValue;
series[curIndex] = minValue;
/* register int k = curIndex;
for (int j = 1; j < N; j++)
{
if(++k >= windowSize)
{
k = 0;
}
if (++k >= windowSize)
{
k = 0;
}
series[k] = minValue;
}
}*/
}

if(prevValB > curValB && curValB < nextValB)
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++)
seriesB[curIndex] = minValue;
/* register int k = curIndex;
for (int j = 1; j < N; j++)
{
if(++k >= windowSize)
{
k = 0;
}
if (++k >= windowSize)
{
k = 0;
}
seriesB[k] = minValue;
}
}*/
}
prevVal = curVal;
prevValB = curValB;
prevValB = curValB;
curIndex++;
indexPos++;
}
Expand All @@ -166,13 +163,13 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i

FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
{
//This is the value N of the LULU filter.
// This is the value N of the LULU filter.
register int filterN = filter->N;
//This is the total window size for the rolling buffer
// 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 float inputVal = input;
register int newIndex = (windowIndex + 1) % filterWindow;
filter->windowBufIndex = newIndex;
filter->luluInterim[windowIndex] = inputVal;
Expand All @@ -182,8 +179,8 @@ FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)

FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
{
//This is the UL filter
// This is the UL filter
float resultA = luluFilterPartialApply(filter, input);
//We use the median interpretation of this filter to remove bias in the output
// We use the median interpretation of this filter to remove bias in the output
return resultA;
}
}

0 comments on commit 7fa7b30

Please sign in to comment.