mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-13 04:33:33 +08:00
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.
This commit is contained in:
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user