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)