diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index 7ea3de38cb..ea0a8f0f65 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -122,10 +122,14 @@ void ModeZigZag::return_to_manual_control(bool maintain_target) if (stage == AUTO) { stage = MANUAL_REGAIN; loiter_nav->clear_pilot_desired_acceleration(); - const Vector3f wp_dest = wp_nav->get_wp_destination(); - loiter_nav->init_target(wp_dest); - if (maintain_target && wp_nav->origin_and_destination_are_terrain_alt()) { - copter.surface_tracking.set_target_alt_cm(wp_dest.z); + if (maintain_target) { + const Vector3f wp_dest = wp_nav->get_wp_destination(); + loiter_nav->init_target(wp_dest); + if (wp_nav->origin_and_destination_are_terrain_alt()) { + copter.surface_tracking.set_target_alt_cm(wp_dest.z); + } + } else { + loiter_nav->init_target(); } gcs().send_text(MAV_SEVERITY_INFO, "ZigZag: manual control"); }