From 1332ed3b5c74f7045877f87ea5acf755542f374f Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Mon, 17 Dec 2012 16:17:49 +0900
Subject: [PATCH] ArduCopter: restore initial climb to RTL

---
 ArduCopter/commands_logic.pde | 35 ++++++++++++++++++++++++-----------
 ArduCopter/defines.h          |  9 +++++----
 2 files changed, 29 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 0cb4010638..39718bfcb5 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -219,28 +219,21 @@ static bool verify_may()
 static void do_RTL(void)
 {
     // set rtl state
-    rtl_state = RTL_STATE_RETURNING_HOME;
+    rtl_state = RTL_STATE_INITIAL_CLIMB;
 
     // set roll, pitch and yaw modes
     set_roll_pitch_mode(RTL_RP);
-    set_yaw_mode(RTL_YAW);
+    set_yaw_mode(YAW_HOLD);     // first stage of RTL is the initial climb so just hold current yaw
     set_throttle_mode(RTL_THR);
 
     // set navigation mode
-    wp_control = WP_MODE;
+    wp_control = LOITER_MODE;
 
-    // so we know where we are navigating from
+    // initial climb starts at current location
     next_WP = current_loc;
 
-    // Set navigation target to home
-    set_next_WP(&home);
-
     // override altitude to RTL altitude
     set_new_altitude(get_RTL_alt());
-
-    // output control mode to the ground station
-    // -----------------------------------------
-    gcs_send_message(MSG_HEARTBEAT);
 }
 
 /********************************************************************************/
@@ -480,6 +473,26 @@ static bool verify_RTL()
 
     switch( rtl_state ) {
 
+        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) {
+                // Set navigation target to home
+                set_next_WP(&home);
+
+                // override target altitude to RTL altitude
+                set_new_altitude(get_RTL_alt());
+
+                // set navigation mode
+                wp_control = WP_MODE;
+
+                // set yaw mode
+                set_yaw_mode(RTL_YAW);
+
+                // advance to next rtl state
+                rtl_state = RTL_STATE_RETURNING_HOME;
+            }
+            break;
+
         case RTL_STATE_RETURNING_HOME:
             // if we've reached home initiate loiter
             if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) {
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 0c5c475070..9b6af58023 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -214,10 +214,11 @@
 #define WP_OPTION_NEXT_CMD                      128
 
 // RTL state
-#define RTL_STATE_RETURNING_HOME    0
-#define RTL_STATE_LOITERING_AT_HOME 1
-#define RTL_STATE_FINAL_DESCENT     2
-#define RTL_STATE_LAND              3
+#define RTL_STATE_INITIAL_CLIMB     0
+#define RTL_STATE_RETURNING_HOME    1
+#define RTL_STATE_LOITERING_AT_HOME 2
+#define RTL_STATE_FINAL_DESCENT     3
+#define RTL_STATE_LAND              4
 
 //repeating events
 #define RELAY_TOGGLE 5