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 Nov 1, 2023
1 parent 29f5bce commit 898ac0a
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 @@ -71,7 +71,7 @@ void NotchFilter<T>::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);
Expand Down

0 comments on commit 898ac0a

Please sign in to comment.