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
This commit is contained in:
parent
1c3a3229ad
commit
4dd65a2876
|
@ -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
|
// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k
|
||||||
void biquadRCFIR2FilterInit(biquadFilter_t *filter, float q, float r)
|
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->b0 = k / 2;
|
||||||
filter->b1 = k / 2;
|
filter->b1 = k / 2;
|
||||||
filter->b2 = 0;
|
filter->b2 = 0;
|
||||||
|
|
|
@ -96,7 +96,7 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
|
||||||
float biquadFilterApply(biquadFilter_t *filter, float input);
|
float biquadFilterApply(biquadFilter_t *filter, float input);
|
||||||
float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff);
|
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);
|
void fastKalmanInit(fastKalman_t *filter, float q, float r, float p);
|
||||||
float fastKalmanUpdate(fastKalman_t *filter, float input);
|
float fastKalmanUpdate(fastKalman_t *filter, float input);
|
||||||
|
|
|
@ -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_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_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) },
|
{ "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_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_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) },
|
{ "gyro_filter_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) },
|
||||||
|
|
|
@ -138,7 +138,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
|
||||||
#if defined(USE_GYRO_FAST_KALMAN)
|
#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);
|
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)
|
#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
|
#endif
|
||||||
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
|
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_hz_2 = 200,
|
||||||
.gyro_soft_notch_cutoff_2 = 100,
|
.gyro_soft_notch_cutoff_2 = 100,
|
||||||
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
|
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
|
||||||
|
.gyro_soft_lpf_hz_2 = 0,
|
||||||
.gyro_filter_q = 0,
|
.gyro_filter_q = 0,
|
||||||
.gyro_filter_r = 0,
|
.gyro_filter_r = 0,
|
||||||
.gyro_filter_p = 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)
|
#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;
|
gyroSensor->biquadRCFIR2ApplyFn = nullFilterApply;
|
||||||
|
const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
|
||||||
// If the biquad RC+FIR2 Kalman-esque Process and Measurement noise covariances are non-zero, we treat as enabled
|
const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
|
||||||
if (gyro_filter_q != 0 && gyro_filter_r != 0) {
|
if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
|
||||||
gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
|
||||||
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
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)
|
#if defined(USE_GYRO_FAST_KALMAN)
|
||||||
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r, gyroConfig()->gyro_filter_p);
|
gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r, gyroConfig()->gyro_filter_p);
|
||||||
#elif defined(USE_GYRO_BIQUAD_RC_FIR2)
|
#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
|
#endif
|
||||||
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
|
gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz);
|
||||||
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
|
||||||
|
|
|
@ -65,6 +65,7 @@ typedef struct gyroConfig_s {
|
||||||
bool gyro_high_fsr;
|
bool gyro_high_fsr;
|
||||||
bool gyro_use_32khz;
|
bool gyro_use_32khz;
|
||||||
uint8_t gyro_to_use;
|
uint8_t gyro_to_use;
|
||||||
|
uint16_t gyro_soft_lpf_hz_2;
|
||||||
uint16_t gyro_soft_notch_hz_1;
|
uint16_t gyro_soft_notch_hz_1;
|
||||||
uint16_t gyro_soft_notch_cutoff_1;
|
uint16_t gyro_soft_notch_cutoff_1;
|
||||||
uint16_t gyro_soft_notch_hz_2;
|
uint16_t gyro_soft_notch_hz_2;
|
||||||
|
|
Loading…
Reference in New Issue