Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

gyro & d-term filters: remove filtering options except biquad/pt1

Browse source 
* through extensive testing prior to the beginning of the RC cycle, we have
  discovered that the simplest combination of filters appears to be up to four
  PT1 filters: two for gyro, and two for d-term.
* non-cascaded biquad filter plumbing is retained for noisy setups and the
  dynamic notch bandpass, although gyro and d-term variants of the filtering may
  eventually be removed in favor of pt1
* update all related unit tests
This commit is contained in:
AJ Christensen 2018-05-26 19:25:27 +12:00
parent 418fd4beaa
commit a63c8b0079
8 changed files with 28 additions and 531 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -106,16 +106,9 @@ typedef struct gyroCalibration_s {
bool firstArmingCalibrationWasStarted = false;
#if GYRO_LPF_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
#error "GYRO_LPF_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
#endif
typedef union gyroLowpassFilter_u {
pt1Filter_t pt1FilterState;
biquadFilter_t biquadFilterState;
biquadFilterCascade_t biquadFilterCascadeState;
firFilterDenoise_t denoiseFilterState;
fastKalman_t fastKalmanFilterState;
} gyroLowpassFilter_t;
typedef struct gyroSensor_s {
@ -130,15 +123,13 @@ typedef struct gyroSensor_s {
filterApplyFnPtr lowpass2FilterApplyFn;
gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
// lagged moving average smoothing
filterApplyFnPtr lmaSmoothingApplyFn;
laggedMovingAverage_t lmaSmoothingFilter[XYZ_AXIS_COUNT];
// notch filters
filterApplyFnPtr notchFilter1ApplyFn;
biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
@ -149,7 +140,6 @@ typedef struct gyroSensor_s {
timeUs_t yawSpinTimeUs;
bool yawSpinDetected;
#endif // USE_YAW_SPIN_RECOVERY
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -163,7 +153,7 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
#endif
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order);
static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz);
#define DEBUG_GYRO_CALIBRATION 3
@ -179,7 +169,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ
#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps for 2000dps gyro)
#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps for 2000dps gyro)
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 3);
#ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
@ -194,10 +184,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
.gyro_lowpass_type = FILTER_PT1,
.gyro_lowpass_hz = 90,
.gyro_lowpass_order = 1,
.gyro_lowpass2_type = FILTER_PT1,
.gyro_lowpass2_hz = 0,
.gyro_lowpass2_order = 1,
.gyro_high_fsr = false,
.gyro_use_32khz = false,
.gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
@ -207,8 +195,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_soft_notch_cutoff_2 = 100,
.checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
.gyro_offset_yaw = 0,
.gyro_lma_depth = 0,
.gyro_lma_weight = 100,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
);
@ -638,7 +624,7 @@ bool gyroInit(void)
return ret;
}
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order)
void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
{
filterApplyFnPtr *lowpassFilterApplyFn;
gyroLowpassFilter_t *lowpassFilter = NULL;
@ -684,51 +670,6 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
}
break;
case FILTER_BIQUAD_RC_FIR2:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadRCFIR2FilterInit(&lowpassFilter[axis].biquadFilterState, gain);
}
break;
case FILTER_BUTTERWORTH:
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadCascadeFilterApply;
if (order <= GYRO_LPF_ORDER_MAX) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
int *sections = &lowpassFilter[axis].biquadFilterCascadeState.sections;
biquadFilter_t *biquadSections = lowpassFilter[axis].biquadFilterCascadeState.biquad;
*sections = biquadFilterLpfCascadeInit(biquadSections, order, lpfHz, gyro.targetLooptime);
}
}
break;
case FILTER_FAST_KALMAN:
*lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
fastKalmanInit(&lowpassFilter[axis].fastKalmanFilterState, gain / 2);
}
break;
#if defined(USE_FIR_FILTER_DENOISE)
case FILTER_FIR:
*lowpassFilterApplyFn = (filterApplyFnPtr) firFilterDenoiseUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
firFilterDenoiseInit(&lowpassFilter[axis].denoiseFilterState, lpfHz, gyro.targetLooptime);
}
break;
#endif
}
}
}
static void gyroInitLmaSmoothing(gyroSensor_t *gyroSensor, uint8_t depth, uint8_t weight)
{
gyroSensor->lmaSmoothingApplyFn = nullFilterApply;
if (depth && weight) {
const uint8_t windowSize = depth + 1;
const float lmaWeight = weight * 0.01f;
gyroSensor->lmaSmoothingApplyFn = (filterApplyFnPtr)lmaSmoothingUpdate;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
lmaSmoothingInit(&gyroSensor->lmaSmoothingFilter[axis], windowSize, lmaWeight);
}
}
}
@ -817,20 +758,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
gyroSensor,
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyroConfig()->gyro_lowpass_hz,
gyroConfig()->gyro_lowpass_order
gyroConfig()->gyro_lowpass_hz
);
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz,
gyroConfig()->gyro_lowpass2_order
gyroConfig()->gyro_lowpass2_hz
);
gyroInitLmaSmoothing(gyroSensor, gyroConfig()->gyro_lma_depth, gyroConfig()->gyro_lma_weight);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE
@ -1118,7 +1055,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
@ -1144,7 +1080,6 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
// apply lowpass2 filter
gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
#ifdef USE_GYRO_DATA_ANALYSE
// apply dynamic notch filter