diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 5d89b65306..9b4886e81f 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -804,6 +804,19 @@ bool NavEKF3::healthy(void) const return core[primary].healthy(); } +bool NavEKF3::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 NavEKF3::getPrimaryCoreIndex(void) const @@ -1447,7 +1460,13 @@ const char *NavEKF3::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[primary].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 diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index d1e6bbd485..51398e00f0 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -59,6 +59,8 @@ public: // Check basic filter health metrics and return a consolidated health status bool healthy(void) const; + // Check that all cores are started and healthy + bool all_cores_healthy(void) const; // returns the index of the primary core // return -1 if no primary core selected