From 25940c8e0f89267169128ef087f44d738e4a389f Mon Sep 17 00:00:00 2001
From: chambana <akwatson@comcast.net>
Date: Tue, 2 Aug 2016 11:26:48 +0900
Subject: [PATCH] Copter: add THROW_TYPE and allow dropping vehicle to trigger
 motors

---
 ArduCopter/Parameters.cpp    |  7 +++++++
 ArduCopter/Parameters.h      |  3 ++-
 ArduCopter/control_throw.cpp | 19 ++++++++++++++-----
 ArduCopter/defines.h         |  6 ++++++
 4 files changed, 29 insertions(+), 6 deletions(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index 4156df3c34..94c61d18aa 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -981,6 +981,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("THROW_NEXTMODE", 3, ParametersG2, throw_nextmode, 18),
 
+    // @Param: THROW_TYPE
+    // @DisplayName: Type of Type
+    // @Description: Used by THROW mode. Specifies whether Copter is thrown upward or dropped.
+    // @Values: 0:Upward Throw,1:Drop
+    // @User: Standard
+    AP_GROUPINFO("THROW_TYPE", 4, ParametersG2, throw_type, ThrowType_Upward),
+
     AP_GROUPEND
 };
 
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index 452cd8ddbc..7d1d048ed8 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -551,8 +551,9 @@ public:
     // button checking
     AP_Button button;
 
-    // Throw mode state
+    // Throw mode parameters
     AP_Int8 throw_nextmode;
+    AP_Int8 throw_type;
 };
 
 extern const AP_Param::Info        var_info[];
diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp
index bd8d69ce1a..e3dafd1c55 100644
--- a/ArduCopter/control_throw.cpp
+++ b/ArduCopter/control_throw.cpp
@@ -97,7 +97,11 @@ void Copter::throw_run()
 
         // initialise the demanded height to 3m above the throw height
         // we want to rapidly clear surrounding obstacles
-        pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
+        if (g2.throw_type == ThrowType_Drop) {
+            pos_control.set_alt_target(inertial_nav.get_altitude() - 100);
+        } else {
+            pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
+        }
 
         // set the initial velocity of the height controller demand to the measured velocity if it is going up
         // if it is going down, set it to zero to enforce a very hard stop
@@ -200,8 +204,13 @@ bool Copter::throw_detected()
     // Check for high speed (note get_inertial_nav methods use a cm length scale)
     bool high_speed = inertial_nav.get_velocity().length() > 500.0f;
 
-    // check for upwards trajectory
-    bool gaining_height = inertial_nav.get_velocity().z > 50.0f;
+    // check for upwards or downwards trajectory (airdrop)
+    bool changing_height;
+    if (g2.throw_type == ThrowType_Drop) {
+        changing_height = inertial_nav.get_velocity().z < -50.0f;
+    } else {
+        changing_height = inertial_nav.get_velocity().z > 50.0f;
+    }
 
     // Check the vertical acceleraton is greater than 0.25g
     bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
@@ -209,8 +218,8 @@ bool Copter::throw_detected()
     // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
     bool no_throw_action = ins.get_accel().length() < 1.0f * GRAVITY_MSS;
 
-    // High velocity or free-fall combined with incresing height indicate a possible throw release
-    bool possible_throw_detected = (free_falling || high_speed) && gaining_height && no_throw_action;
+    // High velocity or free-fall combined with incresing height indicate a possible air-drop or throw release
+    bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;
 
     // Record time and vertical velocity when we detect the possible throw
     if (possible_throw_detected && ((AP_HAL::millis() - throw_free_fall_start_ms) > 500)) {
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 55465dbe6e..ce5afec63b 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -259,6 +259,12 @@ enum ThrowModeStage {
     Throw_PosHold
 };
 
+// Throw types
+enum ThrowModeType {
+    ThrowType_Upward = 0,
+    ThrowType_Drop = 1
+};
+
 // LAND state
 #define LAND_STATE_FLY_TO_LOCATION  0
 #define LAND_STATE_DESCENDING       1