From b0b78e974bbe90837a55930b0d879a8299ded09b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 1 Apr 2020 10:08:54 +0900
Subject: [PATCH] AP_AHRS: add get_quaternion

---
 libraries/AP_AHRS/AP_AHRS.h          |  2 ++
 libraries/AP_AHRS/AP_AHRS_DCM.cpp    |  7 +++++++
 libraries/AP_AHRS/AP_AHRS_DCM.h      |  3 +++
 libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 30 ++++++++++++++++++++++++++++
 libraries/AP_AHRS/AP_AHRS_NavEKF.h   |  3 +++
 5 files changed, 45 insertions(+)

diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h
index c8e69e0459..89f98d2ed2 100644
--- a/libraries/AP_AHRS/AP_AHRS.h
+++ b/libraries/AP_AHRS/AP_AHRS.h
@@ -392,6 +392,8 @@ public:
         return _sin_yaw;
     }
 
+    // return the quaternion defining the rotation from NED to XYZ (body) axes
+    virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;
 
     // return secondary attitude solution if available, as eulers in radians
     virtual bool get_secondary_attitude(Vector3f &eulers) const WARN_IF_UNUSED {
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
index 7d2c9e1150..fe94e93fb0 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp
@@ -473,6 +473,13 @@ bool AP_AHRS_DCM::use_compass(void)
     return true;
 }
 
+// return the quaternion defining the rotation from NED to XYZ (body) axes
+bool AP_AHRS_DCM::get_quaternion(Quaternion &quat) const
+{
+    quat.from_rotation_matrix(_dcm_matrix);
+    return true;
+}
+
 // yaw drift correction using the compass or GPS
 // this function prodoces the _omega_yaw_P vector, and also
 // contributes to the _omega_I.z long term yaw drift estimate
diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h
index 2d995c4c4a..bb20df4b71 100644
--- a/libraries/AP_AHRS/AP_AHRS_DCM.h
+++ b/libraries/AP_AHRS/AP_AHRS_DCM.h
@@ -96,6 +96,9 @@ public:
 
     bool            use_compass() override;
 
+    // return the quaternion defining the rotation from NED to XYZ (body) axes
+    bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
+
     bool set_home(const Location &loc) override WARN_IF_UNUSED;
     void estimate_wind(void);
 
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
index a67a3bcd4f..0a7c2f7006 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
@@ -545,6 +545,36 @@ bool AP_AHRS_NavEKF::use_compass(void)
     return AP_AHRS_DCM::use_compass();
 }
 
+// return the quaternion defining the rotation from NED to XYZ (body) axes
+bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const
+{
+    switch (active_EKF_type()) {
+    case EKFType::NONE:
+        return AP_AHRS_DCM::get_quaternion(quat);
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+        EKF2.getQuaternion(-1, quat);
+        return _ekf2_started;
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        EKF3.getQuaternion(-1, quat);
+        return _ekf3_started;
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SITL:
+        if (_sitl) {
+            const struct SITL::sitl_fdm &fdm = _sitl->state;
+            quat = fdm.quaternion;
+            return true;
+        } else {
+            return false;
+        }
+#endif
+    }
+    // since there is no default case above, this is unreachable
+    return false;
+}
 
 // return secondary attitude solution if available, as eulers in radians
 bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
index 7101f1ca8c..4682e1d02f 100644
--- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h
+++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h
@@ -115,6 +115,9 @@ public:
     }
 #endif
 
+    // return the quaternion defining the rotation from NED to XYZ (body) axes
+    bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
+
     // return secondary attitude solution if available, as eulers in radians
     bool get_secondary_attitude(Vector3f &eulers) const override;