diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde
index a54c79586c..004b3ff14a 100644
--- a/ArduCopter/control_rtl.pde
+++ b/ArduCopter/control_rtl.pde
@@ -2,13 +2,16 @@
 
 /*
  * control_rtl.pde - init and run calls for RTL flight mode
+ *
+ * There are two parts to RTL, the high level decision making which controls which state we are in
+ * and the lower implementation of the waypoint or landing controllers within those states
  */
 
 // rtl_init - initialise rtl controller
 static bool rtl_init(bool ignore_checks)
 {
     if (GPS_ok() || ignore_checks) {
-        do_RTL();
+        rtl_climb_start();
         return true;
     }else{
         return false;
@@ -19,6 +22,271 @@ static bool rtl_init(bool ignore_checks)
 // should be called at 100hz or more
 static void rtl_run()
 {
-    verify_RTL();
+    // check if we need to move to next state
+    if (rtl_state_complete) {
+        switch (rtl_state) {
+        case InitialClimb:
+            rtl_return_start();
+            break;
+        case ReturnHome:
+            rtl_loiterathome_start();
+            break;
+        case LoiterAtHome:
+            if (g.rtl_alt_final > 0) {
+                rtl_descent_start();
+            }else{
+                rtl_land_start();
+            }
+            break;
+        case FinalDescent:
+            // do nothing
+            break;
+        case Land:
+            // do nothing
+            break;
+        }
+    }
+
+    // call the correct run function
+    switch (rtl_state) {
+
+    case InitialClimb:
+        rtl_climb_return_descent_run();
+        break;
+
+    case ReturnHome:
+        rtl_climb_return_descent_run();
+        break;
+
+    case LoiterAtHome:
+        rtl_loiterathome_run();
+        break;
+
+    case FinalDescent:
+        rtl_climb_return_descent_run();
+        break;
+
+    case Land:
+        rtl_land_run();
+        break;
+    }
+}
+
+// rtl_climb_start - initialise climb to RTL altitude
+static void rtl_climb_start()
+{
+    rtl_state = InitialClimb;
+    rtl_state_complete = false;
+
+    // get horizontal stopping point
+    Vector3f destination;
+    wp_nav.get_wp_stopping_point_xy(destination);
+    destination.z = get_RTL_alt();
+
+    wp_nav.set_wp_destination(destination);
+
+    // hold current yaw during initial climb
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+}
+
+// rtl_return_start - initialise return to home
+static void rtl_return_start()
+{
+    rtl_state = ReturnHome;
+    rtl_state_complete = false;
+
+    // initialise original_wp_bearing which is used to point the nose home
+    wp_bearing = wp_nav.get_wp_bearing_to_destination();
+    original_wp_bearing = wp_bearing;
+
+    // set target to above home
+    Vector3f destination = Vector3f(0,0,get_RTL_alt());
+    wp_nav.set_wp_destination(destination);
+
+    // initialise yaw to point home (maybe)
+    set_auto_yaw_mode(get_default_auto_yaw_mode(true));
+}
+
+// rtl_descent_start - initialise descent to final alt
+static void rtl_descent_start()
+{
+    rtl_state = FinalDescent;
+    rtl_state_complete = false;
+
+    // set target to above home
+    Vector3f destination = Vector3f(0,0,g.rtl_alt_final);
+    wp_nav.set_wp_destination(destination);
+
+    // initialise yaw to point home (maybe)
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+}
+
+// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
+//      called by rtl_run at 100hz or more
+static void rtl_climb_return_descent_run()
+{
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed) {
+        // reset attitude control targets
+        attitude_control.init_targets();
+        attitude_control.set_throttle_out(0, false);
+        // To-Do: re-initialise wpnav targets
+        return;
+    }
+
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+        if (target_yaw_rate != 0) {
+            set_auto_yaw_mode(AUTO_YAW_HOLD);
+        }
+    }
+
+    // run waypoint controller
+    wp_nav.update_wpnav();
+
+    // call z-axis position controller (wpnav should have already updated it's alt target)
+    pos_control.update_z_controller();
+
+    // call attitude controller
+    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+        // roll & pitch from waypoint controller, yaw rate from pilot
+        attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+    }else{
+        // roll, pitch from waypoint controller, yaw heading from auto_heading()
+        attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
+    }
+
+    // check if we've completed this stage of RTL
+    rtl_state_complete = wp_nav.reached_wp_destination();
+
+    // re-fetch angle targets for reporting
+    const Vector3f angle_target = attitude_control.angle_ef_targets();
+    control_roll = angle_target.x;
+    control_pitch = angle_target.y;
+    control_yaw = angle_target.z;
+}
+
+// rtl_return_start - initialise return to home
+static void rtl_loiterathome_start()
+{
+    rtl_state = LoiterAtHome;
+    rtl_state_complete = false;
+    rtl_loiter_start_time = millis();
+
+    // yaw back to initial take-off heading yaw unless pilot has already overridden yaw
+    if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
+        set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
+    } else {
+        set_auto_yaw_mode(AUTO_YAW_HOLD);
+    }
+}
+
+// rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller
+//      called by rtl_run at 100hz or more
+static void rtl_loiterathome_run()
+{
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed) {
+        // reset attitude control targets
+        attitude_control.init_targets();
+        attitude_control.set_throttle_out(0, false);
+        // To-Do: re-initialise wpnav targets
+        return;
+    }
+
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+        if (target_yaw_rate != 0) {
+            set_auto_yaw_mode(AUTO_YAW_HOLD);
+        }
+    }
+
+    // run waypoint controller
+    wp_nav.update_wpnav();
+
+    // call z-axis position controller (wpnav should have already updated it's alt target)
+    pos_control.update_z_controller();
+
+    // call attitude controller
+    if (auto_yaw_mode == AUTO_YAW_HOLD) {
+        // roll & pitch from waypoint controller, yaw rate from pilot
+        attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+    }else{
+        // roll, pitch from waypoint controller, yaw heading from auto_heading()
+        attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading());
+    }
+
+    // check if we've completed this stage of RTL
+    // To-Do: add extra check that we've reached the target yaw
+    rtl_state_complete = ((millis() - rtl_loiter_start_time) > (uint32_t)g.rtl_loiter_time.get());
+
+    // re-fetch angle targets for reporting
+    const Vector3f angle_target = attitude_control.angle_ef_targets();
+    control_roll = angle_target.x;
+    control_pitch = angle_target.y;
+    control_yaw = angle_target.z;
+}
+
+// rtl_loiterathome_start - initialise controllers to loiter over home
+static void rtl_land_start()
+{
+    rtl_state = Land;
+    rtl_state_complete = false;
+
+    // Set wp navigation target to above home
+    wp_nav.set_loiter_target(Vector3f(0,0,0));
+
+    // initialise altitude target to stopping point
+    pos_control.set_target_to_stopping_point_z();
+
+    // initialise yaw
+    set_auto_yaw_mode(AUTO_YAW_HOLD);
+}
+
+// rtl_returnhome_run - return home
+//      called by rtl_run at 100hz or more
+static void rtl_land_run()
+{
+    // if not auto armed set throttle to zero and exit immediately
+    if(!ap.auto_armed) {
+        attitude_control.init_targets();
+        attitude_control.set_throttle_out(0, false);
+        // set target to current position
+        wp_nav.init_loiter_target();
+        return;
+    }
+
+    // process pilot's yaw input
+    float target_yaw_rate = 0;
+    if (!failsafe.radio) {
+        // get pilot's desired yaw rate
+        target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
+    }
+
+    // run loiter controller
+    wp_nav.update_loiter();
+
+    // call z-axis position controller
+    float cmb_rate = get_throttle_land();
+    pos_control.set_alt_target_from_climb_rate(cmb_rate, G_Dt);
+    pos_control.update_z_controller();
+
+    // roll & pitch from waypoint controller, yaw rate from pilot
+    attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate);
+
+    // check if we've completed this stage of RTL
+    rtl_state_complete = ap.land_complete;
+
+    // re-fetch angle targets for reporting
+    const Vector3f angle_target = attitude_control.angle_ef_targets();
+    control_roll = angle_target.x;
+    control_pitch = angle_target.y;
+    control_yaw = angle_target.z;
 }
 
diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h
index 9a9060cbe3..612f4f418f 100644
--- a/ArduCopter/defines.h
+++ b/ArduCopter/defines.h
@@ -213,13 +213,15 @@
 //#define WP_OPTION_					64
 #define WP_OPTION_NEXT_CMD                      128
 
-// RTL state
-#define RTL_STATE_START             0
-#define RTL_STATE_INITIAL_CLIMB     1
-#define RTL_STATE_RETURNING_HOME    2
-#define RTL_STATE_LOITERING_AT_HOME 3
-#define RTL_STATE_FINAL_DESCENT     4
-#define RTL_STATE_LAND              5
+// RTL states
+enum RTLState {
+    Start,
+    InitialClimb,
+    ReturnHome,
+    LoiterAtHome,
+    FinalDescent,
+    Land
+};
 
 // LAND state
 #define LAND_STATE_FLY_TO_LOCATION  0