From 3ae0b0d4460c739fa3ca888106ab0024c10e0224 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 5 Apr 2020 09:53:49 +1000
Subject: [PATCH] AP_NavEKF3: added using_external_yaw() method

needed by AHRS for attitudes_consistent() check
---
 libraries/AP_NavEKF3/AP_NavEKF3.cpp         |  9 +++++++++
 libraries/AP_NavEKF3/AP_NavEKF3.h           |  3 +++
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 10 ++++++++++
 libraries/AP_NavEKF3/AP_NavEKF3_core.h      |  3 +++
 4 files changed, 25 insertions(+)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index b489b6b1f4..daff25c632 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -1237,6 +1237,15 @@ bool NavEKF3::use_compass(void) const
     return core[primary].use_compass();
 }
 
+// are we using an external yaw source? Needed for ahrs attitudes_consistent
+bool NavEKF3::using_external_yaw(void) const
+{
+    if (!core) {
+        return false;
+    }
+    return core[primary].using_external_yaw();
+}
+
 // write the raw optical flow measurements
 // rawFlowQuality is a measured of quality between 0 and 255, with 255 being the best quality
 // rawFlowRates are the optical flow rates in rad/sec about the X and Y sensor axes.
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h
index 1f7f24cbc8..d4b70b3e14 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.h
@@ -384,6 +384,9 @@ public:
     // write EKF information to on-board logs
     void Log_Write();
 
+    // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
+    bool using_external_yaw(void) const;
+    
 private:
     uint8_t num_cores; // number of allocated cores
     uint8_t primary;   // current primary core
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 2a782384d5..42b9d8cdae 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -456,6 +456,16 @@ bool NavEKF3_core::use_compass(void) const
     return effective_magCal() != MagCal::EXTERNAL_YAW && _ahrs->get_compass() && _ahrs->get_compass()->use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
 }
 
+// are we using an external yaw source? Needed for ahrs attitudes_consistent
+bool NavEKF3_core::using_external_yaw(void) const
+{
+    MagCal mag_cal = effective_magCal();
+    if (mag_cal != MagCal::EXTERNAL_YAW && mag_cal != MagCal::EXTERNAL_YAW_FALLBACK) {
+        return false;
+    }
+    return AP_HAL::millis() - last_gps_yaw_fusion_ms < 5000;
+}
+
 /*
   should we assume zero sideslip?
  */
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index acf995a7f3..d20658484f 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -384,6 +384,9 @@ public:
         EXTERNAL_YAW_FALLBACK = 6,
     };
 
+    // are we using an external yaw source? This is needed by AHRS attitudes_consistent check
+    bool using_external_yaw(void) const;
+    
 private:
     // Reference to the global EKF frontend for parameters
     NavEKF3 *frontend;