AP_AHRS: make AHRS2 quaternion available

libraries/AP_AHRS/AP_AHRS.h

@@ -420,6 +420,11 @@ public:
         return false;
     }
 
+    // return secondary attitude solution if available, as quaternion
+    virtual bool get_secondary_quaternion(Quaternion &quat) {
+        return false;
+    }
+    
     // return secondary position solution if available
     virtual bool get_secondary_position(struct Location &loc) {
         return false;

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -269,7 +269,7 @@ void AP_AHRS_NavEKF::update_SITL(void)
         pitch = radians(fdm.pitchDeg);
         yaw   = radians(fdm.yawDeg);
 
-        _dcm_matrix.from_euler(roll, pitch, yaw);
+        fdm.quaternion.rotation_matrix(_dcm_matrix);
 
         update_cd_values();
         update_trig();
@@ -461,6 +461,27 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
     }
 }
 
+
+// return secondary attitude solution if available, as quaternion
+bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat)
+{
+    switch (active_EKF_type()) {
+    case EKF_TYPE_NONE:
+        // EKF is secondary
+        EKF2.getQuaternion(-1, quat);
+        return _ekf2_started;
+
+    case EKF_TYPE2:
+
+    case EKF_TYPE3:
+
+    default:
+        // DCM is secondary
+        quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
+        return true;
+    }
+}
+
 // return secondary position solution if available
 bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
 {

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -103,6 +103,9 @@ public:
     // return secondary attitude solution if available, as eulers in radians
     bool get_secondary_attitude(Vector3f &eulers) override;
 
+    // return secondary attitude solution if available, as quaternion
+    bool get_secondary_quaternion(Quaternion &quat) override;
+    
     // return secondary position solution if available
     bool get_secondary_position(struct Location &loc) override;