From 4268759d4a0660800ecbcaedcc6d4fbb489b5909 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 19 Jan 2024 14:21:46 +0100
Subject: [PATCH] ekf2-agp: reset to measurement on fusion timeout

---
 src/modules/ekf2/EKF/aux_global_position.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/modules/ekf2/EKF/aux_global_position.cpp b/src/modules/ekf2/EKF/aux_global_position.cpp
index 539643513c..269ba29e44 100644
--- a/src/modules/ekf2/EKF/aux_global_position.cpp
+++ b/src/modules/ekf2/EKF/aux_global_position.cpp
@@ -119,6 +119,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 			if (continuing_conditions) {
 				ekf.fuseHorizontalPosition(aid_src);
 
+				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)) {
+					ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
+					aid_src.time_last_fuse = imu_delayed.time_us;
+				}
+
 			} else {
 				ekf.disableControlStatusAuxGpos();
 				_state = State::stopped;