Copter: Throw mode check altitude within params

ArduPilot · Jan 8, 2024 · 13cbe20 · 13cbe20
1 parent 635c764
commit 13cbe20
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 2 deletions.
diff --git 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 - 0 to disable the check
+    // @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
@@ -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
@@ -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 = (g.throw_altitude_min == 0 || 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)) {