From 9ced648479d348a0a3fee653f11abcb55b29314f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 8 Oct 2014 21:52:16 +0900
Subject: [PATCH] Copter: throttle zero debounce to separate function

Also initialise throttle_zero  to true on startup
Treat throttle less than zero as zero
---
 ArduCopter/radio.pde | 29 ++++++++++++++++++++---------
 1 file changed, 20 insertions(+), 9 deletions(-)

diff --git a/ArduCopter/radio.pde b/ArduCopter/radio.pde
index 35cc823002..b1e78803ea 100644
--- a/ArduCopter/radio.pde
+++ b/ArduCopter/radio.pde
@@ -38,6 +38,9 @@ static void init_rc_in()
 
     // set default dead zones
     default_dead_zones();
+
+    // initialise throttle_zero flag
+    ap.throttle_zero = true;
 }
 
  // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
@@ -95,11 +98,9 @@ void output_min()
     motors.output_min();
 }
 
-#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
 static void read_radio()
 {
     static uint32_t last_update_ms = 0;
-    static uint32_t last_nonzero_throttle_ms = 0;
     uint32_t tnow_ms = millis();
 
     if (hal.rcin->new_input()) {
@@ -111,6 +112,7 @@ static void read_radio()
         g.rc_2.set_pwm(periods[rcmap.pitch()-1]);
 
         set_throttle_and_failsafe(periods[rcmap.throttle()-1]);
+        set_throttle_zero_flag(g.rc_3.control_in);
 
         g.rc_4.set_pwm(periods[rcmap.yaw()-1]);
         g.rc_5.set_pwm(periods[4]);
@@ -118,13 +120,6 @@ static void read_radio()
         g.rc_7.set_pwm(periods[6]);
         g.rc_8.set_pwm(periods[7]);
 
-        if (g.rc_3.control_in != 0) {
-            last_nonzero_throttle_ms = tnow_ms;
-            ap.throttle_zero = false;
-        } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
-            ap.throttle_zero = true;
-        }
-
         // read channels 9 ~ 14
         for (uint8_t i=8; i<RC_MAX_CHANNELS; i++) {
             if (RC_Channel::rc_channel(i) != NULL) {
@@ -192,6 +187,22 @@ 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
+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) {
+        last_nonzero_throttle_ms = tnow_ms;
+        ap.throttle_zero = false;
+    } else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
+        ap.throttle_zero = true;
+    }
+}
+
 static void trim_radio()
 {
     for (uint8_t i = 0; i < 30; i++) {