From 2a7d3b0cec3a43216feea0142fd710c617bcbc49 Mon Sep 17 00:00:00 2001 From: Balduin Date: Tue, 10 Feb 2026 14:04:26 +0100 Subject: [PATCH] airspeed_fusion: continue fusing with timeout after exiting FW So that we don't rely purely on optical flow immediately after initiating a backtransition, when it can still be unreliable due to high speed. --- .../EKF/aid_sources/airspeed/airspeed_fusion.cpp | 13 +++++++++++-- src/modules/ekf2/EKF/ekf.h | 2 ++ 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 49d5b4f396..6c5d2ca269 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -66,6 +66,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _external_wind_init = false; } + if (_control_status.flags.fixed_wing) { + _time_last_fixed_wing = _airspeed_sample_delayed.time_us; + } + + #if defined(CONFIG_EKF2_GNSS) // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) @@ -93,8 +98,12 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - const bool continuing_conditions_passing = _control_status.flags.in_air && (_control_status.flags.fixed_wing - || _control_status.flags.in_transition_to_fw) + const bool was_recently_in_fixedwing = _airspeed_sample_delayed.time_us - _time_last_fixed_wing > (uint64_t) 3e6f; + const bool is_airspeed_accurate = _control_status.flags.fixed_wing || _control_status.flags.in_transition_to_fw + || was_recently_in_fixedwing; + + const bool continuing_conditions_passing = _control_status.flags.in_air + && is_airspeed_accurate && !_control_status.flags.fake_pos; const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 80ef94b9fc..bd66c8dc33 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -501,6 +501,8 @@ private: uint64_t _time_last_heading_fuse{0}; uint64_t _time_last_terrain_fuse{0}; + uint64_t _time_last_fixed_wing{0}; + LatLonAlt _last_known_gpos{}; Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s