Skip to content

Commit

Permalink
Filter: protect against extremely low notch filter frequencies
Browse files Browse the repository at this point in the history
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
  • Loading branch information
tridge committed Oct 24, 2023
1 parent 146485f commit 60a1e96
Showing 1 changed file with 1 addition and 1 deletion.
2 changes: 1 addition & 1 deletion libraries/Filter/NotchFilter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
template <class T>
T NotchFilter<T>::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;
Expand Down

0 comments on commit 60a1e96

Please sign in to comment.