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

Merge pull request #4965 from fujin/biquad-RC-FIR2-filter 

Biquad RC+FIR2 Filter: Prototype of ledvinap's suggestion on #4890
This commit is contained in:
Michael Keller 2018-01-16 16:15:33 +13:00 committed by GitHub
parent 5b95cc37f5 17bd318ec1
commit bca7b905b5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 56 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -303,6 +303,17 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
}
}
// ledvinap's proposed RC+FIR2 Biquad-- equivalent to 'static' fast Kalman, without error estimation
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float q, float r)
{
float k = 2 * 2 * sqrt(q) / (sqrt(q + 4 * r) + sqrt(q)); // 1st order IIR filter k
filter->b0 = k / 2;
filter->b1 = k / 2;
filter->b2 = 0;
filter->a1 = -(1 - k);
filter->a2 = 0;
}
// Fast two-state Kalman
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p)
{

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

@ -96,6 +96,8 @@ 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, float q, float r);
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p);
float fastKalmanUpdate(fastKalman_t *filter, float input);

8
src/main/interface/settings.c
View File

@ -356,10 +356,10 @@ const clivalue_t valueTable[] = {
{ "gyro_notch1_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_1) },
{ "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) },
{ "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) },
#if defined(USE_GYRO_FAST_KALMAN)
{ "gyro_kalman_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_q) },
{ "gyro_kalman_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_r) },
{ "gyro_kalman_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_kalman_p) },
#if defined(USE_GYRO_FAST_KALMAN) || defined(USE_GYRO_BIQUAD_RC_FIR2)
{ "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) },
#endif
{ "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
#ifdef USE_GYRO_OVERFLOW_CHECK

42
src/main/sensors/gyro.c
View File

@ -122,6 +122,10 @@ 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
} gyroSensor_t;
@ -132,7 +136,9 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_kalman_q, uint16_t gyro_kalman_r, uint16_t gyro_kalman_p);
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 gyro_filter_q, uint16_t gyro_filter_r);
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
@ -164,9 +170,9 @@ 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_kalman_q = 0,
.gyro_kalman_r = 0,
.gyro_kalman_p = 0,
.gyro_filter_q = 0,
.gyro_filter_r = 0,
.gyro_filter_p = 0,
);
@ -554,15 +560,28 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
#endif
#if defined(USE_GYRO_FAST_KALMAN)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_kalman_q, uint16_t gyro_kalman_r, uint16_t gyro_kalman_p)
static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r, uint16_t gyro_filter_p)
{
gyroSensor->fastKalmanApplyFn = nullFilterApply;
// If Kalman Filter noise covariances for Process and Measurement are non-zero, we treat as enabled
if (gyro_kalman_q != 0 && gyro_kalman_r != 0) {
if (gyro_filter_q != 0 && gyro_filter_r != 0) {
gyroSensor->fastKalmanApplyFn = (filterApplyFnPtr)fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&gyroSensor->fastKalman[axis], gyro_kalman_q, gyro_kalman_r, gyro_kalman_p);
fastKalmanInit(&gyroSensor->fastKalman[axis], gyro_filter_q, gyro_filter_r, gyro_filter_p);
}
}
}
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r)
{
gyroSensor->biquadRCFIR2ApplyFn = nullFilterApply;
// If the biquad RC+FIR2 Kalman-esque Process and Measurement noise covariances are non-zero, we treat as enabled
if (gyro_filter_q != 0 && gyro_filter_r != 0) {
gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadRCFIR2FilterInit(&gyroSensor->biquadRCFIR2[axis], gyro_filter_q, gyro_filter_r);
}
}
}
@ -574,7 +593,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroInitSlewLimiter(gyroSensor);
#endif
#if defined(USE_GYRO_FAST_KALMAN)
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_kalman_q, gyroConfig()->gyro_kalman_r, gyroConfig()->gyro_kalman_p);
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_filter_q, gyroConfig()->gyro_filter_r);
#endif
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
@ -780,6 +801,8 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren
float gyroADCf = (float)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
#ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
@ -805,6 +828,9 @@ 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
#ifdef USE_GYRO_DATA_ANALYSE

6
src/main/sensors/gyro.h
View File

@ -70,9 +70,9 @@ typedef struct gyroConfig_s {
uint16_t gyro_soft_notch_hz_2;
uint16_t gyro_soft_notch_cutoff_2;
gyroOverflowCheck_e checkOverflow;
uint16_t gyro_kalman_q;
uint16_t gyro_kalman_r;
uint16_t gyro_kalman_p;
uint16_t gyro_filter_q;
uint16_t gyro_filter_r;
uint16_t gyro_filter_p;
} gyroConfig_t;
PG_DECLARE(gyroConfig_t, gyroConfig);

3
src/main/target/common_fc_pre.h
View File

@ -171,5 +171,6 @@
#define USE_NAV
#define USE_TELEMETRY_IBUS
#define USE_UNCOMMON_MIXERS
#define USE_GYRO_FAST_KALMAN
// #define USE_GYRO_FAST_KALMAN
#define USE_GYRO_BIQUAD_RC_FIR2
#endif