5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00

AP_NavEKF2: only fuse airspeed if healthy

This commit is contained in:
Andrew Tridgell 2020-12-08 14:04:03 +11:00
parent d16af5448b
commit 5e72907730

View File

@ -756,6 +756,7 @@ void NavEKF2_core::readAirSpdData()
const auto *aspeed = dal.airspeed();
if (aspeed &&
aspeed->use() &&
aspeed->healthy() &&
aspeed->last_update_ms() != timeTasReceived_ms) {
tasDataNew.tas = aspeed->get_airspeed() * dal.get_EAS2TAS();
timeTasReceived_ms = aspeed->last_update_ms();