ardupilot
ardupilot copied to clipboard
ArduPilot
Airspeed Sensor Failure Handling Improvements
Refer to https://github.com/ArduPilot/ardupilot/pull/19883 for prior discussion of the issues
The interaction between the airspeed library fault handling and the EKF and TECS is complex, so this needs careful review and testing in SITL.
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.
@tridge Please describe the flight testing that you think is necessary to have this approved for merge. TXS.
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
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
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
@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.
@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'
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.
@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
I've rebased but am having now having problems running the auto-test. I will be available at the Wed dev call to discuss the PR.
@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
I've checked the log and the fault is detected as expected:
Airspeed scale factor high error test just exceeds the 5 sigma gate and is detected
2022-09-11 11:52:41.774 Airspeed sensor 1 failure. Disabling
2022-09-11 11:53:24.674 Airspeed sensor 1 now OK. Re-enabled
Airspeed scale factor low error test just exceeds the 5 sigma gate and is detected
2022-09-11 11:49:03.669 Airspeed sensor 1 failure. Disabling
2022-09-11 11:49:50.469 Airspeed sensor 1 now OK. Re-enabled
Other smaller scale factor up changes were not detected as failures
The 'noise floor' on the consistency test level is 2.5 which is half the ARSPD_WIND_GATE threshold of 5 sigma, so there is no requirement at the moment to adjust the default value.
Wind speed estimates were protected from corruption when the larger scale factor errors were introduced
The white space merge conflict has been fixed and I've rebased on current master.
@priseborough test ratio really oughtn't be dependent on ground course, should it?
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 https://github.com/ArduPilot/ardupilot/pull/21040/commits/09df67186b9aaa0e4eab1eccb2e7c6485af0a5a0 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 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 https://github.com/ArduPilot/ardupilot/pull/21040/commits/09df67186b9aaa0e4eab1eccb2e7c6485af0a5a0 change I pushed makes the wind states recover faster so that this difference reduces faster after the airspeed sensor blockage is removed.
The patch https://github.com/ArduPilot/ardupilot/commit/09df67186b9aaa0e4eab1eccb2e7c6485af0a5a0 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.
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.