From 3b1827ca2175d7a62a0d23c67ddd0fe06b2dc21c Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Wed, 5 Apr 2017 14:43:43 -0400
Subject: [PATCH] Sub: Remove throttle-zero flag

Doesn't help us like in Copter
---
 ArduSub/Sub.h        |  3 ---
 ArduSub/failsafe.cpp | 21 ---------------------
 ArduSub/radio.cpp    | 24 ------------------------
 ArduSub/system.cpp   |  4 ++--
 4 files changed, 2 insertions(+), 50 deletions(-)

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index d35b43a1ce..d9a71f2072 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -236,7 +236,6 @@ private:
             uint8_t compass_mot         : 1; // true if we are currently performing compassmot calibration
             uint8_t motor_test          : 1; // true if we are currently performing the motors test
             uint8_t initialised         : 1; // true once the init_ardupilot function has completed.  Extended status to GCS is not sent until this completes
-            uint8_t throttle_zero       : 1; // true if the throttle stick is at zero
             uint8_t system_time_set     : 1; // true if the system time has been set from the GPS
             uint8_t gps_base_pos_set    : 1; // true when the gps base position has been set (used for RTK gps only)
             enum HomeState home_state   : 2; // home status (unset, set, locked)
@@ -621,7 +620,6 @@ private:
     bool ekf_over_threshold();
     void failsafe_ekf_event();
     void failsafe_ekf_off_event(void);
-    bool should_disarm_on_failsafe();
     void failsafe_battery_check(void);
     void failsafe_gcs_check();
     void failsafe_manual_control_check(void);
@@ -683,7 +681,6 @@ private:
     void handle_jsbutton_press(uint8_t button,bool shift=false,bool held=false);
     JSButton* get_button(uint8_t index);
     void default_js_buttons(void);
-    void set_throttle_zero_flag(int16_t throttle_control);
     void init_barometer(bool save);
     void read_barometer(void);
     void init_rangefinder(void);
diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
index 77fe0302ca..66f9329e63 100644
--- a/ArduSub/failsafe.cpp
+++ b/ArduSub/failsafe.cpp
@@ -424,24 +424,3 @@ void Sub::failsafe_terrain_act()
         init_disarm_motors();
     }
 }
-
-bool Sub::should_disarm_on_failsafe()
-{
-    switch (control_mode) {
-    case STABILIZE:
-    case ACRO:
-        // if throttle is zero OR vehicle is landed disarm motors
-        return ap.throttle_zero;
-        break;
-    case AUTO:
-        // if mission has not started AND vehicle is landed, disarm motors
-        return !ap.auto_armed;
-        break;
-    default:
-        // used for AltHold, Guided, Loiter, RTL, Circle, Drift, Sport, Flip, PosHold
-        // if landed disarm
-        //            return ap.land_complete;
-        return false;
-        break;
-    }
-}
diff --git a/ArduSub/radio.cpp b/ArduSub/radio.cpp
index a994b354c8..9752a91119 100644
--- a/ArduSub/radio.cpp
+++ b/ArduSub/radio.cpp
@@ -54,9 +54,6 @@ void Sub::init_rc_in()
         }
     }
 #endif
-
-    // initialise throttle_zero flag
-    ap.throttle_zero = true;
 }
 
 // init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
@@ -84,27 +81,6 @@ void Sub::enable_motor_output()
     motors.output_min();
 }
 
-#define THROTTLE_ZERO_DEBOUNCE_TIME_MS 400
-// 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 vehicle in the air and it is free-falling
-void Sub::set_throttle_zero_flag(int16_t throttle_control)
-{
-    static uint32_t last_nonzero_throttle_ms = 0;
-    uint32_t tnow_ms = millis();
-
-    // 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 < 475 || throttle_control > 525) &&  !ap.motor_emergency_stop) || (ap.using_interlock && motors.get_interlock())) {
-        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;
-    }
-}
-
 // save_trim - adds roll and pitch trims from the radio to ahrs
 void Sub::save_trim()
 {
diff --git a/ArduSub/system.cpp b/ArduSub/system.cpp
index 4207ce01da..25c6067d3d 100644
--- a/ArduSub/system.cpp
+++ b/ArduSub/system.cpp
@@ -312,8 +312,8 @@ void Sub::update_auto_armed()
             set_auto_armed(false);
             return;
         }
-        // if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
-        if (mode_has_manual_throttle(control_mode) && ap.throttle_zero && !failsafe.manual_control) {
+        // if in stabilize or acro flight mode auto-armed should become false
+        if (mode_has_manual_throttle(control_mode) && !failsafe.manual_control) {
             set_auto_armed(false);
         }
     } else {