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

Dynamic Filter Update

Browse source 
Improves on earlier 3.5RC1 dynamic filter with:
- better FFT tracking performance overall, even with a narrower default notch width
- can reach 850Hz for high kV 2.5-3" and 6S quads
- works better with 32k gyros
- can be applied pre- (location 1) and post- (location 2) static filters. Pre-static filter location works best, but post-static may work well in 32k modes or with PT1 option
- option to use a PT1 filter rather than the classical notch filter, perhaps useful for quads with a lot of noise above their peak.
- ability to totally bypass the pre-FFT bandpass filter, by setting Q=0, maximising the range of responsiveness
- "ignore" value, absolute FFT bin amplitude below which the bin will be excluded from consideration. May be tweaked to optimise peak detection; primarily for advanced tuners. If too high, only the very peaks will be included, if too low, noise may clutter peak detection.
- "threshold" value for peak detection, which, when divided by 10, is the multiple by which one bin must exceed the previous one for peak detection to commence. If too low, FFT detection becomes more random, if too high, no peaks are detected.
This commit is contained in:
ctzsnooze 2018-08-05 17:51:28 +10:00
parent 40a4ab77fe
commit 1e960c95eb
7 changed files with 179 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -132,8 +132,8 @@ typedef struct gyroSensor_s {
filterApplyFnPtr notchFilter2ApplyFn;
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
filterApplyFnPtr notchFilterDynApplyFn;
biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
filterApplyFnPtr dynFilterApplyFn;
gyroDynamicFilter_t dynFilter[XYZ_AXIS_COUNT];
// overflow and recovery
timeUs_t overflowTimeUs;
@ -144,8 +144,12 @@ typedef struct gyroSensor_s {
#endif // USE_YAW_SPIN_RECOVERY
#ifdef USE_GYRO_DATA_ANALYSE
#define DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ 200
#define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 400
#define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 350
gyroAnalyseState_t gyroAnalyseState;
#endif
} gyroSensor_t;
STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
@ -203,8 +207,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
.gyro_offset_yaw = 0,
.yaw_spin_recovery = true,
.yaw_spin_threshold = 1950,
.dyn_notch_quality = 70,
.dyn_notch_width_percent = 50,
.dyn_filter_type = FILTER_BIQUAD,
.dyn_filter_width_percent = 40,
.dyn_notch_quality = 20,
.dyn_filter_location = DYN_FILTER_BEFORE_STATIC_FILTERS,
.dyn_filter_threshold = 30,
.dyn_filter_ignore = 20,
);
@ -510,6 +518,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor)
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif
return true;
@ -662,7 +671,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint
// Dereference the pointer to null before checking valid cutoff and filter
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = &nullFilterApply;
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
@ -742,15 +751,35 @@ static bool isDynamicFilterActive(void)
return feature(FEATURE_DYNAMIC_FILTER);
}
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
static void gyroInitFilterDynamic(gyroSensor_t *gyroSensor)
{
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
gyroSensor->dynFilterApplyFn = nullFilterApply;
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
const float notchQ = filterGetNotchQ(400, 390); //just any init value
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
switch (gyroConfig()->dyn_filter_type) {
case FILTER_PT1: {
const float gyroDt = gyro.targetLooptime * 1e-6f;
const float gain = pt1FilterGain(DYNAMIC_LOWPASS_DEFAULT_CUTOFF_HZ, gyroDt);
gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt1FilterInit(&gyroSensor->dynFilter[axis].pt1FilterState, gain);
}
break;
}
case FILTER_BIQUAD: {
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ);
gyroSensor->dynFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->dynFilter[axis].biquadFilterState, DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
break;
}
}
}
}
@ -780,7 +809,7 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
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
gyroInitFilterDynamicNotch(gyroSensor);
gyroInitFilterDynamic(gyroSensor);
#endif
}
@ -1075,7 +1104,7 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn);
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->dynFilter);
}
#endif
}