From 0d8cf00c5a0443663a70f9300d2e8d713c64457e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 14 Oct 2021 19:48:04 +1100
Subject: [PATCH] AP_NavEKF2: add accessor for GSF yaw estimator

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp    | 9 +++++++++
 libraries/AP_NavEKF2/AP_NavEKF2.h      | 4 ++++
 libraries/AP_NavEKF2/AP_NavEKF2_core.h | 3 +++
 3 files changed, 16 insertions(+)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 10a6639479..cdc3fcd978 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -1595,3 +1595,12 @@ void NavEKF2::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSt
         }
     }
 }
+
+// get a yaw estimator instance
+const EKFGSF_yaw *NavEKF2::get_yawEstimator(void) const
+{
+    if (core) {
+        return core[primary].get_yawEstimator();
+    }
+    return nullptr;
+}
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index 2952184c7b..a1f5b1a123 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -29,6 +29,7 @@
 #include <AP_NavEKF/AP_Nav_Common.h>
 
 class NavEKF2_core;
+class EKFGSF_yaw;
 
 class NavEKF2 {
     friend class NavEKF2_core;
@@ -305,6 +306,9 @@ public:
     // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
     void writeDefaultAirSpeed(float airspeed);
 
+    // get a yaw estimator instance
+    const EKFGSF_yaw *get_yawEstimator(void) const;
+    
 private:
     uint8_t num_cores; // number of allocated cores
     uint8_t primary;   // current primary core
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
index 1b36283990..e4035ae286 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h
@@ -327,6 +327,9 @@ public:
 
     void Log_Write(uint64_t time_us);
 
+    // get a yaw estimator instance
+    const EKFGSF_yaw *get_yawEstimator(void) const { return yawEstimator; }
+    
 private:
     EKFGSF_yaw *yawEstimator;
     AP_DAL &dal;