diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 4fa0f0fdaa..25faca7079 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9732a96859..32d2864525 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -280,7 +280,9 @@ void gyroInitFilters(void) notchFilter1ApplyFn = 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) { softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; 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; const float gyroSoftNotchQ1 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_1, gyroConfig->gyro_soft_notch_cutoff_1); 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); } } - if (gyroConfig->gyro_soft_notch_hz_2) { + if (gyroConfig->gyro_soft_notch_hz_2 && gyroConfig->gyro_soft_notch_hz_2 <= gyroFrequencyNyquist) { notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float gyroSoftNotchQ2 = filterGetNotchQ(gyroConfig->gyro_soft_notch_hz_2, gyroConfig->gyro_soft_notch_cutoff_2); for (int axis = 0; axis < 3; axis++) {