Copter: pre-takeoff throttle for sprung throttle sticks

Adds PILOT_THR_BHV, PILOT_TKOFF_DZ parameters

ArduCopter/Attitude.pde

@@ -188,42 +188,43 @@ static float get_non_takeoff_throttle()
     return (g.throttle_mid / 2.0f);
 }
 
+static float get_takeoff_trigger_throttle()
+{
+    return g.rc_3.get_control_mid() + g.takeoff_trigger_dz;
+}
+
 // get_throttle_pre_takeoff - convert pilot's input throttle to a throttle output before take-off
 // used only for althold, loiter, hybrid flight modes
 // returns throttle output 0 to 1000
-static int16_t get_throttle_pre_takeoff(int16_t throttle_control)
+static float get_throttle_pre_takeoff(float input_thr)
 {
-    int16_t throttle_out;
-
-    // exit immediately if throttle_control is zero
-    if (throttle_control <= 0) {
+    // exit immediately if input_thr is zero
+    if (input_thr <= 0) {
         return 0;
     }
 
-    // calculate mid stick and deadband
-    int16_t mid_stick = g.rc_3.get_control_mid();
-    int16_t deadband_top = mid_stick + g.throttle_deadzone;
-
-    // sanity check throttle input
-    throttle_control = constrain_int16(throttle_control,0,1000);
-
-    // sanity check throttle_mid
+    // TODO: does this parameter sanity check really belong here?
     g.throttle_mid = constrain_int16(g.throttle_mid,300,700);
 
-    // sanity check throttle_min vs throttle_mid
-    if (g.throttle_min > get_non_takeoff_throttle()) {
-        return g.throttle_min;
+    float in_min = g.throttle_min;
+    float in_max = get_takeoff_trigger_throttle();
+
+    float out_min = motors.get_throttle_warn();
+    float out_max = get_non_takeoff_throttle();
+
+    if ((g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0) {
+        in_min = g.rc_3.get_control_mid();
     }
 
-    // check throttle is below top of deadband
-    if (throttle_control < deadband_top) {
-        throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(get_non_takeoff_throttle() - g.throttle_min))/((float)(deadband_top-g.throttle_min));
-    }else{
-        // must be in the deadband
-        throttle_out = get_non_takeoff_throttle();
+    float input_range = in_max-in_min;
+    float output_range = out_max-out_min;
+
+    // sanity check ranges
+    if (input_range <= 0.0f || output_range <= 0.0f) {
+        return 0;
     }
 
-    return throttle_out;
+    return constrain_float(out_min + (input_thr-in_min)*output_range/input_range, out_min, out_max);
 }
 
 // get_surface_tracking_climb_rate - hold copter at the desired distance above the ground

ArduCopter/Parameters.h

@@ -127,8 +127,9 @@ public:
         k_param_optflow,
         k_param_dcmcheck_thresh,        // deprecated - remove
         k_param_log_bitmask,
-        k_param_cli_enabled,            // 61
-        k_param_throttle_filt,          // 62
+        k_param_cli_enabled,
+        k_param_throttle_filt,
+        k_param_throttle_behavior,
 
         // 65: AP_Limits Library
         k_param_limits = 65,            // deprecated - remove
@@ -190,6 +191,7 @@ public:
         k_param_ch10_option,
         k_param_ch11_option,
         k_param_ch12_option,     // 123
+        k_param_takeoff_trigger_dz,
 
         //
         // 140: Sensor parameters
@@ -341,6 +343,8 @@ public:
 #endif
 
     AP_Float        throttle_filt;
+    AP_Int16        throttle_behavior;
+    AP_Int16        takeoff_trigger_dz;
 
     AP_Int16        rtl_altitude;
     AP_Float        sonar_gain;

ArduCopter/Parameters.pde

@@ -71,6 +71,21 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Increment: .5
     GSCALAR(throttle_filt,  "PILOT_THR_FILT",     0),
 
+    // @Param: PILOT_TKOFF_DZ
+    // @DisplayName: Takeoff trigger deadzone
+    // @Description: Offset from mid stick at which takeoff is triggered
+    // @User: Standard
+    // @Range 0.0 500.0
+    // @Increment: 10
+    GSCALAR(takeoff_trigger_dz, "PILOT_TKOFF_DZ", THR_DZ_DEFAULT),
+
+    // @Param: PILOT_THR_BHV
+    // @DisplayName: Throttle stick behavior
+    // @Description: Bits for: Feedback starts from mid stick
+    // @User: Standard
+    // @Values: 0:None,1:FeedbackFromMid
+    GSCALAR(throttle_behavior, "PILOT_THR_BHV", 0),
+
     // @Group: SERIAL
     // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
     GOBJECT(serial_manager, "SERIAL",   AP_SerialManager),