From d7cd745cf4276dfc08e51474684c007d14d05e24 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 9 Oct 2014 22:42:47 +0900 Subject: [PATCH] Copter: auto-trim start delays auto-disarm by 15sec Fixes issue in which user only had 5 seconds after starting auto-trim to raise the throttle before the auto-disarm would kick-in. --- ArduCopter/motors.pde | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 70a5e1ba65..d031855541 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -5,6 +5,8 @@ #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define AUTO_DISARMING_DELAY 15 // called at 1hz so 15 seconds +static uint8_t auto_disarming_counter; + // arm_motors_check - checks for pilot input to arm or disarm the copter // called at 10hz static void arm_motors_check() @@ -68,6 +70,8 @@ static void arm_motors_check() // arm the motors and configure for flight if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) { auto_trim_counter = 250; + // ensure auto-disarm doesn't trigger immediately + auto_disarming_counter = 0; } // full left @@ -94,8 +98,6 @@ static void arm_motors_check() // called at 1hz static void auto_disarm_check() { - static uint8_t auto_disarming_counter; - // exit immediately if we are already disarmed or throttle is not zero if (!motors.armed() || g.rc_3.control_in > 0) { auto_disarming_counter = 0;