mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
Improved efficiency of FIR filters by avoiding memmov
This commit is contained in:
parent
98c5e0298a
commit
6f3abcb27d
2 changed files with 38 additions and 15 deletions
|
@ -125,12 +125,15 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
|
||||||
filter->bufLength = bufLength;
|
filter->bufLength = bufLength;
|
||||||
filter->coeffs = coeffs;
|
filter->coeffs = coeffs;
|
||||||
filter->coeffsLength = coeffsLength;
|
filter->coeffsLength = coeffsLength;
|
||||||
|
filter->movingSum = 0.0f;
|
||||||
|
filter->index = 0;
|
||||||
|
filter->count = 0;
|
||||||
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
|
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
|
||||||
}
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* FIR filter initialisation
|
* 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)
|
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)
|
void firFilterUpdate(firFilter_t *filter, float input)
|
||||||
{
|
{
|
||||||
memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input));
|
filter->movingSum += input; // sum of the last <count> items, to allow quick moving average computation
|
||||||
filter->buf[0] = input;
|
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 firFilterApply(const firFilter_t *filter)
|
||||||
{
|
{
|
||||||
float ret = 0.0f;
|
float ret = 0.0f;
|
||||||
|
int index = filter->index;
|
||||||
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
|
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;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Returns average of the last <count> items.
|
||||||
|
*/
|
||||||
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
|
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
|
||||||
{
|
{
|
||||||
float ret = 0.0f;
|
float ret = 0.0f;
|
||||||
for (int ii = 0; ii < count; ++ii) {
|
int index = filter->index;
|
||||||
ret += filter->buf[ii];
|
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
|
||||||
|
--index;
|
||||||
|
if (index < 0) {
|
||||||
|
index = filter->bufLength - 1;
|
||||||
|
}
|
||||||
|
ret += filter->buf[index];
|
||||||
}
|
}
|
||||||
return ret / count;
|
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)
|
float firFilterLastInput(const firFilter_t *filter)
|
||||||
{
|
{
|
||||||
return filter->buf[0];
|
// filter->index points to next empty item in buffer
|
||||||
}
|
const int index = filter->index == 0 ? filter->bufLength - 1 : filter->index - 1;
|
||||||
|
|
||||||
float firFilterGet(const firFilter_t *filter, int index)
|
|
||||||
{
|
|
||||||
return filter->buf[index];
|
return filter->buf[index];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* int16_t based FIR filter
|
* int16_t based FIR filter
|
||||||
* Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers
|
* Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers
|
||||||
|
|
|
@ -51,6 +51,9 @@ typedef enum {
|
||||||
typedef struct firFilter_s {
|
typedef struct firFilter_s {
|
||||||
float *buf;
|
float *buf;
|
||||||
const float *coeffs;
|
const float *coeffs;
|
||||||
|
float movingSum;
|
||||||
|
uint8_t index;
|
||||||
|
uint8_t count;
|
||||||
uint8_t bufLength;
|
uint8_t bufLength;
|
||||||
uint8_t coeffsLength;
|
uint8_t coeffsLength;
|
||||||
} firFilter_t;
|
} 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);
|
void firFilterUpdate(firFilter_t *filter, float input);
|
||||||
float firFilterApply(const firFilter_t *filter);
|
float firFilterApply(const firFilter_t *filter);
|
||||||
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
|
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 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 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);
|
void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue