diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index bfc553cb94..843b175ca7 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -59,8 +59,8 @@ static void althold_run() if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - // move throttle to minimum to keep us on the ground - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); }else{ // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index b12f12688d..298b3fcb43 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -243,8 +243,8 @@ static void autotune_run() if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - // move throttle to minimum to keep us on the ground - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); }else{ // check if pilot is overriding the controls if (target_roll != 0 || target_pitch != 0 || target_yaw_rate != 0.0f || target_climb_rate != 0) { diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 8d1575b45a..c5d46d47c7 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -72,7 +72,8 @@ static void loiter_run() wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); }else{ // run loiter controller wp_nav.update_loiter(); diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde index 23d1abf670..0edf8535be 100644 --- a/ArduCopter/control_ofloiter.pde +++ b/ArduCopter/control_ofloiter.pde @@ -70,7 +70,8 @@ static void ofloiter_run() if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); reset_optflow_I(); }else{ // mix in user control with optical flow diff --git a/ArduCopter/control_poshold.pde b/ArduCopter/control_poshold.pde index 6a5a262a1a..7134dd537b 100644 --- a/ArduCopter/control_poshold.pde +++ b/ArduCopter/control_poshold.pde @@ -188,7 +188,8 @@ static void poshold_run() wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); return; }else{ // convert pilot input to lean angles diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 179400af74..180a976cdd 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -78,8 +78,8 @@ static void sport_run() if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); attitude_control.set_yaw_target_to_current_heading(); - // move throttle to minimum to keep us on the ground - attitude_control.set_throttle_out(0, false); + // move throttle to between minimum and non-takeoff-throttle to keep us on the ground + attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false); }else{ // call attitude controller