From 93eb1266f6580284f684165ee8d31071610612a7 Mon Sep 17 00:00:00 2001
From: Jan Brehmer <jan.brehmer@dai-labor.de>
Date: Tue, 30 Jul 2019 19:23:56 +0200
Subject: [PATCH] EKF: reset position when stopping GPS use and EV is active

---
 EKF/control.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index ef92c4d9a1..aa5ae33cad 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -616,6 +616,10 @@ void Ekf::controlGpsFusion()
 		// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
 		if (_control_status.flags.gps  && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
 			_control_status.flags.gps = false;
+			// Reset position state to external vision if we are going to use absolute values
+			if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) {
+				resetPosition();
+			}
 			ECL_WARN("EKF GPS data quality poor - stopping use");
 		}