diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index f47d7fa209..8bd7282b34 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -333,17 +333,21 @@ void Copter::guided_run() // called by guided_run at 100hz or more void Copter::guided_takeoff_run() { - // if not auto armed or motors not enabled set throttle to zero and exit immediately + // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) { + // initialise wpnav targets + wp_nav.shift_wp_origin_to_current_pos(); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw // call attitude controller attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); attitude_control.set_throttle_out(0,false,g.throttle_filt); -#else +#else // multicopters do not stabilize roll/pitch/yaw when disarmed motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); - // multicopters do not stabilize roll/pitch/yaw when disarmed + // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); #endif + // clear i term when we're taking off + set_throttle_takeoff(); return; } @@ -354,6 +358,18 @@ void Copter::guided_takeoff_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); } +#if FRAME_CONFIG == HELI_FRAME + // helicopters stay in landed state until rotor speed runup has finished + if (motors.rotor_runup_complete()) { + set_land_complete(false); + } else { + // initialise wpnav targets + wp_nav.shift_wp_origin_to_current_pos(); + } +#else + set_land_complete(false); +#endif + // set motors to full range motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);