Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: arming check that baro is healthy

This commit is contained in:
Randy Mackay 2015-07-29 16:17:10 +09:00
parent 11c9e46ec7
commit cc27fb46bf
1 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ArduCopter/motors.cpp
View File

@ -730,13 +730,21 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
return true;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) && using_baro_ref) {
if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
// baro checks
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// baro health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Barometer not healthy"));
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref && (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM)) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Altitude disparity"));
}