diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 51d80d5f3e22d609892102b4e2e5ef507838d32d..bc4a4739a2c66e4875f86454e2dd49fd16dc2c0f 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);