diff --git a/src/main/common/lulu.c b/src/main/common/lulu.c index 828a6cca77..6c3d9ae5fe 100644 --- a/src/main/common/lulu.c +++ b/src/main/common/lulu.c @@ -18,169 +18,132 @@ 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)); + 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) { - 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; - } - // 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) - { - float maxValue = MAX(prevVal, nextVal); - - series[curIndex] = maxValue; -/* register 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; -/* register 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]; - register float nextVal = series[indexPos]; - register float nextValB = seriesB[indexPos]; - - 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++) - { - if (++k >= windowSize) - { - k = 0; - } - series[k] = minValue; - }*/ - } - - 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++) - { - if (++k >= windowSize) - { - k = 0; - } - seriesB[k] = minValue; - }*/ - } - prevVal = curVal; - prevValB = curValB; - curIndex++; - indexPos++; - } - } - return (curVal - curValB) / 2; + 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); + + curVal = maxValue; + 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); + + curVal = minValue; + series[curIndex] = minValue; + } + + if (prevValB > curValB && curValB < nextValB) + { + float minValue = MIN(prevValB, nextValB); + curValB = minValue; + seriesB[curIndex] = minValue; + } + prevVal = curVal; + prevValB = curValB; + curIndex++; + indexPos++; + } + } + return (curVal - curValB) / 2; } 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); + // 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; + // 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; }