AP_InertialSensor: init all notch center frequencies

[IamPete1]

We were only setting the first notch center freq. If multi source data is missing it just does a hold on that channel so if never init and never getting data it can be stuck holding at that 0 value.

[tridge]

@IamPete1 @andyp1per this fixes part of the problem but not all of it, at least with 4.4.x. With this fix we no longer get the filters down at 1Hz, but with 4.4.x we end up with 48 notch filters all at 40Hz with the following parameters:

INS_HNTC2_ENABLE 0           # Disabled
INS_HNTCH_ATT    40.0
INS_HNTCH_BW     10.0
INS_HNTCH_ENABLE 1           # Enabled
INS_HNTCH_FM_RAT 1.0
INS_HNTCH_FREQ   40.0
INS_HNTCH_HMNCS  15.0        #   1st harmonic|  2nd harmonic|  3rd harmonic|  4th harmonic
INS_HNTCH_MODE   3           # ESC Telemetry
INS_HNTCH_OPTS   18          # Multi-Source|Triple notch
INS_HNTCH_REF    1.0

for 4 motors we should end up with 12 at 40Hz, 12 at 80Hz, 12 at 120Hz and 12 at 160Hz. When ESC telemetry gives zero RPM we squash them all down to 40Hz (they are actually 39.68Hz, 40Hz and 40.3Hz as that is the triple notch spread) instead of correctly spreading them out on the harmonics.
The result is much more phase lag than we should have, so again the SITL quadplane crashes
I think we should disable notches completely when the RPM from the ESC telem is low enough (eg. zero, or maybe half of INS_HNTCH_FREQ?).
So maybe instead of taking max(notch_freq, telem_freq) we should instead treat any telem_freq below half notch_freq as zero and pass zero all the way down, then in NotchFilter treat zero as disabled?

[IamPete1]

@tridge Any tips to reproduce? I'm haven't been able to on master.

For frequencies under the cut off we could "phase out" the filter. The lower frequency then becomes a attenuation limit. So the notch will track under, but it will end up with 0 attenuation. Betaflight does something like this, uses is weighted average between notched and un-notched.

Something like this so we track that amplitude set by the user cutoff:

Or just pick our own easier to work out method. The key thing it to bring them in and out smoothly. I don't think we would want a straight enable/disable threshold.

The other question is what should be done if the ESC telem becomes unavailable completely. Currently we will fall back to throttle notch, but that is extremely unlikely to do anything sensible with the configuration set for ESC's. We have inadvertently created a case where loss of ESC telem can crash a vehicle.

[tridge]

@IamPete1 here are 3 logs showing the problem in SITL:
http://uav.tridgell.net/tmp/notch_test/
the logs include a tiny lua script that triggers the problem, mimicking the original issue from the user. You can extract the lua script from the log, or I have also put it in the folder
The logs are:

• 21.bin this is without either your fix or mine. You can see it crashes on takeoff due to massive lag
• 20.bin is with my very simple fix with the 2Hz threshold. It flies fine
• 22.bin is with both my fix and your change. The vehicle crashes in SITL. The phase lag isn't as bad as 21.bin with no fixes, but still bad enough to prevent flight

I propose we merge my simple fix now, and work towards something more comprehensive in future

[tridge]

@IamPete1 this is the phase lag with your change:

and this is with my change:

same parameters, same flight plan
for reference, this is the original bug (ie. no fixes) in SITL, from 21.bin log above:

this is what happened with the real aircraft.

[tridge]

and this is the notch frequencies with your change:

[IamPete1]

@tridge All 6 notches at set min frequency is the correct behavior in this case? If it crashes there then it will also crash with all the motors anywhere between 0 and 2400 rpm (40 Hz)? If the cut off is set so low that the vehicle crashes isn't that just a miss configuration?

[andyp1per]

I don't think the cutoff was set low was it? I agree that if it crashes with the notches sitting at the configured value then that's down to misconfiguration, but I thought what actually happened was the frequencies went far below this because of the bad initialization.

I do think we should do something simple here for 4.4 - more complicated things will need much more testing as the outcomes are not always predictable. Presumably switching the filter on and off could generate shot noise? If that's true then you phasing idea seems reasonable to me. I do think we should get some input from @priseborough and @lthall on whatever is proposed.

[tridge]

All 6 notches at set min frequency is the correct behavior in this case?

that assumes they ever could all be non-zero but below min freq. On both the SITL test and in the real aircraft this is not actually true.
The lua script always generates zero, just like happened on the real aircraft. With your patch that always-zero source got treated as 40Hz.

[IamPete1]

The lua script always generates zero, just like happened on the real aircraft. With your patch that always-zero source got treated as 40Hz.

I would argue that that is a scripting bug in that case not a AP one.

All this patch does is make all the notches behave the same as the first one. If that is incorrect then the bug already exists now for the first notch.

[tridge]

All this patch does is make all the notches behave the same as the first one. If that is incorrect then the bug already exists now for the first notch.

I think removing the init of the first one would be better

[lthall]

I have been looking over this and I think the best way forward is to initially fix the current code then add the new functionality that Tridge has described. After a quick look this PR looks like it takes care of the first step and is probably going to be safe to back port if needed.

I would suggest we start with the simplest fix then add the extra features Tridge is suggesting as a separate PR afterwards.

edit: I have moved the rest of the points to #25461 as it isn't relevant here.

[tridge]

@lthall thanks Leonard!
I have done a PR here that has a placeholder for the attenuation ramp:
#25442
it moves all the decisions about clamping of the notch frequency, nyquist limit and attenuation ramp to one function, HarmonicNotchFilter::set_center_frequency(), see HarmonicNotchFilter.cpp line 228 in that PR
the idea is to have just one function that makes all these decisions as opposed to what we have now where the clamping is in many places in the code. It also allows the HarmonicNotch access to its params (bizarrely the HarmonicNotch can't access its own parameter structure in master), which means it can access the options bitmask, which allows us to turn on/off this attenuation ramp with a user controlled option. That is important for helis with a governor
the attenuation placeholder in my PR is linear, which sounds like it should be replaced by a sqrt() function based on your above work?
It also looks like your proposal does allow the freq to go below INS_HNTCH_FREQ, but with attenuation adjusted for phase lag. In my PR I only adjust the attenuation, not the frequency at the moment

[tridge]

for 4.5.x I want to remove all these lines and leave the value as zero, meaning unitialised.
The calculated_notch_freq_hz array is an array of observed frequencies from ESCs. We have not observed this frequency in the init, so we should not be filling in a false value
we should handle the zero in the filter

[tridge]

this was flown successfully on 2 vehicles, one a quadplane with throttle notch and a 5" quad with ESC multi-source notch