fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 14:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: fake_pos don't use fuseHorizontalPosition helper

Browse Source 
- fake_pos shouldn't update _time_last_hor_pos_fuse
This commit is contained in:
Daniel Agar
2024-07-05 10:56:25 -04:00
parent c5c27a87f1
commit 6fe0fa6d63
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp
+9 -1
View File
@@ -87,7 +87,15 @@ void Ekf::controlFakePosFusion()
if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
) {
fuseHorizontalPosition(aid_src);
if (!aid_src.innovation_rejected
&& fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
State::pos.idx + 0)
&& fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
State::pos.idx + 1)
) {
aid_src.fused = true;
aid_src.time_last_fuse = _time_delayed_us;
}
}
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);