Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Added FIR filter

Browse source
This commit is contained in:
Martin Budden 2016-08-12 06:57:38 +01:00
parent 74212518a6
commit ed5452d97b
2 changed files with 43 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

30
src/main/common/filter.c
View file

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "common/filter.h"
@ -137,3 +138,32 @@ float filterApplyAveragef(float input, uint8_t averageCount, float averageState[
return averageSum / averageCount;
}
// FIR filter
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
{
filter->buf = buf;
filter->bufLength = bufLength;
filter->coeffs = coeffs;
filter->coeffsLength = coeffsLength;
memset(filter->buf, 0, sizeof(float) * filter->bufLength);
}
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
{
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
}
void firFilterUpdate(firFilter_t *filter, float input)
{
memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
filter->buf[0] = input;
}
float firFilterApply(firFilter_t *filter)
{
float ret = 0.0f;
for (int ii = 0; ii < filter->coeffsLength; ++ii) {
ret += filter->coeffs[ii] * filter->buf[ii];
}
return ret;
}

13
src/main/common/filter.h
View file

@ -32,6 +32,7 @@ typedef struct biquadFilter_s {
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_FIR,
} filterType_e;
typedef enum {
@ -39,6 +40,13 @@ typedef enum {
FILTER_NOTCH
} biquadFilterType_e;
typedef struct firFilter_s {
float *buf;
const float *coeffs;
uint8_t bufLength;
uint8_t coeffsLength;
} firFilter_t;
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float input);
@ -51,3 +59,8 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
int32_t filterApplyAverage(int32_t input, uint8_t averageCount, int32_t averageState[DELTA_MAX_SAMPLES]);
float filterApplyAveragef(float input, uint8_t averageCount, float averageState[DELTA_MAX_SAMPLES]);
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);
float firFilterApply(firFilter_t *filter);