AP_AHRS: allow reporting of secondary AHRS solution

libraries/AP_AHRS/AP_AHRS.h

@@ -264,6 +264,12 @@ public:
     AP_Float _kp;
     AP_Float gps_gain;
 
+    // return secondary attitude solution if available, as eulers in radians
+    virtual bool get_secondary_attitude(Vector3f &eulers) { return false; }
+
+    // return secondary position solution if available
+    virtual bool get_secondary_position(struct Location &loc) { return false; }
+
 protected:
     // settable parameters
     AP_Float beta;

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -47,6 +47,10 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const
 void AP_AHRS_NavEKF::update(void)
 {
     AP_AHRS_DCM::update();
+
+    // keep DCM attitude available for get_secondary_attitude()
+    _dcm_attitude(roll, pitch, yaw);
+
     if (!ekf_started) {
         if (start_time_ms == 0) {
             start_time_ms = hal.scheduler->millis();
@@ -139,4 +143,39 @@ bool AP_AHRS_NavEKF::use_compass(void)
     return AP_AHRS_DCM::use_compass();
 }
 
+
+// return secondary attitude solution if available, as eulers in radians
+bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
+{
+    if (ekf_started && _ekf_use) {
+        // return DCM attitude
+        eulers = _dcm_attitude;
+        return true;
+    }
+    if (ekf_started) {
+        // EKF is secondary
+        EKF.getEulerAngles(eulers);
+        return true;
+    }
+    // no secondary available
+    return false;
+}
+
+// return secondary position solution if available
+bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
+{
+    if (ekf_started && _ekf_use) {
+        // return DCM position
+        AP_AHRS_DCM::get_position(loc);
+        return true;
+    }    
+    if (ekf_started) {
+        // EKF is secondary
+        EKF.getLLH(loc);
+        return true;
+    }
+    // no secondary available
+    return false;
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -73,11 +73,18 @@ public:
 
     const NavEKF &get_NavEKF(void) const { return EKF; }
 
+    // return secondary attitude solution if available, as eulers in radians
+    bool get_secondary_attitude(Vector3f &eulers);
+
+    // return secondary position solution if available
+    bool get_secondary_position(struct Location &loc);
+
 private:
     NavEKF EKF;
     AP_Baro &_baro;
     bool ekf_started;
     Matrix3f _dcm_matrix;
+    Vector3f _dcm_attitude;
     const uint16_t startup_delay_ms;
     uint32_t start_time_ms;
 };