From 6caf4f0942b3312244ef5ab17e787a0e44a02a66 Mon Sep 17 00:00:00 2001 From: Balduin Date: Wed, 26 Nov 2025 16:29:19 +0100 Subject: [PATCH] ekf2: fuse airspeed & beta only in fronttransition and fixed wing (#25980) This allows airspeed and sideslip fusion to start during VTOL front transition, but not in backtransition or MC. This mitigates issues seen due to going in and out of airspeed fusion in strong headwinds in MC. Co-authored-by: bresch --- src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp | 3 ++- src/modules/ekf2/EKF/common.h | 3 +++ src/modules/ekf2/EKF/control.cpp | 1 + src/modules/ekf2/EKF/estimator_interface.h | 2 ++ src/modules/ekf2/EKF2.cpp | 1 + 5 files changed, 9 insertions(+), 1 deletion(-) 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 2e324663de..245c12cd69 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -96,7 +96,8 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag - const bool continuing_conditions_passing = _control_status.flags.in_air + const bool continuing_conditions_passing = _control_status.flags.in_air && (_control_status.flags.fixed_wing + || _control_status.flags.in_transition_to_fw) && !_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/common.h b/src/modules/ekf2/EKF/common.h index c8d61754aa..57fcb0d859 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -267,6 +267,7 @@ struct systemFlagUpdate { bool is_fixed_wing{false}; bool gnd_effect{false}; bool constant_pos{false}; + bool in_transition_to_fw{false}; }; struct parameters { @@ -611,6 +612,8 @@ uint64_t gnss_fault : uint64_t yaw_manual : 1; ///< 46 - true if yaw has been reset manually uint64_t gnss_hgt_fault : 1; ///< 47 - true if GNSS measurements (alt) have been declared faulty and are no longer used + uint64_t in_transition_to_fw : 1; ///< 48 - true if the vehicle is in transition to fw + } flags; uint64_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 0998f86b43..95ee21c014 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -58,6 +58,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) set_in_air_status(system_flags_delayed.in_air); set_is_fixed_wing(system_flags_delayed.is_fixed_wing); + set_in_transition_to_fw(system_flags_delayed.in_transition_to_fw); if (system_flags_delayed.gnd_effect) { set_gnd_effect(); diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index b87472f919..0c3f9bf7be 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -200,6 +200,8 @@ public: // set vehicle is fixed wing status void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; } + void set_in_transition_to_fw(bool in_transition) { _control_status.flags.in_transition_to_fw = in_transition; } + // set flag if static pressure rise due to ground effect is expected // use _params.ekf2_gnd_eff_dz to adjust for expected rise in static pressure // flag will clear after GNDEFFECT_TIMEOUT uSec diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 7a59a0e7a1..c82c447686 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -2622,6 +2622,7 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps) // let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind) flags.is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + flags.in_transition_to_fw = vehicle_status.in_transition_to_fw; #if defined(CONFIG_EKF2_SIDESLIP)