diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 41038740c2..915f78c311 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1030,13 +1030,21 @@ bool NavEKF3::healthy(void) const return core[primary].healthy(); } -bool NavEKF3::all_cores_healthy(void) const +// returns false if we fail arming checks, in which case the buffer will be populated with a failure message +bool NavEKF3::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { if (!core) { + hal.util->snprintf(failure_msg, failure_msg_len, "no EKF3 cores"); return false; } for (uint8_t i = 0; i < num_cores; i++) { if (!core[i].healthy()) { + const char *failure = core[primary].prearm_failure_reason(); + if (failure != nullptr) { + hal.util->snprintf(failure_msg, failure_msg_len, failure); + } else { + hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 core %d unhealthy", (int)i); + } return false; } } @@ -1796,21 +1804,6 @@ uint32_t NavEKF3::getLastVelNorthEastReset(Vector2f &vel) const return core[primary].getLastVelNorthEastReset(vel); } -// report the reason for why the backend is refusing to initialise -const char *NavEKF3::prearm_failure_reason(void) const -{ - if (!core) { - return "no EKF3 cores"; - } - 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 // Returns the time of the last reset or 0 if no reset or core switch has ever occurred // Where there are multiple consumers, they must access this function on the same frame as each other diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index cc77ed4e8a..e09ce64ef6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -59,8 +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 false if we fail arming checks, in which case the buffer will be populated with a failure message + bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const; // Update instance error scores for all available cores float updateCoreErrorScores(void); @@ -398,9 +398,6 @@ public: // returns the time of the last reset or 0 if no reset has ever occurred uint32_t getLastPosDownReset(float &posDelta); - // report any reason for why the backend is refusing to initialise - const char *prearm_failure_reason(void) const; - // set and save the _baroAltNoise parameter void set_baro_alt_noise(float noise) { _baroAltNoise.set_and_save(noise); }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp index a5a693fa2f..9954ea10cc 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp @@ -13,7 +13,7 @@ extern const AP_HAL::HAL& hal; Monitor magnetometer innovations to see if the heading is good enough to use GPS Return true if all criteria pass for 10 seconds - We also record the failure reason so that prearm_failure_reason() + We also record the failure reason so that pre_arm_check() can give a good report to the user on why arming is failing */ void NavEKF3_core::calcGpsGoodToAlign(void)