AP_NavEKF3: process GPS yaw independently of GPS fix

this processes GPS yaw with its own timestamp and as a separated step
from fusing position and velocity. This makes the yaw time handling
more accurate as yaw for moving baseline GPS comes in as a separate
piece of data from the position and velocity

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -701,17 +701,26 @@ void NavEKF3_core::readGpsData()
         // declare GPS available for use
         gpsNotAvailable = false;
     }
+}
 
-    // if the GPS has yaw data then input that as well
+// check for new valid GPS yaw data
+void NavEKF3_core::readGpsYawData()
+{
+    const auto &gps = dal.gps();
+
+    // if the GPS has yaw data then fuse it as an Euler yaw angle
     float yaw_deg, yaw_accuracy_deg;
-    if (dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg)) {
+    uint32_t yaw_time_ms;
+    if (gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_3D &&
+        dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg, yaw_time_ms) &&
+        yaw_time_ms != yawMeasTime_ms) {
         // GPS modules are rather too optimistic about their
         // accuracy. Set to min of 5 degrees here to prevent
         // the user constantly receiving warnings about high
         // normalised yaw innovations
         const ftype min_yaw_accuracy_deg = 5.0f;
         yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
-        writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
+        writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), yaw_time_ms, 2);
     }
 }
 

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -445,6 +445,7 @@ void NavEKF3_core::SelectVelPosFusion()
 
     // Read GPS data from the sensor
     readGpsData();
+    readGpsYawData();
 
     // get data that has now fallen behind the fusion time horizon
     gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -499,6 +499,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     readIMUData();
     readMagData();
     readGpsData();
+    readGpsYawData();
     readBaroData();
 
     if (statesInitialised) {

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -696,6 +696,9 @@ private:
     // check for new valid GPS data and update stored measurement if available
     void readGpsData();
 
+    // check for new valid GPS yaw data
+    void readGpsYawData();
+
     // check for new altitude measurement data and update stored measurement if available
     void readBaroData();