From 6524222397296085f377deb67a83344772b4604b Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Fri, 6 Nov 2015 01:30:11 -0800 Subject: [PATCH] Copter: adapt auto disarm check to use a timer --- ArduCopter/ArduCopter.cpp | 4 +--- ArduCopter/motors.cpp | 30 +++++++++++++----------------- 2 files changed, 14 insertions(+), 20 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index af7b6a03db..89ecbea34f 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -108,6 +108,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(update_batt_compass, 40, 120), SCHED_TASK(read_aux_switches, 40, 50), SCHED_TASK(arm_motors_check, 40, 50), + SCHED_TASK(auto_disarm_check, 40, 50), SCHED_TASK(auto_trim, 40, 75), SCHED_TASK(update_altitude, 40, 140), SCHED_TASK(run_nav_updates, 8, 100), @@ -475,9 +476,6 @@ void Copter::one_hz_loop() pre_arm_checks(false); } - // auto disarm checks - auto_disarm_check(); - if (!motors.armed()) { // make it possible to change ahrs orientation at runtime during initial config ahrs.set_orientation(); diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index bb9dc16d4a..2cd7514993 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -7,7 +7,7 @@ #define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds #define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second -static uint8_t auto_disarming_counter; +static uint32_t auto_disarm_begin; // arm_motors_check - checks for pilot input to arm or disarm the copter // called at 10hz @@ -43,7 +43,7 @@ void Copter::arm_motors_check() 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; + auto_disarm_begin = millis(); } // full left @@ -70,25 +70,24 @@ void Copter::arm_motors_check() } // auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds -// called at 1hz void Copter::auto_disarm_check() { - uint8_t disarm_delay = constrain_int16(g.disarm_delay, 0, 127); + uint32_t tnow_ms = millis(); + uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127); // exit immediately if we are already disarmed, or if auto // disarming is disabled - if (!motors.armed() || disarm_delay == 0) { - auto_disarming_counter = 0; + if (!motors.armed() || disarm_delay_ms == 0) { + auto_disarm_begin = tnow_ms; return; } // always allow auto disarm if using interlock switch or motors are Emergency Stopped if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) { - auto_disarming_counter++; #if FRAME_CONFIG != HELI_FRAME // use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less // obvious the copter is armed as the motors will not be spinning - disarm_delay /= 2; + disarm_delay_ms /= 2; #endif } else { bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0; @@ -100,19 +99,16 @@ void Copter::auto_disarm_check() thr_low = g.rc_3.control_in <= deadband_top; } - if (thr_low && ap.land_complete) { - // increment counter - auto_disarming_counter++; - } else { - // reset counter - auto_disarming_counter = 0; + if (!thr_low || !ap.land_complete) { + // reset timer + auto_disarm_begin = tnow_ms; } } - // disarm once counter expires - if (auto_disarming_counter >= disarm_delay) { + // disarm once timer expires + if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) { init_disarm_motors(); - auto_disarming_counter = 0; + auto_disarm_begin = tnow_ms; } }