From 999d3b88e727ec003bb5b37f5093f7a28fec226c 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 db9e2e234b..538eb02dee 100644 --- a/libraries/Filter/NotchFilter.cpp +++ b/libraries/Filter/NotchFilter.cpp @@ -71,7 +71,7 @@ void NotchFilter::init_with_A_and_Q(float sample_freq_hz, float center_freq_h _center_freq_hz * NOTCH_MAX_SLEW_UPPER); } - if ((new_center_freq > 0.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { + if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) { float omega = 2.0 * M_PI * new_center_freq / sample_freq_hz; float alpha = sinf(omega) / (2 * Q); b0 = 1.0 + alpha*sq(A);