diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde
index ce7e744b86..6648f46bbc 100644
--- a/ArduCopter/control_auto.pde
+++ b/ArduCopter/control_auto.pde
@@ -102,13 +102,14 @@ static void auto_takeoff_run()
 {
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
+        // initialise wpnav targets
+        wp_nav.shift_wp_origin_to_current_pos();
         // reset attitude control targets
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         // tell motors to do a slow start
         motors.slow_start(true);
-        // To-Do: re-initialise wpnav targets
         return;
     }
 
diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde
index 30f5fd9f17..02deb9d461 100644
--- a/ArduCopter/control_guided.pde
+++ b/ArduCopter/control_guided.pde
@@ -140,13 +140,14 @@ static void guided_takeoff_run()
 {
     // if not auto armed set throttle to zero and exit immediately
     if(!ap.auto_armed) {
+        // initialise wpnav targets
+        wp_nav.shift_wp_origin_to_current_pos();
         // reset attitude control targets
         attitude_control.relax_bf_rate_controller();
         attitude_control.set_yaw_target_to_current_heading();
         attitude_control.set_throttle_out(0, false);
         // tell motors to do a slow start
         motors.slow_start(true);
-        // To-Do: re-initialise wpnav targets
         return;
     }