diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 91c4249f2d..f9b946cac8 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -171,10 +171,8 @@ public: // Methods virtual void update(bool skip_ins_update=false) = 0; - // report any reason for why the backend is refusing to initialise - virtual const char *prearm_failure_reason(void) const { - return nullptr; - } + // returns false if we fail arming checks, in which case the buffer will be populated with a failure message + virtual bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const = 0; // check all cores providing consistent attitudes for prearm checks virtual bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { return true; } @@ -482,8 +480,6 @@ public: // is the AHRS subsystem healthy? virtual bool healthy(void) const = 0; - virtual bool prearm_healthy(void) const { return healthy(); } - // true if the AHRS has completed initialisation virtual bool initialised(void) const { return true; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 9f433c8e22..c84f8c4f77 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1142,3 +1142,12 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const return true; } +// returns false if we fail arming checks, in which case the buffer will be populated with a failure message +bool AP_AHRS_DCM::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const +{ + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy"); + return false; + } + return true; +} diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index f0364ae9a7..43dfcc990c 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -120,6 +120,9 @@ public: bool get_velocity_NED(Vector3f &vec) const override; + // 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 override; + private: float _ki; float _ki_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index cf1257187c..997a349b18 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -1344,34 +1344,42 @@ bool AP_AHRS_NavEKF::healthy(void) const return AP_AHRS_DCM::healthy(); } -bool AP_AHRS_NavEKF::prearm_healthy(void) const +// returns false if we fail arming checks, in which case the buffer will be populated with a failure message +bool AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const { - bool prearm_health = false; switch (ekf_type()) { #if CONFIG_HAL_BOARD == HAL_BOARD_SITL case EKFType::SITL: #endif case EKFType::NONE: - prearm_health = true; - break; + if (!healthy()) { + hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy"); + return false; + } + return true; #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: - if (_ekf2_started && EKF2.all_cores_healthy()) { - prearm_health = true; + if (!_ekf2_started) { + hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 not started"); + return false; } - break; + return EKF2.pre_arm_check(failure_msg, failure_msg_len); #endif #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: - if (_ekf3_started && EKF3.all_cores_healthy()) { - prearm_health = true; + if (!_ekf3_started) { + hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started"); + return false; } - break; + return EKF3.pre_arm_check(failure_msg, failure_msg_len); #endif } - return prearm_health && healthy(); + + // if we get here then ekf type is invalid + hal.util->snprintf(failure_msg, failure_msg_len, "invalid EKF type"); + return false; } void AP_AHRS_NavEKF::set_ekf_use(bool setting) @@ -1617,32 +1625,6 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons ret.z += GRAVITY_MSS*dt; } -// report any reason for why the backend is refusing to initialise -const char *AP_AHRS_NavEKF::prearm_failure_reason(void) const -{ - switch (ekf_type()) { - case EKFType::NONE: - return nullptr; - -#if HAL_NAVEKF2_AVAILABLE - case EKFType::TWO: - return EKF2.prearm_failure_reason(); -#endif - -#if HAL_NAVEKF3_AVAILABLE - case EKFType::THREE: - return EKF3.prearm_failure_reason(); -#endif - -#if CONFIG_HAL_BOARD == HAL_BOARD_SITL - case EKFType::SITL: - return nullptr; -#endif - } - - return nullptr; -} - // check all cores providing consistent attitudes for prearm checks bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index fcb5b90a0f..b21ef5237c 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -200,7 +200,8 @@ public: // is the AHRS subsystem healthy? bool healthy() const override; - bool prearm_healthy() const override; + // 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 override; // true if the AHRS has completed initialisation bool initialised() const override; @@ -212,9 +213,6 @@ public: // true if offsets are valid bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const; - // report any reason for why the backend is refusing to initialise - const char *prearm_failure_reason(void) const override; - // check all cores providing consistent attitudes for prearm checks bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const override;