AP_NavEKF2: Allow checking all cores for failure

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -768,6 +768,19 @@ bool NavEKF2::healthy(void) const
     return core[primary].healthy();
 }
 
+bool NavEKF2::all_cores_healthy(void) const
+{
+    if (!core) {
+        return false;
+    }
+    for (uint8_t i = 0; i < num_cores; i++) {
+        if (!core[i].healthy()) {
+            return false;
+        }
+    }
+    return true;
+}
+
 // returns the index of the primary core
 // return -1 if no primary core selected
 int8_t NavEKF2::getPrimaryCoreIndex(void) const
@@ -1361,7 +1374,13 @@ const char *NavEKF2::prearm_failure_reason(void) const
     if (!core) {
         return nullptr;
     }
-    return core[primary].prearm_failure_reason();
+    for (uint8_t i = 0; i < num_cores; i++) {
+        const char * failure = core[i].prearm_failure_reason();
+        if (failure != nullptr) {
+            return failure;
+        }
+    }
+    return nullptr;
 }
 
 // Returns the amount of vertical position change due to the last reset or core switch in metres

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -62,6 +62,8 @@ public:
     
     // Check basic filter health metrics and return a consolidated health status
     bool healthy(void) const;
+    // Ensure that all started cores are considered healthy
+    bool all_cores_healthy(void) const;
 
     // returns the index of the primary core
     // return -1 if no primary core selected