From 60a1e96d24b433f4800e9461b028fe530d71bf80 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Oct 2023 19:30:05 +1100 Subject: [PATCH] Filter: protect against extremely low notch filter frequencies an incorrectly configured ESC telemetry source can lead to a notch running at very low frequencies. A simple example is a lua script like this: function update() esc_telem:update_rpm(12, 0, 0) return update, 10 end return update() where motor 12 is unused. with that script in place we get a 1.0078 Hz filter which leads to massive phase lag and a crashed aircraft this is a safety protection. We should also try to find out why the INS_HNTCH_FREQ lower limit is not working --- libraries/Filter/NotchFilter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp index ea2a77d80faabb..8615c425c7264c 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -104,7 +104,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h template T NotchFilter::apply(const T &sample) { - if (!initialised || need_reset) { + if (!initialised || need_reset || _center_freq_hz <= 2.0) { // if we have not been initialised when return the input // sample as output and update delayed samples signal1 = sample;