From b8c74b7363f80b57f7ae0fbae3bc7965b0944462 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 18 Sep 2014 17:24:40 +0900 Subject: [PATCH] Copter: define limit for baro vs inav alt disparity --- ArduCopter/config.h | 5 +++++ ArduCopter/motors.pde | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index a1159c0adc..628bf53e1c 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -299,6 +299,11 @@ #define FS_GCS_ENABLED_ALWAYS_RTL 1 #define FS_GCS_ENABLED_CONTINUE_MISSION 2 +// pre-arm baro vs inertial nav max alt disparity +#ifndef PREARM_MAX_ALT_DISPARITY_CMS + # define PREARM_MAX_ALT_DISPARITY_CMS 100 // barometer and inertial nav altitude must be within this many centimeters +#endif + // pre-arm check max velocity #ifndef PREARM_MAX_VELOCITY_CMS # define PREARM_MAX_VELOCITY_CMS 50.0f // vehicle must be travelling under 50cm/s before arming diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 2f3681ac8b..21f2efd734 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -536,7 +536,7 @@ static bool arm_checks(bool display_failure) // check Baro & inav alt are within 1m if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) { - if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) { + if(fabs(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CMS) { if (display_failure) { gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity")); }