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
This commit is contained in:
Andrew Tridgell 2023-05-29 21:56:35 +10:00 committed by Randy Mackay
parent 5f6a1f7f71
commit 6f5d3a0dfd
1 changed files with 4 additions and 4 deletions

View File

@ -771,11 +771,11 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
if (fly_forward && if (fly_forward &&
hal.util->get_soft_armed() && hal.util->get_soft_armed() &&
get_filter_status(filter_status) && 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 // special case for when backend is rejecting airspeed data in
// an armed fly_forward state. Then the airspeed data is // an armed fly_forward state and not dead reckoning. Then the
// highly suspect and will be rejected. We will use the // airspeed data is highly suspect and will be rejected. We
// synthentic airspeed instead // will use the synthentic airspeed instead
return false; return false;
} }
return true; return true;