Filter: protect against extremely low notch filter frequencies

[tridge]

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. I suspect the 1.0078 is the last value from the slew limiter before we hit 1.0 Hz
Note: this is a safety protection. We should also try to find out why the INS_HNTCH_FREQ lower limit is not working

this is what the filtered (in red) vs unfiltered (in green) looks like with the above lua script for a quadplane takeoff in QLOITER in SITL:

this is the same test with the protection in place:

[IamPete1]

I think its better to change the existing limit in the init_with_A_and_Q function so initialised is never set true.

This line:

ardupilot/libraries/Filter/NotchFilter.cpp

Line 74 in 4dc9d1e

if ((new_center_freq > 0.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {

Would become:

if ((new_center_freq > 2.0) && (new_center_freq < 0.5 * sample_freq_hz) && (Q > 0.0)) {

[IamPete1]

I think #25355 is the root of this issue.

[tridge]

I think #25355 is the root of this issue.

as noted on that PR, that does not properly fix the issue

[IamPete1]

All looks good. I think the zero check on last_update_ms is needed in lots more places. Might be worth a helper function. There are 8 places that check the timeout.

[andyp1per]

I though @WickedShell already had a helper for this?

[andyp1per]

I do think a helper woulud make this clearer

[tridge]

added stale() helper

[andyp1per]

I think #25355 is the root of this issue.

as noted on that PR, that does not properly fix the issue

Yes, but its still a valid change. I think we need both changes.

[IamPete1]

LGTM

[andyp1per]

I think this is the wrong change. I have updated https://github.com/ArduPilot/ardupilot/pull/25355to reflect what I think should go in. I was going to put a blocking review on this and now wish I had. Please, please can we have a little more discussion.

[tridge]

I have updated https://github.com/ArduPilot/ardupilot/pull/25355to reflect what I think should go in.

I had a look, and it doesn't really fix the problem. It still ends up running the uninitialised RPM sources at the min frequency.
Remember that we also just fixed a bug where all ESC_TELEM_MAX_ESCS (32 by default) ESCs get seen as zero RPM for a short time at boot due to the race in the time handling in AP_ESC_Telem. If a board boots fast enough then we'd end up with 32 ESCs populating zero RPM, ending up with 32 sets of notch filters at INS_HNTCH_BW.
That is an example of why I don't want this in 4.4.x. We just can't cover all the unintended consequences.
For 4.5.x we need to work out how to fix this all properly, and I think the approach you are taking here is a bandaid over a bad design