diff --git a/src/main/common/lulu.c b/src/main/common/lulu.c index 28a7362587..828a6cca77 100644 --- a/src/main/common/lulu.c +++ b/src/main/common/lulu.c @@ -11,9 +11,6 @@ #include "common/maths.h" #include "common/utils.h" -#include "fc/fc_rc.h" - - #ifdef __ARM_ACLE #include #endif /* __ARM_ACLE */ @@ -21,81 +18,81 @@ 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; @@ -103,60 +100,60 @@ FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, i 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++; } @@ -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; @@ -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; -} \ No newline at end of file +}