From 2f486c37fc4a1b5d492b587f80ff371937d7ed9e Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Jul 2022 10:19:53 +0200 Subject: [PATCH] ekf2: start airspeed fusion when test ratio is passing only When wind is already estimated, we don't reset the states using airspeed data, so it could be that the fusion fails if the airspeed isn't consistent with the filter (test ratio > 1). In this case, don't start the fusion. When wind isn't already estimated, the wind states are reset using airspeed so the fusion can start regardless of the current test ratio. --- src/modules/ekf2/EKF/control.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index f5b2b6d8e6..75341b1290 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1028,7 +1028,9 @@ void Ekf::controlAirDataFusion() const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_using_synthetic_position; const bool is_airspeed_significant = _airspeed_sample_delayed.true_airspeed > _params.arsp_thr; - const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant; + const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); + const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant + && (is_airspeed_consistent || !_control_status.flags.wind); // if wind isn't already estimated, the states are reset when starting airspeed fusion if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) {