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

Merge pull request #5144 from fujin/bqrcf2-lpf-cutoff-hz 

Biquad RC+FIR2: Allow user to specify cutoff Hz parameter directly
This commit is contained in:
Michael Keller 2018-02-14 13:23:57 +13:00 committed by GitHub
parent 5593b43af7 4dd65a2876
commit afff03197c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;

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

@ -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);

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

@ -341,7 +341,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) },

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

@ -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);

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

@ -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;