AP_NavEKF: Enable clean entry into GPS aiding whilst in-flight

Ensures that the latest GPS data is used to reset the states.
Separates the logic used to set the origin from the logic used to determine when to reset states and commence GPS aiding

libraries/AP_NavEKF/AP_NavEKF_core.cpp

@@ -3968,29 +3968,33 @@ void NavEKF_core::readGpsData()
             // Monitor qulaity of GPS data inflight
             calcGpsGoodForFlight();
 
-            // read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
+            // Read the GPS locaton in WGS-84 lat,long,height coordinates
-            // If we don't have an origin, then set it to the current GPS coordinates
             const struct Location &gpsloc = _ahrs->get_gps().location();
+
+            // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
+            if (!validOrigin && gpsGoodToAlign) {
+                // 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 - hgtMea;
+                }
+            }
+
+            // Commence GPS aiding when able to
+            if ((frontend._fusionModeGPS != 3) && (PV_AidingMode != AID_ABSOLUTE) && vehicleArmed && gpsGoodToAlign) {
+                PV_AidingMode = AID_ABSOLUTE;
+                constPosMode = false;
+                // Initialise EKF position and velocity states to last GPS measurement
+                ResetPosition();
+                ResetVelocity();
+            }
+
+            // Convert to local coordinates if we have an origin.
             if (validOrigin) {
                 gpsPosNE = 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 - hgtMea;
-                // We are by definition at the origin at the instant of alignment so set NE position to zero
-                gpsPosNE.zero();
-                // If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode
-                if (vehicleArmed && frontend._fusionModeGPS != 3) {
-                    constPosMode = false;
-                    PV_AidingMode = AID_ABSOLUTE;
-                    gpsNotAvailable = false;
-                    // Initialise EKF position and velocity states
-                    ResetPosition();
-                    ResetVelocity();
-                }
             }
         } else {
              // report GPS fix status