From 6b8dfcdeac37b596d4a895d3881b124ef5352920 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 May 2023 21:56:35 +1000 Subject: [PATCH] AP_AHRS: don't reject airspeed using EKF if dead-reckoning when dead-reckoning the EKF wind estimate can diverge from reality, leading to us rejecting a valid airspeed sensor. We are best off trusting airspeed if we are dead-reckoning --- libraries/AP_AHRS/AP_AHRS.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 32623cfe62..abbf71a9da 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -771,11 +771,11 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const if (fly_forward && hal.util->get_soft_armed() && get_filter_status(filter_status) && - filter_status.flags.rejecting_airspeed) { + (filter_status.flags.rejecting_airspeed && !filter_status.flags.dead_reckoning)) { // special case for when backend is rejecting airspeed data in - // an armed fly_forward state. Then the airspeed data is - // highly suspect and will be rejected. We will use the - // synthentic airspeed instead + // an armed fly_forward state and not dead reckoning. Then the + // airspeed data is highly suspect and will be rejected. We + // will use the synthentic airspeed instead return false; } return true;