diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 354e8bffe6..6f62875d5c 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -729,6 +729,10 @@ bool Copter::ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
 void Copter::ModeAuto::takeoff_run()
 {
     auto_takeoff_run();
+    if (wp_nav->reached_wp_destination()) {
+        const Vector3f target = wp_nav->get_wp_destination();
+        wp_start(target);
+    }
 }
 
 // auto_wp_run - runs the auto waypoint controller
diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp
index 506793d218..d4a4c160d8 100644
--- a/ArduCopter/mode_guided.cpp
+++ b/ArduCopter/mode_guided.cpp
@@ -366,6 +366,10 @@ void Copter::ModeGuided::run()
 void Copter::ModeGuided::takeoff_run()
 {
     auto_takeoff_run();
+    if (wp_nav->reached_wp_destination()) {
+        const Vector3f target = wp_nav->get_wp_destination();
+        set_destination(target);
+    }
 }
 
 void Copter::Mode::auto_takeoff_run()