From a776b2c549b8a6317ed4e2c6564710241bd78985 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 14 Nov 2016 19:25:36 +1100
Subject: [PATCH] EKF: Fix bug preventing reset to GPS position when using
 optical flow

---
 EKF/control.cpp | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

diff --git a/EKF/control.cpp b/EKF/control.cpp
index 07726c7a5d..e72e9ef4d2 100644
--- a/EKF/control.cpp
+++ b/EKF/control.cpp
@@ -329,21 +329,21 @@ void Ekf::controlGpsFusion()
 
 				// If the heading is valid start using gps aiding
 				if (_control_status.flags.yaw_align) {
-					_control_status.flags.gps = true;
-					_time_last_gps = _time_last_imu;
-
 					// if we are not already aiding with optical flow, then we need to reset the position and velocity
+					// otherwise we only need to reset the position
+					_control_status.flags.gps = true;
 					if (!_control_status.flags.opt_flow) {
-						if (resetPosition() && resetVelocity()) {
-							_control_status.flags.gps = true;
-
-						} else {
+						if (!resetPosition() || !resetVelocity()) {
 							_control_status.flags.gps = false;
 
 						}
+					} else if (!resetPosition()) {
+						_control_status.flags.gps = false;
+
 					}
 					if (_control_status.flags.gps) {
 						ECL_INFO("EKF commencing GPS aiding");
+					       _time_last_gps = _time_last_imu;
 
 					}
 				}