Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 00:05:33 +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 firFilterDenoise_t denoisingFilter[2];
static pt1Filter_t pt1FilterYaw; 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 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; dtermNotchFilterApplyFn = nullFilterApply;
} else { } else {
dtermNotchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; 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; dtermLpfApplyFn = nullFilterApply;
} else { } else {
switch (pidProfile->dterm_filter_type) { 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; ptermYawFilterApplyFn = nullFilterApply;
} else { } else {
ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; ptermYawFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;

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

@ -280,7 +280,9 @@ void gyroInitFilters(void)
notchFilter1ApplyFn = nullFilterApply; notchFilter1ApplyFn = nullFilterApply;
notchFilter2ApplyFn = nullFilterApply; notchFilter2ApplyFn = nullFilterApply;
if (gyroConfig->gyro_soft_lpf_hz) { // Initialisation needs to happen once samplingrate is known uint32_t gyroFrequencyNyquist = (1.0f / (gyro.targetLooptime * 0.000001f)) / 2; // No rounding needed
if (gyroConfig->gyro_soft_lpf_hz && gyroConfig->gyro_soft_lpf_hz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known
if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) { if (gyroConfig->gyro_soft_lpf_type == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -303,7 +305,7 @@ void gyroInitFilters(void)
} }
} }
if (gyroConfig->gyro_soft_notch_hz_1) { if (gyroConfig->gyro_soft_notch_hz_1 && gyroConfig->gyro_soft_notch_hz_1 <= gyroFrequencyNyquist) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {
@ -311,7 +313,7 @@ void gyroInitFilters(void)
biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH); biquadFilterInit(notchFilter1[axis], gyroConfig->gyro_soft_notch_hz_1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
} }
} }
if (gyroConfig->gyro_soft_notch_hz_2) { if (gyroConfig->gyro_soft_notch_hz_2 && gyroConfig->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2);
for (int axis = 0; axis < 3; axis++) { for (int axis = 0; axis < 3; axis++) {