Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 04:15:44 +03:00
Code Issues Wiki Activity

Higher-order gyro filter (#5257)

Browse source 
* Implement nth order Butterworth

Uses biquad sections

* Purge RC+FIR2

* Add butterworth LPS as gyro filter

Replaces RC+FIR

* Make FKF code conditional

* Add USE_FIR_FILTER_DENOISE

Denoise is almost useless anyway ...
This commit is contained in:
Petr Ledvina 2018-03-14 13:45:20 +01:00 • committed by Michael Keller
parent 35f5e5025f
commit 141d6ec30a
8 changed files with 129 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -29,7 +29,7 @@
#define M_LN2_FLOAT 0.69314718055994530942f
#define M_PI_FLOAT 3.14159265358979323846f
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/
#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/
// NULL filter
@ -94,6 +94,51 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}
// Q coefficients for Butterworth filter of given order. Implicit odd section coefficient of 0.5
// coefficients should be in ascending order
// generated by http://www.earlevel.com/main/2016/09/29/cascading-filters/
static const float butterworthLpfQ[] = {
#if BIQUAD_LPF_ORDER_MAX >= 2
0.70710678, // 2nd
#endif
#if BIQUAD_LPF_ORDER_MAX >= 3
1.0, // 3rd
#endif
#if BIQUAD_LPF_ORDER_MAX >= 4
0.54119610, 1.3065630, // 4th
#endif
#if BIQUAD_LPF_ORDER_MAX >= 5
0.61803399, 1.6180340, // 5th
#endif
#if BIQUAD_LPF_ORDER_MAX >= 6
0.51763809, 0.70710678, 1.9318517 // 6th
#endif
};
#define BUTTERWORTH_QINDEX(o) (((o) - 1) * ((o) - 1) / 4)
// make sure that we have correct number of coefficients
STATIC_ASSERT(BUTTERWORTH_QINDEX(BIQUAD_LPF_ORDER_MAX + 1) == ARRAYLEN(butterworthLpfQ), butterworthLpfQ_mismatch);
// setup cascade of biquad sections for Butterworth LPF filter
// sections is pointer to array of biquad sections, it must be long enough
// return number of sections used
int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate)
{
biquadFilter_t *section = sections;
// single-pole section first
if (order % 2 == 1) {
biquadFilterInit(section, filterFreq, refreshRate, 0.5, FILTER_LPF1);
section++;
}
const float *Qptr = butterworthLpfQ + BUTTERWORTH_QINDEX(order); // first coefficient for given order
// 2 poles per section
for (int i = order; i >= 2; i -= 2) {
biquadFilterInit(section, filterFreq, refreshRate, *Qptr, FILTER_LPF);
Qptr++; section++;
}
return section - sections;
}
void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// setup variables
@ -106,6 +151,8 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
switch (filterType) {
case FILTER_LPF:
// 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q
// described in http://www.ti.com/lit/an/slaa447/slaa447.pdf
b0 = (1 - cs) * 0.5f;
b1 = 1 - cs;
b2 = (1 - cs) * 0.5f;
@ -113,6 +160,18 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
a1 = -2 * cs;
a2 = 1 - alpha;
break;
case FILTER_LPF1: {
// 1st order Butterworth, H(s) = 1 / (1 + s),
// transformed according to http://www.iowahills.com/A4IIRBilinearTransform.html
const float T = 2.0f * sn / (cs + 1.0f); // T = 2 * tan(omega / 2)
b0 = T;
b1 = T;
b2 = 0;
a0 = T + 2.0f;
a1 = T - 2.0f;
a2 = 0;
break;
}
case FILTER_NOTCH:
b0 = 1;
b1 = -2 * cs;
@ -306,18 +365,7 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
}
}
// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k
void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, float dT)
{
float RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
float k = dT / (RC + dT);
filter->b0 = k / 2;
filter->b1 = k / 2;
filter->b2 = 0;
filter->a1 = -(1 - k);
filter->a2 = 0;
}
#if defined(USE_GYRO_FAST_KALMAN)
// Fast two-state Kalman
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p)
{
@ -347,3 +395,5 @@ FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input)
return filter->x;
}
#endif

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

