From 75721018c6070d897cbe3e4835368c0e871a772d Mon Sep 17 00:00:00 2001
From: JanMaciuk <38810823+JanMaciuk@users.noreply.github.com>
Date: Tue, 12 Dec 2023 12:04:41 +0100
Subject: [PATCH] Copter: Throw mode user defined altitude limits

Allow the user to set altitude restrictions in which a throw can be detected via parameters.
---
 ArduCopter/Parameters.cpp | 14 ++++++++++++++
 ArduCopter/Parameters.h   |  4 ++++
 ArduCopter/mode_throw.cpp | 19 ++++++++++++++++---
 3 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
index 80a79da030a7c..a16346e57218f 100644
--- a/ArduCopter/Parameters.cpp
+++ b/ArduCopter/Parameters.cpp
@@ -696,6 +696,20 @@ const AP_Param::Info Copter::var_info[] = {
     // @Values: 0:Stopped,1:Running
     // @User: Standard
     GSCALAR(throw_motor_start, "THROW_MOT_START", (float)ModeThrow::PreThrowMotorState::STOPPED),
+
+    // @Param: THROW_ALT_MIN
+    // @DisplayName: Throw mode minimum altitude
+    // @Description: Minimum altitude above which Throw mode will detect a throw or a drop
+    // @Units: m
+    // @User: Advanced
+    GSCALAR(throw_altitude_min, "THROW_ALT_MIN", 0),
+
+    // @Param: THROW_ALT_MAX
+    // @DisplayName: Throw mode maximum altitude
+    // @Description: Maximum altitude under which Throw mode will detect a throw or a drop - 0 to disable the check
+    // @Units: m
+    // @User: Advanced
+    GSCALAR(throw_altitude_max, "THROW_ALT_MAX", 0),
 #endif
 
 #if OSD_ENABLED || OSD_PARAM_ENABLED
diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
index e59bf2238788b..3d8b62b96d936 100644
--- a/ArduCopter/Parameters.h
+++ b/ArduCopter/Parameters.h
@@ -383,6 +383,8 @@ class Parameters {
         // 254,255: reserved
 
         k_param_vehicle = 257, // vehicle common block of parameters
+        k_param_throw_altitude_min,
+        k_param_throw_altitude_max,
 
         // the k_param_* space is 9-bits in size
         // 511: reserved
@@ -463,6 +465,8 @@ class Parameters {
 
 #if MODE_THROW_ENABLED == ENABLED
     AP_Enum<ModeThrow::PreThrowMotorState>         throw_motor_start;
+    AP_Int16         throw_altitude_min; // minimum altitude in m above which a throw can be detected
+    AP_Int16         throw_altitude_max; // maximum altitude in m below which a throw can be detected
 #endif
 
     AP_Int16                rc_speed; // speed of fast RC Channels in Hz
diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp
index c9ff702e3fd88..e7b96bff92904 100644
--- a/ArduCopter/mode_throw.cpp
+++ b/ArduCopter/mode_throw.cpp
@@ -272,8 +272,21 @@ bool ModeThrow::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 = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
 
-    // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
-    bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action;
+    // fetch the altitude above home
+    float altitude_above_home;  // Use altitude above home if it is set, otherwise relative to EKF origin
+    if (ahrs.home_is_set()) {
+        ahrs.get_relative_position_D_home(altitude_above_home);
+        altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
+    }
+    else {
+        altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
+    }
+
+    // Check that the altitude is within user defined limits
+    const bool height_within_params = (altitude_above_home > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home < g.throw_altitude_max));
+
+    // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release  
+    bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
 
     // Record time and vertical velocity when we detect the possible throw
     if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
@@ -283,7 +296,7 @@ bool ModeThrow::throw_detected()
 
     // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
     bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f));
-
+    
     // start motors and enter the control mode if we are in continuous freefall
     return throw_condition_confirmed;
 }