fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 16:18:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: start airspeed fusion when test ratio is passing only

Browse Source 
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.
This commit is contained in:
bresch
2022-07-07 17:39:06 -04:00
committed by Daniel Agar
parent e1933f6ade
commit 2f486c37fc
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/control.cpp
+3 -1
View File
@@ -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) {