AP_AHRS: add get_quaternion

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 {

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

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);
 

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

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;