From b10cf0f38a58c4a9475a27ae7e756212e4c48d38 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Thu, 30 Apr 2015 15:04:17 +0900 Subject: [PATCH] Copter: pre-takeoff throttle for sprung throttle sticks Adds PILOT_THR_BHV, PILOT_TKOFF_DZ parameters --- ArduCopter/Attitude.pde | 47 ++++++++++++++++++++------------------- ArduCopter/Parameters.h | 8 +++++-- ArduCopter/Parameters.pde | 15 +++++++++++++ 3 files changed, 45 insertions(+), 25 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index ae81b95ef9..f5b46d9b00 100644 --- a/ArduCopter/Attitude.pde +++ b/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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3a53c8558c..984f7547a1 100644 --- a/ArduCopter/Parameters.h +++ b/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; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 4f6d5542f5..c8c7419a13 100644 --- a/ArduCopter/Parameters.pde +++ b/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),