From 7e7b7c9afb7615085d4329f6fe8186aa70228926 Mon Sep 17 00:00:00 2001
From: Rustom Jehangir <rustyj110@gmail.com>
Date: Mon, 4 Apr 2016 21:44:42 -0700
Subject: [PATCH] Sub: Add low pass filter (from Copter)

---
 ArduSub/Attitude.cpp     | 3 ---
 ArduSub/Sub.cpp          | 1 +
 ArduSub/Sub.h            | 3 +++
 ArduSub/control_auto.cpp | 8 ++++++++
 ArduSub/control_land.cpp | 8 ++++++++
 ArduSub/control_rtl.cpp  | 8 ++++++++
 6 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/ArduSub/Attitude.cpp b/ArduSub/Attitude.cpp
index f14b5a3694..8f61994669 100644
--- a/ArduSub/Attitude.cpp
+++ b/ArduSub/Attitude.cpp
@@ -126,9 +126,6 @@ void Sub::set_throttle_takeoff()
 {
     // tell position controller to reset alt target and reset I terms
     pos_control.init_takeoff();
-
-    // tell motors to do a slow start
-    motors.slow_start(true);
 }
 
 // get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick
diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp
index 653594d9d8..180fb05f2f 100644
--- a/ArduSub/Sub.cpp
+++ b/ArduSub/Sub.cpp
@@ -62,6 +62,7 @@ Sub::Sub(void) :
     baro_alt(0),
     baro_climbrate(0.0f),
     land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
+	rc_throttle_control_in_filter(1.0f),
     auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
     yaw_look_at_WP_bearing(0.0f),
     yaw_look_at_heading(0),
diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
index ea971eddc6..984bafb8fb 100644
--- a/ArduSub/Sub.h
+++ b/ArduSub/Sub.h
@@ -400,6 +400,9 @@ private:
     float baro_climbrate;        // barometer climbrate in cm/s
     LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
 
+    // filtered pilot's throttle input used to cancel landing if throttle held high
+    LowPassFilterFloat rc_throttle_control_in_filter;
+
     // 3D Location vectors
     // Current location of the Sub (altitude is relative to home)
     struct Location current_loc;
diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp
index 82baaa6ade..a5a0be1241 100644
--- a/ArduSub/control_auto.cpp
+++ b/ArduSub/control_auto.cpp
@@ -321,6 +321,14 @@ void Sub::auto_land_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+        
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
diff --git a/ArduSub/control_land.cpp b/ArduSub/control_land.cpp
index 187856f738..c136d801a6 100644
--- a/ArduSub/control_land.cpp
+++ b/ArduSub/control_land.cpp
@@ -84,6 +84,14 @@ void Sub::land_gps_run()
 
     // process pilot inputs
     if (!failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+        
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();
diff --git a/ArduSub/control_rtl.cpp b/ArduSub/control_rtl.cpp
index 01df7a192d..42a67a516c 100644
--- a/ArduSub/control_rtl.cpp
+++ b/ArduSub/control_rtl.cpp
@@ -290,6 +290,14 @@ void Sub::rtl_descent_run()
 
     // process pilot's input
     if (!failsafe.radio) {
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+            Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
+            // exit land if throttle is high
+            if (!set_mode(LOITER)) {
+                set_mode(ALT_HOLD);
+            }
+        }
+        
         if (g.land_repositioning) {
             // apply SIMPLE mode transform to pilot inputs
             update_simple_mode();