AP_AHRS_NavEKF: get_secondary_quat and position may return EKF3 value

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -741,22 +741,41 @@ bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const
 // return secondary attitude solution if available, as eulers in radians
 bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
 {
-    switch (active_EKF_type()) {
+    EKFType secondary_ekf_type;
+    if (!get_secondary_EKF_type(secondary_ekf_type)) {
+        return false;
+    }
+
+    switch (secondary_ekf_type) {
+
     case EKFType::NONE:
-        // EKF is secondary
+        // DCM is secondary
-        switch ((EKFType)_ekf_type.get()) {
+        eulers = _dcm_attitude;
-#if HAL_NAVEKF3_AVAILABLE
+        return true;
-        case EKFType::THREE:
+
-            EKF3.getEulerAngles(-1, eulers);
-            return _ekf3_started;
-#endif
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
+        // EKF2 is secondary
         EKF2.getEulerAngles(-1, eulers);
         return _ekf2_started;
 #endif
+
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+        // EKF3 is secondary
+        EKF3.getEulerAngles(-1, eulers);
+        return _ekf3_started;
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SITL:
+        // SITL is secondary (should never happen)
+        return false;
+#endif
+
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL: {
+        // External is secondary
         Quaternion quat;
         if (!AP::externalAHRS().get_quaternion(quat)) {
             return false;
@@ -765,27 +784,8 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
         return true;
     }
 #endif
-        default:
-            return false;
     }
-#if HAL_NAVEKF2_AVAILABLE
-    case EKFType::TWO:
-#endif
 
-#if HAL_NAVEKF3_AVAILABLE
-    case EKFType::THREE:
-#endif
-
-#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
-    case EKFType::SITL:
-#endif
-#if HAL_EXTERNAL_AHRS_ENABLED
-    case EKFType::EXTERNAL:
-#endif
-        // DCM is secondary
-        eulers = _dcm_attitude;
-        return true;
-    }
     // since there is no default case above, this is unreachable
     return false;
 }
@@ -794,36 +794,45 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const
 // return secondary attitude solution if available, as quaternion
 bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
 {
-    switch (active_EKF_type()) {
+    EKFType secondary_ekf_type;
-    case EKFType::NONE:
+    if (!get_secondary_EKF_type(secondary_ekf_type)) {
-        // EKF is secondary
-#if HAL_NAVEKF2_AVAILABLE
-        EKF2.getQuaternion(-1, quat);
-        return _ekf2_started;
-#elif HAL_EXTERNAL_AHRS_ENABLED
-        return AP::externalAHRS().get_quaternion(quat);
-#else
         return false;
-#endif
+    }
+
+    switch (secondary_ekf_type) {
+
+    case EKFType::NONE:
+        // DCM is secondary
+        quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
+        return true;
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
+        // EKF2 is secondary
+        EKF2.getQuaternion(-1, quat);
+        return _ekf2_started;
 #endif
 
 #if HAL_NAVEKF3_AVAILABLE
     case EKFType::THREE:
+        // EKF3 is secondary
+        EKF3.getQuaternion(-1, quat);
+        return _ekf3_started;
 #endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case EKFType::SITL:
+        // SITL is secondary (should never happen)
+        return false;
 #endif
+
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
+        // External is secondary
+        return AP::externalAHRS().get_quaternion(quat);
 #endif
-        // DCM is secondary
-        quat.from_rotation_matrix(AP_AHRS_DCM::get_rotation_body_to_ned());
-        return true;
     }
+
     // since there is no default case above, this is unreachable
     return false;
 }
@@ -831,36 +840,45 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const
 // return secondary position solution if available
 bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const
 {
-    switch (active_EKF_type()) {
+    EKFType secondary_ekf_type;
-    case EKFType::NONE:
+    if (!get_secondary_EKF_type(secondary_ekf_type)) {
-        // EKF is secondary
-#if HAL_NAVEKF2_AVAILABLE
-        EKF2.getLLH(loc);
-        return _ekf2_started;
-#elif HAL_EXTERNAL_AHRS_ENABLED
-        return AP::externalAHRS().get_location(loc);
-#else
         return false;
-#endif
+    }
+
+    switch (secondary_ekf_type) {
+
+    case EKFType::NONE:
+        // return DCM position
+        AP_AHRS_DCM::get_position(loc);
+        return true;
 
 #if HAL_NAVEKF2_AVAILABLE
     case EKFType::TWO:
+        // EKF2 is secondary
+        EKF2.getLLH(loc);
+        return _ekf2_started;
 #endif
 
 #if HAL_NAVEKF3_AVAILABLE
     case EKFType::THREE:
+        // EKF3 is secondary
+        EKF3.getLLH(loc);
+        return _ekf3_started;
 #endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     case EKFType::SITL:
+        // SITL is secondary (should never happen)
+        return false;
 #endif
+
 #if HAL_EXTERNAL_AHRS_ENABLED
     case EKFType::EXTERNAL:
+        // External is secondary
+        return AP::externalAHRS().get_location(loc);
 #endif
-        // return DCM position
-        AP_AHRS_DCM::get_position(loc);
-        return true;
     }
+
     // since there is no default case above, this is unreachable
     return false;
 }
@@ -1542,6 +1560,54 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const
     return ret;
 }
 
+// get secondary EKF type.  returns false if no secondary (i.e. only using DCM)
+bool AP_AHRS_NavEKF::get_secondary_EKF_type(EKFType &secondary_ekf_type) const
+{
+
+    switch (active_EKF_type()) {
+    case EKFType::NONE:
+        // EKF2, EKF3 or External is secondary
+#if HAL_NAVEKF3_AVAILABLE
+        if ((EKFType)_ekf_type.get() == EKFType::THREE) {
+            secondary_ekf_type = EKFType::THREE;
+            return true;
+        }
+#endif
+#if HAL_NAVEKF2_AVAILABLE
+        if ((EKFType)_ekf_type.get() == EKFType::TWO) {
+            secondary_ekf_type = EKFType::TWO;
+            return true;
+        }
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+        if ((EKFType)_ekf_type.get() == EKFType::EXTERNAL) {
+            secondary_ekf_type = EKFType::EXTERNAL;
+            return true;
+        }
+#endif
+        return false;
+
+#if HAL_NAVEKF2_AVAILABLE
+    case EKFType::TWO:
+#endif
+#if HAL_NAVEKF3_AVAILABLE
+    case EKFType::THREE:
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    case EKFType::SITL:
+#endif
+#if HAL_EXTERNAL_AHRS_ENABLED
+    case EKFType::EXTERNAL:
+#endif
+        // DCM is secondary
+        secondary_ekf_type = EKFType::NONE;
+        return true;
+    }
+
+    // since there is no default case above, this is unreachable
+    return false;
+}
+
 /*
   check if the AHRS subsystem is healthy
 */

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -348,6 +348,10 @@ private:
     };
     EKFType active_EKF_type(void) const;
 
+    // if successful returns true and sets secondary_ekf_type to None (for DCM), EKF3 or EKF3
+    // returns false if no secondary (i.e. only using DCM)
+    bool get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
+
     bool always_use_EKF() const {
         return _ekf_flags & FLAG_ALWAYS_USE_EKF;
     }