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

add PT2 and PT3 lowpass filter options

Browse source
This commit is contained in:
ctzsnooze 2021-05-18 09:59:27 +10:00
parent 636d563abe
commit 3b62b2e5d4
8 changed files with 123 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

32
src/main/sensors/gyro_init.c
View file

@ -181,8 +181,8 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
// type. It will be overridden for positive cases.
*lowpassFilterApplyFn = nullFilterApply;
// If lowpass cutoff has been specified and is less than the Nyquist frequency
if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
// If lowpass cutoff has been specified
if (lpfHz) {
switch (type) {
case FILTER_PT1:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
@ -192,13 +192,29 @@ static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_
ret = true;
break;
case FILTER_BIQUAD:
if (lpfHz <= gyroFrequencyNyquist) {
#ifdef USE_DYN_LPF
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
#else
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
*lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
#endif
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
}
ret = true;
}
break;
case FILTER_PT2:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
pt2FilterInit(&lowpassFilter[axis].pt2FilterState, gain);
}
ret = true;
break;
case FILTER_PT3:
*lowpassFilterApplyFn = (filterApplyFnPtr) pt3FilterApply;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
pt3FilterInit(&lowpassFilter[axis].pt3FilterState, gain);
}
ret = true;
break;
@ -218,6 +234,12 @@ static void dynLpfFilterInit()
case FILTER_BIQUAD:
gyro.dynLpfFilter = DYN_LPF_BIQUAD;
break;
case FILTER_PT2:
gyro.dynLpfFilter = DYN_LPF_PT2;
break;
case FILTER_PT3:
gyro.dynLpfFilter = DYN_LPF_PT3;
break;
default:
gyro.dynLpfFilter = DYN_LPF_NONE;
break;