@ -70,9 +70,10 @@ typedef enum {
} filterType_e;
typedef enum {
FILTER_LPF,
FILTER_LPF, // 2nd order Butterworth section
FILTER_NOTCH,
FILTER_BPF,
FILTER_LPF1, // 1st order Butterworth section
} biquadFilterType_e;
typedef struct firFilter_s {
@ -89,15 +90,17 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input);
float nullFilterApply(filter_t *filter, float input);
#define BIQUAD_LPF_ORDER_MAX 6
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);
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate);
float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
float biquadFilterApply(biquadFilter_t *filter, float input);
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, float dT);
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p);
float fastKalmanUpdate(fastKalman_t *filter, float input);
@ -120,5 +123,7 @@ float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
float firFilterCalcMovingAverage(const firFilter_t *filter);
float firFilterLastInput(const firFilter_t *filter);
#if defined(USE_FIR_FILTER_DENOISE)
void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input);
#endif

4
src/main/flight/pid.c
View file

@ -177,7 +177,9 @@ static FAST_RAM void *ptermYawFilter;
typedef union dtermFilterLpf_u {
pt1Filter_t pt1Filter[2];
biquadFilter_t biquadFilter[2];
#if defined(USE_FIR_FILTER_DENOISE)
firFilterDenoise_t denoisingFilter[2];
#endif
} dtermFilterLpf_t;
void pidInitFilters(const pidProfile_t *pidProfile)
@ -239,6 +241,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
break;
#if defined(USE_FIR_FILTER_DENOISE)
case FILTER_FIR:
dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@ -246,6 +249,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
}
break;
#endif
}
}

6
src/main/interface/settings.c
View file

@ -380,8 +380,10 @@ const clivalue_t valueTable[] = {
{ "gyro_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_q) },
{ "gyro_filter_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_r) },
{ "gyro_filter_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) },
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
{ "gyro_stage2_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz_2) },
#endif
#if defined(USE_GYRO_LPF2)
{ "gyro_stage2_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_hz) },
{ "gyro_stage2_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF2_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_order) },
#endif
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
{ "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },

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

@ -101,7 +101,9 @@ bool firstArmingCalibrationWasStarted = false;
typedef union gyroSoftFilter_u {
biquadFilter_t gyroFilterLpfState[XYZ_AXIS_COUNT];
pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT];
#if defined(USE_FIR_FILTER_DENOISE)
firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
#endif
} gyroSoftLpfFilter_t;
typedef struct gyroSensor_s {
@ -124,10 +126,11 @@ typedef struct gyroSensor_s {
// gyro kalman filter
filterApplyFnPtr fastKalmanApplyFn;
fastKalman_t fastKalman[XYZ_AXIS_COUNT];
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
// gyro biquad RC FIR2 filter
filterApplyFnPtr biquadRCFIR2ApplyFn;
biquadFilter_t biquadRCFIR2[XYZ_AXIS_COUNT];
#endif
#if defined(USE_GYRO_LPF2)
// lowpass filter, cascaded biquad sections
int biquadLpf2Sections;
biquadFilter_t biquadLpf2[XYZ_AXIS_COUNT][(GYRO_LPF2_ORDER_MAX + 1) / 2]; // each section is of second order
#endif
} gyroSensor_t;
@ -143,8 +146,9 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r, uint16_t gyro_filter_p);
#elif defined (USE_GYRO_BIQUAD_RC_FIR2)
static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz);
#endif
#if defined (USE_GYRO_LPF2)
static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz);
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
@ -187,11 +191,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_hz_2 = 200,
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_soft_lpf_hz_2 = 0,
.gyro_soft_lpf2_hz = 0,
.gyro_filter_q = 0,
.gyro_filter_r = 0,
.gyro_filter_p = 0,
.gyro_offset_yaw = 0,
.gyro_soft_lpf2_order = 1,
);
@ -577,11 +582,14 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz)
}
break;
default:
#if defined(USE_FIR_FILTER_DENOISE)
// this should be case FILTER_FIR:
gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroDenoiseState[axis];
firFilterDenoiseInit(&gyroSensor->softLpfFilter.gyroDenoiseState[axis], lpfHz, gyro.targetLooptime);
}
#endif
break;
}
}
@ -673,18 +681,26 @@ static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_
}
}
}
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz)
#endif
#if defined(USE_GYRO_LPF2)
#if GYRO_LPF2_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
# error "GYRO_LPF2_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
#endif
static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz)
{
gyroSensor->biquadRCFIR2ApplyFn = nullFilterApply;
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const int gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
int sections = 0;
if (lpfHz && lpfHz <= gyroFrequencyNyquist && order <= GYRO_LPF2_ORDER_MAX) { // Initialisation needs to happen once samplingrate is known
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadRCFIR2FilterInit(&gyroSensor->biquadRCFIR2[axis], lpfHz, gyroDt);
const int axisSections = biquadFilterLpfCascadeInit(gyroSensor->biquadLpf2[axis], order, lpfHz, gyro.targetLooptime);
sections = MAX(sections, axisSections);
}
}
gyroSensor->biquadLpf2Sections = sections;
}
#endif
@ -695,8 +711,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
#endif
#if defined(USE_GYRO_FAST_KALMAN)
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r, gyroConfig()->gyro_filter_p);
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
gyroInitFilterBiquadRCFIR2(gyroSensor, gyroConfig()->gyro_soft_lpf_hz_2);
#endif
#if defined(USE_GYRO_LPF2)
gyroInitFilterLpf2(gyroSensor, gyroConfig()->gyro_soft_lpf2_order, gyroConfig()->gyro_soft_lpf2_hz);
#endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
@ -933,8 +950,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
#if defined(USE_GYRO_FAST_KALMAN)
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf);
#endif
#if defined(USE_GYRO_LPF2)
for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) {
gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf);
}
#endif
#ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
@ -960,9 +980,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
#if defined(USE_GYRO_FAST_KALMAN)
// apply fast kalman
gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf);
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
// apply biquad RC+FIR2
gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf);
#endif
#if defined(USE_GYRO_LPF2)
for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) {
gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf);
}
#endif
#ifdef USE_GYRO_DATA_ANALYSE

