From 6f3abcb27dfbd720661ef9e427c4f979c48a859c Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 6 Oct 2016 20:16:57 +0100 Subject: [PATCH 1/3] Improved efficiency of FIR filters by avoiding memmov --- src/main/common/filter.c | 47 +++++++++++++++++++++++++++++----------- src/main/common/filter.h | 6 +++-- 2 files changed, 38 insertions(+), 15 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 63d914d19..b9606795e 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -125,12 +125,15 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl filter->bufLength = bufLength; filter->coeffs = coeffs; filter->coeffsLength = coeffsLength; + filter->movingSum = 0.0f; + filter->index = 0; + filter->count = 0; memset(filter->buf, 0, sizeof(float) * filter->bufLength); } /* * FIR filter initialisation - * If FIR filter is just used for averaging, coeffs can be set to NULL + * If the FIR filter is just to be used for averaging, then coeffs can be set to NULL */ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) { @@ -139,43 +142,61 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo void firFilterUpdate(firFilter_t *filter, float input) { - memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input)); - filter->buf[0] = input; + filter->movingSum += input; // sum of the last items, to allow quick moving average computation + filter->movingSum -= filter->buf[filter->index]; // subtract the value that "drops off" the end of the moving sum + filter->buf[filter->index++] = input; // index is at the first empty buffer positon + if (filter->index >= filter->bufLength) { + filter->index = 0; + } + if (filter->count < filter->bufLength) { + ++filter->count; + } } float firFilterApply(const firFilter_t *filter) { float ret = 0.0f; + int index = filter->index; for (int ii = 0; ii < filter->coeffsLength; ++ii) { - ret += filter->coeffs[ii] * filter->buf[ii]; + --index; + if (index < 0) { + index = filter->bufLength - 1; + } + ret += filter->coeffs[ii] * filter->buf[index]; } return ret; } +/* + * Returns average of the last items. + */ float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count) { float ret = 0.0f; - for (int ii = 0; ii < count; ++ii) { - ret += filter->buf[ii]; + int index = filter->index; + for (int ii = 0; ii < filter->coeffsLength; ++ii) { + --index; + if (index < 0) { + index = filter->bufLength - 1; + } + ret += filter->buf[index]; } return ret / count; } -float firFilterCalcAverage(const firFilter_t *filter) +float firFilterCalcMovingAverage(const firFilter_t *filter) { - return firFilterCalcPartialAverage(filter, filter->coeffsLength); + return filter->movingSum / filter->count; } float firFilterLastInput(const firFilter_t *filter) { - return filter->buf[0]; -} - -float firFilterGet(const firFilter_t *filter, int index) -{ + // filter->index points to next empty item in buffer + const int index = filter->index == 0 ? filter->bufLength - 1 : filter->index - 1; return filter->buf[index]; } + /* * int16_t based FIR filter * Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 86dc421ac..514418f06 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -51,6 +51,9 @@ typedef enum { typedef struct firFilter_s { float *buf; const float *coeffs; + float movingSum; + uint8_t index; + uint8_t count; uint8_t bufLength; uint8_t coeffsLength; } firFilter_t; @@ -77,9 +80,8 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl void firFilterUpdate(firFilter_t *filter, float input); float firFilterApply(const firFilter_t *filter); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); -float firFilterCalcAverage(const firFilter_t *filter); +float firFilterCalcMovingAverage(const firFilter_t *filter); float firFilterLastInput(const firFilter_t *filter); -float firFilterGet(const firFilter_t *filter, int index); void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs); void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); From 64e1a3a723cc3eed7bd598ca8e39d37a55148860 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 12 Oct 2016 15:16:11 +0100 Subject: [PATCH 2/3] Improved firFilterApply efficiency --- src/main/common/filter.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index b9606795e..dde44ca9a 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -156,12 +156,12 @@ void firFilterUpdate(firFilter_t *filter, float input) float firFilterApply(const firFilter_t *filter) { float ret = 0.0f; - int index = filter->index; - for (int ii = 0; ii < filter->coeffsLength; ++ii) { - --index; - if (index < 0) { - index = filter->bufLength - 1; - } + int ii = 0; + int index; + for (index = filter->index - 1; index >= 0; ++ii, --index) { + ret += filter->coeffs[ii] * filter->buf[index]; + } + for (index = filter->bufLength - 1; ii < filter->coeffsLength; ++ii, --index) { ret += filter->coeffs[ii] * filter->buf[index]; } return ret; From 9e3102d35536d20b6113eaf3e3173181981efc8b Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 12 Oct 2016 17:49:01 +0100 Subject: [PATCH 3/3] Now have firFilterUpdate and firFilterUpdateAverage --- src/main/common/filter.c | 11 +++++++++++ src/main/common/filter.h | 1 + 2 files changed, 12 insertions(+) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index dde44ca9a..2cef6fa84 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -141,6 +141,17 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo } void firFilterUpdate(firFilter_t *filter, float input) +{ + filter->buf[filter->index++] = input; // index is at the first empty buffer positon + if (filter->index >= filter->bufLength) { + filter->index = 0; + } +} + +/* + * Update FIR filter maintaining a moving sum for quick moving average computation + */ +void firFilterUpdateAverage(firFilter_t *filter, float input) { filter->movingSum += input; // sum of the last items, to allow quick moving average computation filter->movingSum -= filter->buf[filter->index]; // subtract the value that "drops off" the end of the moving sum diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 514418f06..74afed490 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -78,6 +78,7 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT) void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); void firFilterUpdate(firFilter_t *filter, float input); +void firFilterUpdateAverage(firFilter_t *filter, float input); float firFilterApply(const firFilter_t *filter); float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); float firFilterCalcMovingAverage(const firFilter_t *filter);