AP_NavEKF3: 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_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -664,10 +664,6 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -284,6 +284,12 @@ void NavEKF3_core::SelectVelPosFusion()
         posVelFusionDelayed = false;
     }
 
+    // Read GPS data from the sensor
+    readGpsData();
+
+    // get data that has now fallen behind the fusion time horizon
+    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) {
 

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -631,20 +631,21 @@ void NavEKF3_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 or external yaw sensor 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();