5
src/main/sensors/gyro.h
View file

@ -69,7 +69,7 @@ typedef struct gyroConfig_s {
bool gyro_high_fsr;
bool gyro_use_32khz;
uint8_t gyro_to_use;
uint16_t gyro_soft_lpf_hz_2;
uint16_t gyro_soft_lpf2_hz;
uint16_t gyro_soft_notch_hz_1;
uint16_t gyro_soft_notch_cutoff_1;
uint16_t gyro_soft_notch_hz_2;
@ -79,8 +79,11 @@ typedef struct gyroConfig_s {
uint16_t gyro_filter_r;
uint16_t gyro_filter_p;
int16_t gyro_offset_yaw;
uint8_t gyro_soft_lpf2_order;
} gyroConfig_t;
#define GYRO_LPF2_ORDER_MAX 6
PG_DECLARE(gyroConfig_t, gyroConfig);
bool gyroInit(void);

2
src/main/target/OMNIBUS/target.h
View file

@ -20,7 +20,7 @@
// Removed to make the firmware fit into flash (in descending order of priority):
#undef USE_DSHOT_DMAR // OMNIBUS (F3) does not benefit from burst Dshot
#undef USE_GYRO_OVERFLOW_CHECK
#undef USE_GYRO_BIQUAD_RC_FIR2
#undef USE_GYRO_LPF2
#undef USE_SERIALRX_XBUS
#undef USE_TELEMETRY_LTM

2
src/main/target/common_fc_pre.h
View file

@ -156,7 +156,7 @@
#define USE_VTX_CONTROL
#define USE_VTX_SMARTAUDIO
#define USE_VTX_TRAMP
#define USE_GYRO_BIQUAD_RC_FIR2
#define USE_GYRO_LPF2
#ifdef USE_SERIALRX_SPEKTRUM
#define USE_SPEKTRUM_BIND