Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_AHRS: AHRS itself must also be healthy to arm, not just the "backend"

Browse Source
This commit is contained in:
Peter Barker 2020-12-10 22:14:12 +11:00 committed by Andrew Tridgell
parent 813723d0b7
commit ca41a19072
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -1372,16 +1372,20 @@ bool AP_AHRS_NavEKF::healthy(void) const
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message // 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 AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const
{ {
bool ret = true;
if (!healthy()) {
// this rather generic failure might be overwritten by
// something more specific in the "backend"
hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy");
ret = false;
}
switch (ekf_type()) { switch (ekf_type()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SITL: case EKFType::SITL:
#endif #endif
case EKFType::NONE: case EKFType::NONE:
if (!healthy()) { return ret;
hal.util->snprintf(failure_msg, failure_msg_len, "Not healthy");
return false;
}
return true;
#if HAL_NAVEKF2_AVAILABLE #if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: case EKFType::TWO:
@ -1389,7 +1393,7 @@ bool AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) c
hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 not started"); hal.util->snprintf(failure_msg, failure_msg_len, "EKF2 not started");
return false; return false;
} }
return EKF2.pre_arm_check(failure_msg, failure_msg_len); return EKF2.pre_arm_check(failure_msg, failure_msg_len) && ret;
#endif #endif
#if HAL_NAVEKF3_AVAILABLE #if HAL_NAVEKF3_AVAILABLE
@ -1398,7 +1402,7 @@ bool AP_AHRS_NavEKF::pre_arm_check(char *failure_msg, uint8_t failure_msg_len) c
hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started"); hal.util->snprintf(failure_msg, failure_msg_len, "EKF3 not started");
return false; return false;
} }
return EKF3.pre_arm_check(failure_msg, failure_msg_len); return EKF3.pre_arm_check(failure_msg, failure_msg_len) && ret;
#endif #endif
} }