From a8983d075fee1d41e5af96abb00c4e2693e6a65c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 6 Nov 2015 09:48:21 +1100
Subject: [PATCH] AP_NavEKF2: Enable clean entry into GPS aiding in-flight

The setting of the EKF origin and the entry into GPS aiding mode have been separated to make the logic clear.
The order of operations has been changed to ensure that when a reset to GPS is performed, a valid GPS measurement is available in the buffer
Declaration of GPS availability is not made unless the GPS data has been entered into the buffer
---
 .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp    | 49 ++++++++++---------
 1 file changed, 26 insertions(+), 23 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index a1665a21ff..b583de891d 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
@@ -432,34 +432,37 @@ void NavEKF2_core::readGpsData()
                 calcGpsGoodForFlight();
             }
 
-            // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
-            // If we don't have an origin, then set it to the current GPS coordinates
+            // Read the GPS locaton in WGS-84 lat,long,height coordinates
             const struct Location &gpsloc = _ahrs->get_gps().location();
-            if (validOrigin) {
-                gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
-            } else if (gpsGoodToAlign) {
-                // Set the NE origin to the current GPS position
-                setOrigin();
-                // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
-                alignMagStateDeclination();
-                // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
-                EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
-                // We are by definition at the origin at the instant of alignment so set NE position to zero
-                gpsDataNew.pos.zero();
-                // If GPS useage isn't explicitly prohibited, we switch to absolute position mode
-                if (isAiding && frontend->_fusionModeGPS != 3) {
-                    PV_AidingMode = AID_ABSOLUTE;
-                    // Initialise EKF position and velocity states
-                    ResetPosition();
-                    ResetVelocity();
+
+            // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
+            if (gpsGoodToAlign && !validOrigin) {
+                // Set the NE origin to the current GPS position if not previously set
+                if (!validOrigin) {
+                    setOrigin();
+                    // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
+                    alignMagStateDeclination();
+                    // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
+                    EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
                 }
             }
 
-            // save measurement to buffer to be fused later
-            StoreGPS();
+            // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
+            if (validOrigin) {
+                gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
+                StoreGPS();
+                // declare GPS available for use
+                gpsNotAvailable = false;
+            }
+
+            // Commence GPS aiding when able to
+            if (readyToUseGPS() && PV_AidingMode != AID_ABSOLUTE) {
+                PV_AidingMode = AID_ABSOLUTE;
+                // Initialise EKF position and velocity states to last GPS measurement
+                ResetPosition();
+                ResetVelocity();
+            }
 
-            // declare GPS available for use
-            gpsNotAvailable = false;
         } else {
             // report GPS fix status
             gpsCheckStatus.bad_fix = true;