From 9f03f5a9a957c9ed38e85df6db33f54ebb9fdf56 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 20 Jul 2021 17:45:05 +1000
Subject: [PATCH] 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 | 15 ++++++++++++---
 libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp |  1 +
 libraries/AP_NavEKF3/AP_NavEKF3_core.cpp         |  1 +
 libraries/AP_NavEKF3/AP_NavEKF3_core.h           |  3 +++
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
index a3eea6da17..b63bfcdcd2 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
+++ b/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);
     }
 }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
index b0c4307b64..e8c429dff5 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
+++ b/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);
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
index 0405046b57..b55495ceed 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
@@ -499,6 +499,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     readIMUData();
     readMagData();
     readGpsData();
+    readGpsYawData();
     readBaroData();
 
     if (statesInitialised) {
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index d5be194406..2ce09595a1 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/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();