From ccaa7a52ef4aa9265384334918ff1d8716d24efa Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 15 Jan 2013 12:13:04 +0900
Subject: [PATCH] Copter: RTL bug fix to initial step is always a climb and not
 a descent

Also bug fix to hold yaw on take-off
---
 ArduCopter/commands_logic.pde | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 2f8ecfde59..06705097ce 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -229,7 +229,7 @@ static void do_RTL(void)
     wp_control = LOITER_MODE;
 
     // initial climb starts at current location
-    next_WP = current_loc;
+    set_next_WP(&current_loc);
 
     // override altitude to RTL altitude
     set_new_altitude(get_RTL_alt());
@@ -250,11 +250,14 @@ static void do_takeoff()
     // alt is always relative
     temp.alt = command_nav_queue.alt;
 
-    // prevent flips
-    reset_I_all();
-
     // Set our waypoint
     set_next_WP(&temp);
+
+    // set our yaw mode
+    set_yaw_mode(YAW_HOLD);
+
+    // prevent flips
+    reset_I_all();    
 }
 
 // do_nav_wp - initiate move to next waypoint
@@ -476,7 +479,7 @@ static bool verify_RTL()
 
         case RTL_STATE_INITIAL_CLIMB:
             // rely on verify_altitude function to update alt_change_flag when we've reached the target
-            if(alt_change_flag == REACHED_ALT) {
+            if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) {
                 // Set navigation target to home
                 set_next_WP(&home);