rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1306 from martinbudden/bf_filters_efficiency 

Improved efficiency of FIR filters by avoiding memmov and caching moving sum
This commit is contained in:
J Blackman 2016-10-13 04:27:30 +11:00 committed by GitHub
parent 855e0a9f90 9e3102d355
commit 145481afb3
2 changed files with 51 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/main/common/filter.c
View File

@ -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,72 @@ 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->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 <count> 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;
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
ret += filter->coeffs[ii] * filter->buf[ii];
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;
}
/*
* Returns average of the last <count> 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

7
src/main/common/filter.h
View File

@ -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;
@ -75,11 +78,11 @@ 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 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);