From c83e2d7c0e83f588313406e098d6d4be606a4cae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 23 Apr 2020 15:21:40 +1000
Subject: [PATCH] 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
---
 .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp      |  4 ----
 .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp      |  4 ++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.cpp        | 17 +++++++++--------
 3 files changed, 13 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
index e44a5f33a9..6a26f9046d 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index 946a76a61a..7ce5592b55 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/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
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
index be86cfafee..7fa2383b7b 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
+++ b/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();