From 4dd65a287664cdb4d2882b93b810f8573ea9433a Mon Sep 17 00:00:00 2001 From: AJ Christensen Date: Sat, 10 Feb 2018 16:39:05 +1300 Subject: [PATCH] Biquad RC+FIR2: Allow user to specify cutoff Hz parameter directly * Generate 'k' per the code for the PT1 * Adjust function prototypes/functions to accept f_cut/dT where applicable * Adjust gyro configuration, parameter group, interface settings to suit --- src/main/common/filter.c | 7 ++++--- src/main/common/filter.h | 2 +- src/main/interface/settings.c | 4 +++- src/main/sensors/gyro.c | 15 ++++++++------- src/main/sensors/gyro.h | 1 + 5 files changed, 17 insertions(+), 12 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 8646aa4c01..3424729cd9 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -303,10 +303,11 @@ 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) +// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k +void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, float dT) { - float k = 2 * 2 * sqrt(q) / (sqrt(q + 4 * r) + sqrt(q)); // 1st order IIR filter k + 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; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 5ecbcb0078..8744f889bd 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -96,7 +96,7 @@ 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 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); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 197a1dd8bf..a0013d6d7f 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -345,7 +345,9 @@ 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) || defined(USE_GYRO_BIQUAD_RC_FIR2) +#if 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) }, +#elif defined(USE_GYRO_FAST_KALMAN) { "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) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 58035079a0..550694a166 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -138,7 +138,7 @@ 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 gyro_filter_q, uint16_t gyro_filter_r); +static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz); #endif static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); @@ -170,6 +170,7 @@ 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_filter_q = 0, .gyro_filter_r = 0, .gyro_filter_p = 0, @@ -574,15 +575,15 @@ 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 gyro_filter_q, uint16_t gyro_filter_r) +static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz) { 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) { + 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; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadRCFIR2FilterInit(&gyroSensor->biquadRCFIR2[axis], gyro_filter_q, gyro_filter_r); + biquadRCFIR2FilterInit(&gyroSensor->biquadRCFIR2[axis], lpfHz, gyroDt); } } } @@ -596,7 +597,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) #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_filter_q, gyroConfig()->gyro_filter_r); + gyroInitFilterBiquadRCFIR2(gyroSensor, gyroConfig()->gyro_soft_lpf_hz_2); #endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 4c48892bca..4510c56ce8 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -65,6 +65,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_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2;