Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Added notch filter from betaflight (#592)

Browse source
This commit is contained in:
Martin Budden 2016-09-14 17:20:55 +01:00 • committed by Konstantin Sharlaimov
parent c806a7e568
commit daf9ba7dde
6 changed files with 94 additions and 57 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -20,59 +20,18 @@
#include <string.h>
#include <math.h>
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "drivers/gyro_sync.h"
#define BIQUAD_Q (1.0f / 1.41421356f) /* quality factor - butterworth (1 / sqrt(2)) */
#define M_PI_FLOAT 3.14159265358979323846f
void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate)
{
// setup variables
const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate;
const float sn = sin_approx(omega);
const float cs = cos_approx(omega);
const float alpha = sn / (2 * BIQUAD_Q);
float a0, a1, a2, b0, b1, b2;
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
a0 = 1 + alpha;
a1 = -2 * cs;
a2 = 1 - alpha;
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
// zero initial samples
filter->d1 = filter->d2 = 1;
}
/* Computes a biquad_t filter on a sample */
float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
// PT1 Low Pass filter
// f_cut = cutoff frequency
void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
filter->dT = dT;
}
@ -82,14 +41,15 @@ float pt1FilterApply(pt1Filter_t *filter, float input)
return filter->state;
}
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut );
filter->dT = dT;
}
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
return filter->state;
}
@ -117,7 +77,67 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l
return filter->state;
}
// FIR filter
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff)
{
const float octaves = log2f((float)centerFreq / (float)cutoff) * 2;
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
}
// sets up a biquad Filter
void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate)
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// setup variables
const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f);
const float omega = 2.0f * M_PIf * ((float)filterFreq) / sampleRate;
const float sn = sin_approx(omega);
const float cs = cos_approx(omega);
const float alpha = sn / (2 * Q);
float b0, b1, b2;
switch (filterType) {
case FILTER_LPF:
b0 = (1 - cs) / 2;
b1 = 1 - cs;
b2 = (1 - cs) / 2;
break;
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
b2 = 1;
break;
}
const float a0 = 1 + alpha;
const float a1 = -2 * cs;
const float a2 = 1 - alpha;
// precompute the coefficients
filter->b0 = b0 / a0;
filter->b1 = b1 / a0;
filter->b2 = b2 / a0;
filter->a1 = a1 / a0;
filter->a2 = a2 / a0;
// zero initial samples
filter->d1 = filter->d2 = 0;
}
// Computes a biquad_t filter on a sample
float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
filter->d2 = filter->b2 * input - filter->a2 * result;
return result;
}
/*
* FIR filter
*/
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
{
filter->buf = buf;
@ -127,6 +147,10 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
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
*/
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
{
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
@ -138,7 +162,7 @@ void firFilterUpdate(firFilter_t *filter, float input)
filter->buf[0] = input;
}
float firFilterApply(firFilter_t *filter)
float firFilterApply(const firFilter_t *filter)
{
float ret = 0.0f;
for (int ii = 0; ii < filter->coeffsLength; ++ii) {

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

@ -33,6 +33,17 @@ typedef struct biquadFilter_s {
float d1, d2;
} biquadFilter_t;
typedef enum {
FILTER_PT1 = 0,
FILTER_BIQUAD,
FILTER_FIR,
} filterType_e;
typedef enum {
FILTER_LPF,
FILTER_NOTCH
} biquadFilterType_e;
typedef struct firFilter_s {
float *buf;
const float *coeffs;
@ -41,17 +52,19 @@ typedef struct firFilter_s {
} firFilter_t;
float pt1FilterApply(pt1Filter_t *filter, float input);
float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt);
void pt1FilterReset(pt1Filter_t *filter, float input);
void rateLimitFilterInit(rateLimitFilter_t *filter);
float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT);
void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate);
void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate);
void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
float biquadFilterApply(biquadFilter_t *filter, float sample);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
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);
float firFilterApply(const firFilter_t *filter);

2
src/main/fc/mw.c
View file

@ -499,7 +499,7 @@ void filterRc(bool isRXDataNew)
// Calculate average cycle time (1Hz LPF on cycle time)
if (!filterInitialised) {
biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime);
biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime);
filterInitialised = true;
}

2
src/main/flight/mixer.c
View file

@ -921,7 +921,7 @@ void filterServos(void)
// Initialize servo lowpass filter (servos are calculated at looptime rate)
if (!servoFilterIsSet) {
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
biquadFilterInitLPF(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime);
}
servoFilterIsSet = true;

4
src/main/sensors/acceleration.c
View file

@ -58,7 +58,7 @@ void accInit(uint32_t targetLooptime)
accTargetLooptime = targetLooptime;
if (accLpfCutHz) {
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}
@ -223,7 +223,7 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz)
accLpfCutHz = initialAccLpfCutHz;
if (accTargetLooptime) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime);
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
}
}

2
src/main/sensors/gyro.c
View file

@ -59,7 +59,7 @@ void gyroInit(void)
{
if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}