diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c64fb2b0c5..3e28b5f8f8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1875,6 +1875,7 @@ bool set_throttle_mode( uint8_t new_throttle_mode ) void update_throttle_mode(void) { int16_t pilot_climb_rate; + int16_t pilot_throttle_scaled; if(ap.do_flip) // this is pretty bad but needed to flip in AP modes. return; @@ -1902,19 +1903,20 @@ void update_throttle_mode(void) set_throttle_out(0, false); }else{ // send pilot's output directly to motors - set_throttle_out(g.rc_3.control_in, false); + pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); + set_throttle_out(pilot_throttle_scaled, false); // update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME update_throttle_cruise(motors.coll_out); #else - update_throttle_cruise(g.rc_3.control_in); + update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME // check if we've taken off yet if (!ap.takeoff_complete && motors.armed()) { - if (g.rc_3.control_in > g.throttle_cruise) { + if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } @@ -1927,17 +1929,18 @@ void update_throttle_mode(void) if (g.rc_3.control_in <= 0) { set_throttle_out(0, false); // no need for angle boost with zero throttle }else{ - set_throttle_out(g.rc_3.control_in, true); + pilot_throttle_scaled = get_pilot_desired_throttle(g.rc_3.control_in); + set_throttle_out(pilot_throttle_scaled, true); // update estimate of throttle cruise #if FRAME_CONFIG == HELI_FRAME update_throttle_cruise(motors.coll_out); #else - update_throttle_cruise(g.rc_3.control_in); + update_throttle_cruise(pilot_throttle_scaled); #endif //HELI_FRAME if (!ap.takeoff_complete && motors.armed()) { - if (g.rc_3.control_in > g.throttle_cruise) { + if (pilot_throttle_scaled > g.throttle_cruise) { // we must be in the air by now set_takeoff_complete(true); } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index cc796ff706..a944af959f 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -833,10 +833,41 @@ get_throttle_accel(int16_t z_target_accel) return output; } +// get_pilot_desired_throttle - transform pilot's throttle input to make cruise throttle mid stick +// used only for manual throttle modes +// returns throttle output 0 to 1000 +#define THROTTLE_IN_MIDDLE 500 // the throttle mid point +static int16_t get_pilot_desired_throttle(int16_t throttle_control) +{ + int16_t throttle_out; + + // exit immediately in the simple cases + if( throttle_control == 0 || g.throttle_mid == 500) { + return throttle_control; + } + + // ensure reasonable throttle values + throttle_control = constrain(throttle_control,0,1000); + g.throttle_mid = constrain(g.throttle_mid,300,700); + + // check throttle is above, below or in the deadband + if (throttle_control < THROTTLE_IN_MIDDLE) { + // below the deadband + throttle_out = (float)throttle_control * (float)g.throttle_mid / 500.0f; + }else if(throttle_control > THROTTLE_IN_MIDDLE) { + // above the deadband + throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f; + }else{ + // must be in the deadband + throttle_out = g.throttle_mid; + } + + return throttle_out; +} + // get_pilot_desired_climb_rate - transform pilot's throttle input to // climb rate in cm/s. we use radio_in instead of control_in to get the full range // without any deadzone at the bottom -#define THROTTLE_IN_MIDDLE 500 // the throttle mid point #define THROTTLE_IN_DEADBAND 100 // the throttle input channel's deadband in PWM #define THROTTLE_IN_DEADBAND_TOP (THROTTLE_IN_MIDDLE+THROTTLE_IN_DEADBAND) // top of the deadband #define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-THROTTLE_IN_DEADBAND) // bottom of the deadband diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index fd5e5d184b..c69707a997 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -188,7 +188,8 @@ public: k_param_radio_tuning_high, k_param_radio_tuning_low, k_param_rc_speed = 192, - k_param_failsafe_battery_enabled, // 193 + k_param_failsafe_battery_enabled, + k_param_throttle_mid, // 194 // // 200: flight modes @@ -305,6 +306,7 @@ public: AP_Int8 failsafe_throttle; AP_Int16 failsafe_throttle_value; AP_Int16 throttle_cruise; + AP_Int16 throttle_mid; // Flight modes // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index a032f04f56..e444f2cb28 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -334,6 +334,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE), + // @Param: THR_MID + // @DisplayName: Throttle Mid Position + // @Description: The throttle output (0 ~ 1000) when throttle stick is in mid position. Used to scale the manual throttle so that the mid throttle stick position is close to the throttle required to hover + // @User: Standard + // @Range: 300 700 + // @Increment: 1 + GSCALAR(throttle_mid, "THR_MID", THR_MID), + // @Param: FLTMODE1 // @DisplayName: Flight Mode 1 // @Description: Flight mode when Channel 5 pwm is <= 1230 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4514dcd75d..6cd8a43de4 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -1005,6 +1005,10 @@ # define THROTTLE_CRUISE 450 // #endif +#ifndef THR_MID + # define THR_MID 500 // Throttle output (0 ~ 1000) when throttle stick is in mid position +#endif + #ifndef ALT_HOLD_P # define ALT_HOLD_P 2.0f #endif