AP_NavEKF2: fixed loss of GPS fusion

we must not do a storedGPS.recall unless we will be using the data,
otherwise we will lose GPS samples and risk stopping GPS fusion

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -616,10 +616,6 @@ void NavEKF2_core::readGpsData()
             hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
         }
     }
-
-    // get data that has now fallen behind the fusion time horizon
-    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
-
 }
 
 // read the delta angle and corresponding time interval from the IMU

libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@@ -315,6 +315,10 @@ void NavEKF2_core::SelectVelPosFusion()
     // Check for data at the fusion time horizon
     extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
 
+    // read GPS data from the sensor and check for new data in the buffer
+    readGpsData();
+    gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
+
     // Determine if we need to fuse position and velocity data on this time step
     if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
         // set fusion request flags

libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

@@ -588,20 +588,21 @@ void NavEKF2_core::UpdateFilter(bool predict)
         // Predict the covariance growth
         CovariancePrediction();
 
-        // Read GPS data from the sensor
-        // This is required by multiple processes so needs to be done before other fusion steps
-        readGpsData();
-
-        // Generate an alternative yaw estimate used for inflight recovery from bad compass data
-        // requires horizontal GPS velocity
-        runYawEstimator();
-
         // Update states using  magnetometer data
         SelectMagFusion();
 
         // Update states using GPS and altimeter data
         SelectVelPosFusion();
 
+        // Generate an alternative yaw estimate used for inflight
+        // recovery from bad compass data requires horizontal GPS
+        // velocity. We only do this when posVelFusionDelayed is false
+        // as otherwise there will be no new GPS data, as the
+        // sttoredGPS recall will have been skipped
+        if (!posVelFusionDelayed) {
+            runYawEstimator();
+        }
+
         // Update states using range beacon data
         SelectRngBcnFusion();