Copter: Throw mode add params for target ascent alt after throw

hendjoshsr71 · Feb 17, 2024 · e7151c3 · e7151c3
1 parent 9986fb9
commit e7151c3
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 4 deletions.
diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp
@@ -706,6 +706,20 @@ const AP_Param::Info Copter::var_info[] = {
     // @Units: m
     // @User: Advanced
     GSCALAR(throw_altitude_max, "THROW_ALT_MAX", 0),
+
+    // @Param: THROW_ALT_DCSND
+    // @DisplayName: Throw mode target altitude to descend
+    // @Description: Target altitude to descend during a drop, (must be positive)
+    // @Units: m
+    // @User: Advanced
+    GSCALAR(throw_altitude_min, "THROW_ALT_DCSND", 100),
+
+    // @Param: THROW_ALT_ACSND
+    // @DisplayName: Throw mode target altitude to ascsend
+    // @Description: Target altitude to ascend during a throw upwards (must be positive)
+    // @Units: m
+    // @User: Advanced
+    GSCALAR(throw_altitude_max, "THROW_ALT_ACSND", 300),
 #endif
 
 #if OSD_ENABLED || OSD_PARAM_ENABLED

diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h
@@ -385,6 +385,8 @@ class Parameters {
         k_param_vehicle = 257, // vehicle common block of parameters
         k_param_throw_altitude_min,
         k_param_throw_altitude_max,
+        k_param_throw_altitude_descent,
+        k_param_throw_altitude_ascent,
 
         // the k_param_* space is 9-bits in size
         // 511: reserved
@@ -467,6 +469,9 @@ class Parameters {
     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
+
+    AP_Float         throw_altitude_descent; // target altitude (meters) to descend during a drop, (must be positive)
+    AP_Float         throw_altitude_ascent; // target altitude (meters) to ascend during a throw upwards (must be positive)
 #endif
 
     AP_Int16                rc_speed; // speed of fast RC Channels in Hz

diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp
@@ -69,12 +69,12 @@ void ModeThrow::run()
         // initialise the z controller
         pos_control->init_z_controller_no_descent();
 
-        // initialise the demanded height to 3m above the throw height
-        // we want to rapidly clear surrounding obstacles
+        // initialise the demanded height below/above the throw height from user parameters
+        // this allows for rapidly clearing surrounding obstacles
         if (g2.throw_type == ThrowType::Drop) {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100);
+            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - throw_altitude_descent);
         } else {
-            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300);
+            pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + throw_altitude_ascent);
         }
 
         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum