From 6fe0fa6d63ed20683295468153e1919b7d7d9f49 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 5 Jul 2024 10:56:25 -0400 Subject: [PATCH] ekf2: fake_pos don't use fuseHorizontalPosition helper - fake_pos shouldn't update _time_last_hor_pos_fuse --- src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index 3704434d5f..df7257d9b0 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -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);