From cc27fb46bffd7618ac20e6390d12c571aa359bff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 29 Jul 2015 16:17:10 +0900 Subject: [PATCH] Copter: arming check that baro is healthy --- ArduCopter/motors.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index d2db9dee0e..4f7ecaf7a0 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -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")); }