diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 91fb26b128..4ac969f0ae 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -124,27 +124,23 @@ set_throttle_takeoff() // 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; - } + int16_t mid_stick = g.rc_3.get_control_mid(); // ensure reasonable throttle values throttle_control = constrain_int16(throttle_control,0,1000); g.throttle_mid = constrain_int16(g.throttle_mid,300,700); // check throttle is above, below or in the deadband - if (throttle_control < THROTTLE_IN_MIDDLE) { + if (throttle_control < mid_stick) { // below the deadband - throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(500-g.throttle_min)); - }else if(throttle_control > THROTTLE_IN_MIDDLE) { + throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(g.throttle_mid - g.throttle_min))/((float)(mid_stick-g.throttle_min)); + }else if(throttle_control > mid_stick) { // above the deadband - throttle_out = g.throttle_mid + ((float)(throttle_control-500))*(float)(1000-g.throttle_mid)/500.0f; + throttle_out = g.throttle_mid + ((float)(throttle_control-mid_stick))*(float)(1000-g.throttle_mid)/mid_stick; }else{ // must be in the deadband throttle_out = g.throttle_mid; @@ -156,8 +152,6 @@ static int16_t get_pilot_desired_throttle(int16_t throttle_control) // 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_DEADBAND_TOP (THROTTLE_IN_MIDDLE+g.throttle_deadzone) // top of the deadband -#define THROTTLE_IN_DEADBAND_BOTTOM (THROTTLE_IN_MIDDLE-g.throttle_deadzone) // bottom of the deadband static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) { int16_t desired_rate = 0; @@ -167,19 +161,24 @@ static int16_t get_pilot_desired_climb_rate(int16_t throttle_control) return 0; } + int16_t mid_stick = g.rc_3.get_control_mid(); + int16_t deadband_top = mid_stick + g.throttle_deadzone; + int16_t deadband_bottom = mid_stick - g.throttle_deadzone; + + // ensure a reasonable throttle value - throttle_control = constrain_int16(throttle_control,0,1000); + throttle_control = constrain_int16(throttle_control,g.throttle_min,1000); // ensure a reasonable deadzone g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400); // check throttle is above, below or in the deadband - if (throttle_control < THROTTLE_IN_DEADBAND_BOTTOM) { + if (throttle_control < deadband_bottom) { // below the deadband - desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_BOTTOM) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone); - }else if (throttle_control > THROTTLE_IN_DEADBAND_TOP) { + desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-deadband_bottom) / (deadband_bottom-g.throttle_min); + }else if (throttle_control > deadband_top) { // above the deadband - desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-THROTTLE_IN_DEADBAND_TOP) / (THROTTLE_IN_MIDDLE - g.throttle_deadzone); + desired_rate = (int32_t)g.pilot_velocity_z_max * (throttle_control-deadband_top) / (1000-deadband_top); }else{ // must be in the deadband desired_rate = 0; @@ -209,6 +208,9 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control) return 0; } + 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); @@ -221,8 +223,8 @@ static int16_t get_throttle_pre_takeoff(int16_t throttle_control) } // check throttle is below top of deadband - if (throttle_control < THROTTLE_IN_DEADBAND_TOP) { - throttle_out = g.throttle_min + ((float)(throttle_control-g.throttle_min))*((float)(get_non_takeoff_throttle() - g.throttle_min))/((float)(THROTTLE_IN_DEADBAND_TOP-g.throttle_min)); + 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();