From da2a463f18eedac351d4ec9a53eea44857079410 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 29 Sep 2014 15:26:54 +0900
Subject: [PATCH] Copter: shift pos targets to current location before takeoff

---
 ArduCopter/control_auto.pde   | 3 ++-
 ArduCopter/control_guided.pde | 3 ++-
 2 files changed, 4 insertions(+), 2 deletions(-)

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;
     }