diff --git a/src/main/common/filter.c b/src/main/common/filter.c index ff14d70597..e61f1fcd2c 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -192,3 +192,26 @@ FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input) filter->x2 = filter->b2 * input - filter->a2 * result; return result; } + +void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf) +{ + filter->movingWindowIndex = 0; + filter->windowSize = windowSize; + filter->buf = buf; + filter->primed = false; +} + +FAST_CODE float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input) +{ + filter->movingSum -= filter->buf[filter->movingWindowIndex]; + filter->buf[filter->movingWindowIndex] = input; + filter->movingSum += input; + + if (++filter->movingWindowIndex == filter->windowSize) { + filter->movingWindowIndex = 0; + filter->primed = true; + } + + const uint16_t denom = filter->primed ? filter->windowSize : filter->movingWindowIndex; + return filter->movingSum / denom; +} diff --git a/src/main/common/filter.h b/src/main/common/filter.h index db0cb115f0..c6befb1537 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -19,6 +19,7 @@ */ #pragma once +#include struct filter_s; typedef struct filter_s filter_t; @@ -40,6 +41,14 @@ typedef struct biquadFilter_s { float x1, x2, y1, y2; } biquadFilter_t; +typedef struct laggedMovingAverage_s { + uint16_t movingWindowIndex; + uint16_t windowSize; + float movingSum; + float *buf; + bool primed; +} laggedMovingAverage_t; + typedef enum { FILTER_PT1 = 0, FILTER_BIQUAD, @@ -63,6 +72,9 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); float filterGetNotchQ(float centerFreq, float cutoffFreq); +void laggedMovingAverageInit(laggedMovingAverage_t *filter, uint16_t windowSize, float *buf); +float laggedMovingAverageUpdate(laggedMovingAverage_t *filter, float input); + float pt1FilterGain(uint16_t f_cut, float dT); void pt1FilterInit(pt1Filter_t *filter, float k); float pt1FilterApply(pt1Filter_t *filter, float input);