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

Fix filter bug for trying filter over nyquist

Browse source
This commit is contained in:
borisbstyle 2017-02-20 01:52:15 +01:00
parent 1e695ab7d6
commit 84105629ea
2 changed files with 10 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

8
src/main/flight/pid.c
View file

@ -96,9 +96,11 @@ void pidInitFilters(const pidProfile_t *pidProfile)
static firFilterDenoise_t denoisingFilter[2];
static pt1Filter_t pt1FilterYaw;
uint32_t pidFrequencyNyquist = (1.0f / dT) / 2; // No rounding needed
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
if (pidProfile->dterm_notch_hz == 0) {
if (pidProfile->dterm_notch_hz == 0 || pidProfile->dterm_notch_hz > pidFrequencyNyquist) {
dtermNotchFilterApplyFn = nullFilterApply;
} else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@ -109,7 +111,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
if (pidProfile->dterm_lpf_hz == 0) {
if (pidProfile->dterm_lpf_hz == 0 || pidProfile->dterm_lpf_hz > pidFrequencyNyquist) {
dtermLpfApplyFn = nullFilterApply;
} else {
switch (pidProfile->dterm_filter_type) {
@ -140,7 +142,7 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
if (pidProfile->yaw_lpf_hz == 0) {
if (pidProfile->yaw_lpf_hz == 0 || pidProfile->yaw_lpf_hz > pidFrequencyNyquist) {
ptermYawFilterApplyFn = nullFilterApply;
} else {
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;