diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index cb79a6fe7c..48c2f289de 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -353,7 +353,7 @@ static union { uint8_t motor_test : 1; // 24 // true if we are currently performing the motors test uint8_t initialised : 1; // 25 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes uint8_t land_complete_maybe : 1; // 26 // true if we may have landed (less strict version of land_complete) - uint8_t throttle_zero : 1; // 27 // true if the throttle stick is at zero, debounced + uint8_t throttle_zero : 1; // 27 // true if the throttle stick is at zero, debounced, determines if pilot intends shut-down when not using motor interlock uint8_t system_time_set : 1; // 28 // true if the system time has been set from the GPS uint8_t gps_base_pos_set : 1; // 29 // true when the gps base position has been set (used for RTK gps only) enum HomeState home_state : 2; // 30,31 // home status (unset, set, locked) diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 3b08a0cc59..64bcb5b2fb 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -19,7 +19,7 @@ static void acro_run() int16_t pilot_throttle_scaled; // if motors not running reset angle targets - if(!motors.armed() || g.rc_3.control_in <= 0) { + if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 2396b444d8..46e7067562 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -233,7 +233,7 @@ static bool autotune_start(bool ignore_checks) } // ensure throttle is above zero - if (g.rc_3.control_in <= 0) { + if (ap.throttle_zero) { return false; } diff --git a/ArduCopter/control_flip.pde b/ArduCopter/control_flip.pde index c82dd87bf0..1b048f3a6f 100644 --- a/ArduCopter/control_flip.pde +++ b/ArduCopter/control_flip.pde @@ -45,7 +45,7 @@ static bool flip_init(bool ignore_checks) } // if in acro or stabilize ensure throttle is above zero - if ((g.rc_3.control_in <= 0) && (control_mode == ACRO || control_mode == STABILIZE)) { + if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) { return false; } diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 64ccb1dc77..08841ee307 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -26,7 +26,7 @@ static void sport_run() float takeoff_climb_rate = 0.0f; // if not armed or throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || g.rc_3.control_in <= 0) { + if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(g.rc_3.control_in)-throttle_average); return; diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index 4640ce15a0..722a16904b 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -24,7 +24,7 @@ static void stabilize_run() int16_t pilot_throttle_scaled; // if not armed or throttle at zero, set throttle to zero and exit immediately - if(!motors.armed() || g.rc_3.control_in <= 0) { + if(!motors.armed() || ap.throttle_zero) { attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); return; } diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde index b6a5ad7e74..64f3cefec3 100644 --- a/ArduCopter/radio.pde +++ b/ArduCopter/radio.pde @@ -172,14 +172,19 @@ static void set_throttle_and_failsafe(uint16_t throttle_pwm) } #define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400 -// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control_in +// set_throttle_zero_flag - set throttle_zero flag from debounced throttle control +// throttle_zero is used to determine if the pilot intends to shut down the motors +// Basically, this signals when we are not flying. We are either on the ground +// or the pilot has shut down the copter in the air and it is free-falling static void set_throttle_zero_flag(int16_t throttle_control) { static uint32_t last_nonzero_throttle_ms = 0; uint32_t tnow_ms = millis(); - // if non-zero throttle immediately set as non-zero - if (throttle_control > 0) { + // if not using throttle interlock and non-zero throttle and not E-stopped, + // or using motor interlock and it's enabled, then motors are running, + // and we are flying. Immediately set as non-zero + if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_estop) || (ap.using_interlock && ap.motor_interlock)) { last_nonzero_throttle_ms = tnow_ms; ap.throttle_zero = false; } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {