AP_AHRS: replace prearm_healthy with pre_arm_check
Also removes prearm_failure_reason
This commit is contained in:
parent
02daa4c3ec
commit
1d97416a51
@ -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;
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
{
|
||||
|
@ -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;
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user