Airspeed Sensor Failure Handling Improvements

[priseborough]

Refer to #19883 for prior discussion of the issues

[priseborough]

The interaction between the airspeed library fault handling and the EKF and TECS is complex, so this needs careful review and testing in SITL.

[priseborough]

I've added an autotest that tests the effect of a blocked pitot tube that then partially unblock before fully unblocking. The following figure shows how the airspeed sensor is disabled until fully recovered.

[priseborough]

@pompecukor this is ready for testing.

[priseborough]

@tridge Please describe the flight testing that you think is necessary to have this approved for merge. TXS.

[tridge]

I've fixed the param doc error, hopefully will pass CI now

[tridge]

this currently depends on ARSPD_WIND_MAX being set due to the is_positive check. Is that needed? I think we can just set data_is_implausible false if WIND_MAX is 0 and still do the EKF gate.

[tridge]

Alex, I've put a build of this PR on top of 4.2.3-bigplane here: http://uav.tridgell.net/tmp/plane-CubeOrange-4.2-bigplane-airspeed-fail-test1.apj

[pompecukor]

Real live testing on Griffin Pro seemed fine today. Do not that we have dual airspeed sensor on this unit, so we set one to Not use. However it was still showing the calibrated airspeed, which was useful to see that it was really holding a good speed when we simulated failure on the primary by changing the ratio to first 10% then 2 mins later to 50%, then 2 mins later, back to 100% of original value.
Logs are with @priseborough and @tridge

[priseborough]

Thank you @pompecukor for that test.

Here is the test ratio for the failed sensor showing that when the ARSPD_RATIO was reduced to simulate a blocked sensor, the consistency test ratio rises above the threshold of 5.0 set by ARSPD_WIND_GATE.

2022-09-07 20:27:35.799 Airspeed sensor 1 failure. Disabling

When a partial clearance of the blockage is simulated by increasing the value of ARSPD_RATIO, the test ratio reduces, but is still above 70% of ARSPD_WIND_GATE so the failed status is not cleared.

The fault status doesn't clear until after the simulated blockage is removed

2022-09-07 20:34:15.059 Airspeed sensor 1 now OK. Re-enabled

Inspection of wind estimates from the Nav EKF show that they remain mostly unaffected by the airspeed failure

The EKF is receiving and processing the airspeed measurements as shown by the following plot fo the innovations, but he innovations are not allowed to modify EKF states while the airspeed sensor is declared failed.

No changes to the default value of the ARSPD_WIND_GATE parameter are required at this point in time.

Note that the legacy speed mismatch algorithm is disabled by the default value of ARSPD_OPTIONS parameter of 11 used, however the warning message associated with this check is still logged, eg:

2022-09-07 20:27:30.999 Airspeed 1 warning 15.3m/s air to gnd speed diff

[WickedShell]

@priseborough Can you test a slowly failing sensor?

I've seen multiple MS4525DO based airspeed sensors fail slowly inflight where it drifts off on the scale of 150 pascals a minute or so and the plane was flying a long straight leg (multiple km) flight. This has historically resulted in stalls and crashes on the systems I've seen it with. My past attempts to catch this in the EKF basically suffered from the EKF resetting before the failure was seen. I'd be curious to know how this is handled with these patches.

[priseborough]

@WickedShell A slow failure like the one you describe will not be picked up during straight and level flight because it will be indistinguishable from a increasing tail wind, however it would be detected when the vehicle starts turning, but by then it would be too late. We can discuss offline about how the auto test could be modified to simulate the type of fault you are interested in, but given this PR improves the current level of protection I don't want to delay merging it while we address additional 'what if's'

[priseborough]

I've seen multiple MS4525DO based airspeed sensors fail slowly inflight where it drifts off on the scale of 150 pascals a minute or so and the plane was flying a long straight leg (multiple km) flight.

Thinking about this some more, this scenario could be detected for a single airspeed sensor with some combination of the following:

Elevator trim curve depending on the amount of longitudinal static stability margin the aircraft has.
Relationship between propeller RPM, density altitude, airspeed and climb rate

This requires model based algorithms and the challenge is how to do so in a way that is sufficiently easy for users to tune and applicable to a sufficiently wide range of propulsion methods.

Airspeed sensors are relatively small, light and cheap, so if I was developing a platform that had to fly BVLOS and was reliant on airspeed sensing for safe flight, I would use dual sensors at a minimum and pick the sensor with the smallest airspeed innovation. I think this is where we should focus development efforts for this use case.

[tridge]

@priseborough I did a flight today:
http://uav.tridgell.net/tmp/00000161.BIN
I tested with both too high and too low ARSPD_RATIO
this was the branch I flew:
https://github.com/tridge/ardupilot/tree/SV-2022-09-11
it has a change to allow for EKF checking without a ARSPD_WIND_MAX

[priseborough]

The white space merge conflict has been fixed and I've rebased on current master.

[peterbarker]

@priseborough test ratio really oughtn't be dependent on ground course, should it?

[priseborough]

Whilst investigating the discrepancy between the auto test results for the same test running on my dev environment vs the build server a potential issue was highlighted which was the slow recovery of wind state estimates corrupted by takeoff with a blocked pitot tube. The following figures show the airspeed innovation levels, test ratio and wind velocity estimates showing how the states recover slowly.

This has been improved by 09df671 which allows the wind states to learn faster when the airspeed sensor is disabled and wind states are bing estimated using the zero sideslip assumption.

[priseborough]

@priseborough test ratio really oughtn't be dependent on ground course, should it?

Yes it will be because with the sensor disabled and an incorrect wind state learned during takeoff with a blocked pitot tube the airspeed, ground speed and wind states will be consistent twice per orbit. The 09df671 change I pushed makes the wind states recover faster so that this difference reduces faster after the airspeed sensor blockage is removed.

[priseborough]

The patch 09df671 has also been tested on replay of the flight test log submitted by @tridge http://uav.tridgell.net/tmp/00000161.BIN

This flight failed the airspeed sensor after takeoff, so the patch is not expected to be of much benefit for that scenario. There is a slight reduction in test ratios immediately after the sensor scale factor error is removed which is a small improvement.

[peterbarker]

Merged, thanks!

As with new tests with heuristics involved, there may be a chance of something flapping in here. Merging means it starts to get run a lot, so we'll see soon enough.