diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 6cf01238d0..c599f91cd1 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -652,6 +652,9 @@ void ModeGuided::set_angle(const Quaternion &attitude_quat, const Vector3f &ang_ // check we are in velocity control mode if (guided_mode != SubMode::Angle) { angle_control_start(); + } else if (!use_thrust && guided_angle_state.use_thrust) { + // Already angle control but changing from thrust to climb rate + pos_control->init_z_controller(); } guided_angle_state.attitude_quat = attitude_quat;