ArduCopter: RTL clean-up and slightly improved landing sensor

Consolidated RTL state to be captured by rtl_state variable.
Combined update_RTL_Nav and verify_RTL functions which performed the same function but one was for missions, the other for the RTL flight mode.
Renamed some RTL parameters and global variables to have RTL at the front.
Landing detector now checks accel-throttle's I term and/or a very low throttle value

ArduCopter/AP_State.pde

@@ -96,15 +96,6 @@ void set_land_complete(bool b)
 	ap.land_complete = b;
 }
 
-// ---------------------------------------------
-void set_rtl_reached_alt(bool b)
-{
-	if(b){
-		Log_Write_Event(DATA_RTL_REACHED_ALT);
-	}
-	ap.rtl_reached_alt = b;
-}
-
 // ---------------------------------------------
 
 void set_alt_change(uint8_t flag){

ArduCopter/ArduCopter.pde

@@ -400,11 +400,10 @@ static union {
         uint8_t failsafe           : 1; // 9    // A status flag for the failsafe state
         uint8_t do_flip            : 1; // 10   // Used to enable flip code
         uint8_t takeoff_complete   : 1; // 11
-        uint8_t rtl_reached_alt    : 1; // 12
+        uint8_t land_complete      : 1; // 12
-        uint8_t land_complete      : 1; // 13
+        uint8_t compass_status     : 1; // 13
-        uint8_t compass_status     : 1; // 14
+        uint8_t gps_status         : 1; // 14
-        uint8_t gps_status         : 1; // 15
+        uint8_t fast_corner        : 1; // 15   // should we take the waypoint quickly or slow down?
-        uint8_t fast_corner        : 1; // 16   // should we take the waypoint quickly or slow down?
     };
     uint16_t value;
 } ap;
@@ -574,6 +573,7 @@ static int16_t max_speed_old;
 static int32_t long_error, lat_error;
 static int16_t control_roll;
 static int16_t control_pitch;
+static uint8_t rtl_state;
 
 ////////////////////////////////////////////////////////////////////////////////
 // Orientation
@@ -812,7 +812,7 @@ static int32_t nav_yaw;
 static int32_t auto_yaw;
 static uint8_t yaw_timer;
 // Options include: no tracking, point at next wp, or at a target
-static byte yaw_tracking = MAV_ROI_WPNEXT;
+static byte auto_yaw_tracking = MAV_ROI_WPNEXT;
 // In AP Mission scripting we have a fine level of control for Yaw
 // This is our initial angle for relative Yaw movements
 static int32_t command_yaw_start;
@@ -828,7 +828,7 @@ static int16_t command_yaw_speed;
 static byte command_yaw_dir;
 // Direction we will turn – 1 = relative, 0 = Absolute
 static byte command_yaw_relative;
-// Yaw will point at this location if yaw_tracking is set to MAV_ROI_LOCATION
+// Yaw will point at this location if auto_yaw_tracking is set to MAV_ROI_LOCATION
 static struct   Location target_WP;
 
 
@@ -897,7 +897,7 @@ static float dTnav;
 // Counters for branching from 4 minute control loop used to save Compass offsets
 static int16_t superslow_loopCounter;
 // Loiter timer - Records how long we have been in loiter
-static uint32_t loiter_timer;
+static uint32_t rtl_loiter_start_time;
 // disarms the copter while in Acro or Stabilize mode after 30 seconds of no flight
 static uint8_t auto_disarming_counter;
 // prevents duplicate GPS messages from entering system
@@ -2242,17 +2242,24 @@ static void update_nav_wp()
 
 static void update_auto_yaw()
 {
-    if(wp_control == CIRCLE_MODE) {
+    switch( wp_control ) {
-        auto_yaw = get_bearing_cd(&current_loc, &circle_WP);
+        case LOITER_MODE:
-
-    }else if(wp_control == LOITER_MODE) {
             // just hold nav_yaw
-
+            break;
-    }else if(yaw_tracking == MAV_ROI_LOCATION) {
+        case WP_MODE:
+            if(auto_yaw_tracking == MAV_ROI_LOCATION) {
                 auto_yaw = get_bearing_cd(&current_loc, &target_WP);
-
+            }else if(auto_yaw_tracking == MAV_ROI_WPNEXT) {
-    }else if(yaw_tracking == MAV_ROI_WPNEXT) {
                 // Point towards next WP
+                // we don't use target_bearing because we don't want the copter to turn too much during flight
                 auto_yaw = original_target_bearing;
             }
+            break;
+        case CIRCLE_MODE:
+            auto_yaw = get_bearing_cd(&current_loc, &circle_WP);
+            break;
+        case NO_NAV_MODE:
+            // just hold nav_yaw
+            break;
+    }
 }

ArduCopter/Attitude.pde

@@ -764,6 +764,8 @@ void set_throttle_out( int16_t throttle_out, bool apply_angle_boost )
         // clear angle_boost for logging purposes
         angle_boost = 0;
     }
+
+    // TO-DO: fix line below because this function is also called by accel based throttle controller
     throttle_accel_controller_active = false;
 }
 
@@ -1032,7 +1034,7 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate)
 // sends the desired acceleration in the accel based throttle controller
 // called at 50hz
 #define LAND_START_ALT 1000         // altitude in cm where land controller switches to slow rate of descent
-#define LAND_DETECTOR_TRIGGER 250   // number of 50hz iterations with near zero climb rate that triggers landing complete.
+#define LAND_DETECTOR_TRIGGER 50    // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete.
 static void
 get_throttle_land()
 {
@@ -1042,17 +1044,17 @@ get_throttle_land()
     }else{
         get_throttle_rate_stabilized(-abs(g.land_speed));
 
-        // detect whether we have landed
+        // detect whether we have landed by watching for minimum throttle and now movement
 #if INERTIAL_NAV_Z == ENABLED
-        if (abs(inertial_nav._velocity.z) < 20) {
+        if (abs(inertial_nav._velocity.z) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) {
 #else
-        if (abs(climb_rate) < 20) {
+        if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) {
 #endif
             if( land_detector < LAND_DETECTOR_TRIGGER ) {
                 land_detector++;
             }else{
                 set_land_complete(true);
-                if( g.rc_3.control_in == 0 ) {
+                if( g.rc_3.control_in == 0 || ap.failsafe ) {
                     init_disarm_motors();
                 }
             }

ArduCopter/Parameters.h

@@ -134,10 +134,10 @@ public:
         //
         // 160: Navigation parameters
         //
-        k_param_RTL_altitude = 160,
+        k_param_rtl_altitude = 160,
         k_param_crosstrack_gain,
-        k_param_auto_land_timeout,
+        k_param_rtl_loiter_time,
-        k_param_rtl_approach_alt,
+        k_param_rtl_alt_final,
         k_param_tilt_comp, //164
 
 
@@ -243,7 +243,7 @@ public:
     AP_Int8         serial3_baud;
     AP_Int8         telem_delay;
 
-    AP_Int16        RTL_altitude;
+    AP_Int16        rtl_altitude;
     AP_Int8         sonar_enabled;
     AP_Int8         sonar_type;       // 0 = XL, 1 = LV,
                                       // 2 = XLL (XL with 10m range)
@@ -260,7 +260,7 @@ public:
     AP_Int8         optflow_enabled;
     AP_Float        low_voltage;
     AP_Int8         super_simple;
-    AP_Int16        rtl_approach_alt;
+    AP_Int16        rtl_alt_final;
     AP_Int8         tilt_comp;
     AP_Int8         axis_enabled;
     AP_Int8         copter_leds_mode;           // Operating mode of LED
@@ -281,7 +281,7 @@ public:
     AP_Int16        waypoint_speed_max;
     AP_Float        crosstrack_gain;
     AP_Int16 		crosstrack_min_distance;
-    AP_Int32        auto_land_timeout;
+    AP_Int32        rtl_loiter_time;
     AP_Int16        land_speed;
 
 

ArduCopter/Parameters.pde

@@ -36,14 +36,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Increment: 1
     GSCALAR(telem_delay,            "TELEM_DELAY",     0),
 
-    // @Param: ALT_HOLD_RTL
+    // @Param: ALT_RTL
     // @DisplayName: RTL Altitude
-    // @Description: This is the altitude the model will move to before Returning to Launch.  Set to zero to return at current altitude.
+    // @Description: The minimum altitude the model will move to before Returning to Launch.  Set to zero to return at current altitude.
     // @Units: centimeters
     // @Range: 0 4000
     // @Increment: 1
     // @User: Standard
-    GSCALAR(RTL_altitude,   "ALT_HOLD_RTL",     RTL_HOLD_ALT),
+    GSCALAR(rtl_altitude,   "RTL_ALT",     RTL_ALT),
 
     // @Param: SONAR_ENABLE
     // @DisplayName: Enable Sonar
@@ -106,14 +106,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(super_simple,   "SUPER_SIMPLE",     SUPER_SIMPLE),
 
-    // @Param: APPROACH_ALT
+    // @Param: RTL_ALT_FINAL
-    // @DisplayName: RTL Approach Altitude
+    // @DisplayName: RTL Final Altitude
-    // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
+    // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission.  Set to -1 to disable, zero to land.
     // @Units: centimeters
-    // @Range: 0 1000
+    // @Range: -1 1000
     // @Increment: 1
     // @User: Standard
-    GSCALAR(rtl_approach_alt,       "APPROACH_ALT", RTL_APPROACH_ALT),
+    GSCALAR(rtl_alt_final,  "RTL_ALT_FINAL", RTL_ALT_FINAL),
 
 	// @Param: TILT
     // @DisplayName: Auto Tilt Compensation
@@ -191,7 +191,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST",  CROSSTRACK_MIN_DISTANCE),
 
-    GSCALAR(auto_land_timeout,      "AUTO_LAND",    AUTO_LAND_TIME*1000),
+    // @Param: RTL_LOITER_TIME
+    // @DisplayName: RTL loiter time
+    // @Description: Time (in milliseconds) to loiter above home before begining final descent
+    // @Units: ms
+    // @Range: 0 60000
+    // @Increment: 1000
+    // @User: Standard
+    GSCALAR(rtl_loiter_time,      "RTL_LOIT_TIME",    RTL_LOITER_TIME),
 
     // @Param: LAND_SPEED
     // @DisplayName: Land speed

ArduCopter/commands.pde

@@ -12,8 +12,8 @@ static void init_commands()
     ap.fast_corner          = false;
 	reset_desired_speed();
 
-    // default Yaw tracking
+    // default auto mode yaw tracking
-    yaw_tracking                    = MAV_ROI_WPNEXT;
+    auto_yaw_tracking       = MAV_ROI_WPNEXT;
 }
 
 // Getters
@@ -110,12 +110,12 @@ static void set_cmd_with_index(struct Location temp, int i)
 
 static int32_t get_RTL_alt()
 {
-    if(g.RTL_altitude <= 0) {
+    if(g.rtl_altitude <= 0) {
-		return min(current_loc.alt, MAXIMUM_RTL_ALT);
+		return min(current_loc.alt, RTL_ALT_MAX);
-    }else if (g.RTL_altitude < current_loc.alt) {
+    }else if (g.rtl_altitude < current_loc.alt) {
-		return min(current_loc.alt, MAXIMUM_RTL_ALT);
+		return min(current_loc.alt, RTL_ALT_MAX);
     }else{
-        return g.RTL_altitude;
+        return g.rtl_altitude;
     }
 }
 
@@ -202,6 +202,8 @@ static void init_home()
 #if INERTIAL_NAV_XY == ENABLED
     // set inertial nav's home position
     inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude);
+    inertial_nav.set_altitude(home.alt);
+    inertial_nav.set_velocity_z(0);
 #endif
 
     if (g.log_bitmask & MASK_LOG_CMD)
@@ -213,7 +215,7 @@ static void init_home()
     // Load home for a default guided_WP
     // -------------
     guided_WP = home;
-    guided_WP.alt += g.RTL_altitude;
+    guided_WP.alt += g.rtl_altitude;
 }
 
 

ArduCopter/commands_logic.pde

@@ -209,19 +209,29 @@ static bool verify_may()
 //
 /********************************************************************************/
 
+// do_RTL - start Return-to-Launch
 static void do_RTL(void)
 {
-    // TODO: Altitude option from mission planner
+    // set rtl state
-    Location temp   = home;
+    rtl_state = RTL_STATE_RETURNING_HOME;
-    temp.alt                = get_RTL_alt();
 
-    //so we know where we are navigating from
+    // set roll, pitch and yaw modes
-    // --------------------------------------
+    roll_pitch_mode     = RTL_RP;
+    yaw_mode            = YAW_AUTO;
+    auto_yaw_tracking   = MAV_ROI_WPNEXT;
+    set_throttle_mode(RTL_THR);
+
+    // set navigation mode
+    wp_control = WP_MODE;
+
+    // so we know where we are navigating from
     next_WP = current_loc;
 
-    // Loads WP from Memory
+    // Set navigation target to home
-    // --------------------
+    set_next_WP(&home);
-    set_next_WP(&temp);
+
+    // override altitude to RTL altitude
+    set_new_altitude(get_RTL_alt());
 
     // output control mode to the ground station
     // -----------------------------------------
@@ -354,6 +364,7 @@ static bool verify_land()
     }
 
     // turn off loiter below 1m
+    // To-Do: instead of turning off loiter we should make loiter less aggressive
     if(current_loc.alt < 100 ) {
         wp_control      = NO_NAV_MODE;
     }
@@ -452,19 +463,79 @@ static bool verify_loiter_turns()
     return false;
 }
 
+// verify_RTL - handles any state changes required to implement RTL
+// do_RTL should have been called once first to initialise all variables
+// returns true with RTL has completed successfully
 static bool verify_RTL()
 {
-    wp_control      = WP_MODE;
+    bool retval = false;
 
-    // Did we pass the WP?	// Distance checking
+    switch( rtl_state ) {
-    if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) {
+
+        case RTL_STATE_RETURNING_HOME:
+            // if we've reached home initiate loiter
+            if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) {
+                rtl_state = RTL_STATE_LOITERING_AT_HOME;
                 wp_control = LOITER_MODE;
 
-        //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home"));
+                // set loiter timer
-        return true;
+                rtl_loiter_start_time = millis();
-    }else{
+
-        return false;
+                // give pilot back control of yaw
+                yaw_mode = YAW_HOLD;
             }
+            break;
+
+        case RTL_STATE_LOITERING_AT_HOME:
+            // check if we've loitered long enough
+            if( millis() - rtl_loiter_start_time > (uint32_t)g.rtl_loiter_time.get() ) {
+                // initiate landing or descent
+                if(g.rtl_alt_final == 0) {
+                    // land
+                    do_land();
+                    // override landing location (do_land defaults to current location)
+                    next_WP.lat = home.lat;
+                    next_WP.lng = home.lng;
+                    // update RTL state
+                    rtl_state = RTL_STATE_LAND;
+                }else{
+                    // descend
+                    if(current_loc.alt > g.rtl_alt_final) {
+                        set_new_altitude(g.rtl_alt_final);
+                    }
+                    // update RTL state
+                    rtl_state = RTL_STATE_FINAL_DESCENT;
+                }
+            }
+            break;
+
+        case RTL_STATE_FINAL_DESCENT:
+            // rely on altitude check to confirm we have reached final altitude
+            if(current_loc.alt <= g.rtl_alt_final || alt_change_flag == REACHED_ALT) {
+                // switch to regular loiter mode
+                set_mode(LOITER);
+                // override location and altitude
+                set_next_WP(&home);
+                // override altitude to RTL altitude
+                set_new_altitude(g.rtl_alt_final);
+                retval = true;
+            }
+            break;
+
+        case RTL_STATE_LAND:
+            // rely on verify_land to return correct status
+            retval = verify_land();
+            break;
+
+        default:
+            // this should never happen
+            // TO-DO: log an error
+            retval = true;
+            break;
+    }
+
+    // true is returned if we've successfully completed RTL
+    return retval;
 }
 
 /********************************************************************************/
@@ -496,7 +567,7 @@ static void do_within_distance()
 static void do_yaw()
 {
     //cliSerial->println("dyaw ");
-    yaw_tracking = MAV_ROI_NONE;
+    auto_yaw_tracking = MAV_ROI_NONE;
 
     // target angle in degrees
     command_yaw_start               = nav_yaw;     // current position
@@ -669,9 +740,9 @@ static void do_change_speed()
 
 static void do_target_yaw()
 {
-    yaw_tracking = command_cond_queue.p1;
+    auto_yaw_tracking = command_cond_queue.p1;
 
-    if(yaw_tracking == MAV_ROI_LOCATION) {
+    if(auto_yaw_tracking == MAV_ROI_LOCATION) {
         target_WP = command_cond_queue;
     }
 }
@@ -793,7 +864,7 @@ static void do_nav_roi()
 
     // check if mount type requires us to rotate the quad
     if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) {
-        yaw_tracking = MAV_ROI_LOCATION;
+        auto_yaw_tracking = MAV_ROI_LOCATION;
         target_WP = command_nav_queue;
         auto_yaw = get_bearing_cd(&current_loc, &target_WP);
     }
@@ -808,7 +879,7 @@ static void do_nav_roi()
     //		4: point at a target given a target id (can't be implmented)
 #else
     // if we have no camera mount simply rotate the quad
-    yaw_tracking = MAV_ROI_LOCATION;
+    auto_yaw_tracking = MAV_ROI_LOCATION;
     target_WP = command_nav_queue;
     auto_yaw = get_bearing_cd(&current_loc, &target_WP);
 #endif

ArduCopter/commands_process.pde

@@ -217,11 +217,11 @@ static void exit_mission()
         // we will disarm the motors after landing.
     }else{
         // If the approach altitude is valid (above 1m), do approach, else land
-        if(g.rtl_approach_alt == 0) {
+        if(g.rtl_alt_final == 0) {
             set_mode(LAND);
         }else{
             set_mode(LOITER);
-            set_new_altitude(g.rtl_approach_alt);
+            set_new_altitude(g.rtl_alt_final);
         }
     }
 

ArduCopter/config.h

@@ -433,8 +433,8 @@
  # define MAXIMUM_THROTTLE       1000
 #endif
 
-#ifndef AUTO_LAND_TIME
+#ifndef RTL_LOITER_TIME
- # define AUTO_LAND_TIME 5
+ # define RTL_LOITER_TIME 5000       // Time (in milliseconds) to loiter above home before begining final descent
 #endif
 
 #ifndef LAND_SPEED
@@ -584,20 +584,12 @@
 
 
 // RTL Mode
-#ifndef RTL_YAW
- #if FRAME_CONFIG == HELI_FRAME
-  # define RTL_YAW                  YAW_LOOK_AT_HOME
- #else
-  # define RTL_YAW                  YAW_HOLD
- #endif
-#endif
-
 #ifndef RTL_RP
  # define RTL_RP                    ROLL_PITCH_AUTO
 #endif
 
 #ifndef RTL_THR
- # define RTL_THR                   THROTTLE_HOLD
+ # define RTL_THR                   THROTTLE_AUTO
 #endif
 
 #ifndef SUPER_SIMPLE
@@ -609,16 +601,16 @@
 #endif
 
 // RTL Mode
-#ifndef RTL_APPROACH_ALT
+#ifndef RTL_ALT_FINAL
- # define RTL_APPROACH_ALT       	200 // cm!!!
+ # define RTL_ALT_FINAL             200     // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
 #endif
 
-#ifndef RTL_HOLD_ALT
+#ifndef RTL_ALT
- # define RTL_HOLD_ALT 				1500    // height to return to Home in CM, 0 = Maintain current altitude
+ # define RTL_ALT 				    1500    // default alt to return to home in cm, 0 = Maintain current altitude
 #endif
 
-#ifndef MAXIMUM_RTL_ALT
+#ifndef RTL_ALT_MAX
- # define MAXIMUM_RTL_ALT 			8000    // Max height to return to Home in CM
+ # define RTL_ALT_MAX               8000    // Max height to return to home in cm
 #endif
 
 

ArduCopter/defines.h

@@ -220,12 +220,11 @@
 #define NO_COMMAND 0
 
 
+// Navigation modes held in wp_control variable
 #define LOITER_MODE 1
 #define WP_MODE 2
 #define CIRCLE_MODE 3
 #define NO_NAV_MODE 4
-#define TOY_MODE 5                      // THOR This mode defines the Virtual
-                                        // WP following mode
 
 // TOY mixing options
 #define TOY_LOOKUP_TABLE 0
@@ -243,6 +242,12 @@
 //#define WP_OPTION_					64
 #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
+
 //repeating events
 #define NO_REPEAT 0
 #define CH_5_TOGGLE 1

ArduCopter/limits.pde

@@ -23,7 +23,7 @@ void set_recovery_home_alt() {
             return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2)));
         }
     } else {
-        return_altitude_cm_ahl = (uint32_t) (home.alt + g.RTL_altitude*100);
+        return_altitude_cm_ahl = (uint32_t) (home.alt + g.rtl_altitude);
     }
     // final sanity check
     // if our return is less than 4 meters from ground, set it to 4m, to clear "people" height.

ArduCopter/navigation.pde

@@ -201,25 +201,11 @@ static void run_navigation_contollers()
         break;
 
     case RTL:
-        // have we reached the desired Altitude?
+        // execute the RTL state machine
-        if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) {
+        verify_RTL();
-            // we are at or above the target alt
-            if(false == ap.rtl_reached_alt) {
-                set_rtl_reached_alt(true);
-                do_RTL();
-            }
-            wp_control = WP_MODE;
-            // checks if we have made it to home
-            update_nav_RTL();
-        } else{
-            // we need to loiter until we are ready to come home
-            wp_control = LOITER_MODE;
-        }
 
         // calculates desired Yaw
-#if FRAME_CONFIG ==     HELI_FRAME
         update_auto_yaw();
-#endif
 
         // calculates the desired Roll and Pitch
         update_nav_wp();
@@ -254,25 +240,6 @@ static void run_navigation_contollers()
             wp_control = LOITER_MODE;
         }
 
-        if(loiter_timer != 0) {
-            // If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach
-            if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()) {
-                // just to make sure we clear the timer
-                loiter_timer = 0;
-                if(g.rtl_approach_alt == 0) {
-                    set_mode(LAND);
-                    if(home_distance < 300) {
-                        next_WP.lat = home.lat;
-                        next_WP.lng = home.lng;
-                    }
-                }else{
-                    if(g.rtl_approach_alt < current_loc.alt) {
-                        set_new_altitude(g.rtl_approach_alt);
-                    }
-                }
-            }
-        }
-
         // calculates the desired Roll and Pitch
         update_nav_wp();
         break;
@@ -318,30 +285,6 @@ static void run_navigation_contollers()
     }
 }
 
-static void update_nav_RTL()
-{
-    // Have we have reached Home?
-    if(wp_distance <= 200 || check_missed_wp()) {
-        // if loiter_timer value > 0, we are set to trigger auto_land or approach
-        set_mode(LOITER);
-
-        // just in case we arrive and we aren't at the lower RTL alt yet.
-        set_new_altitude(get_RTL_alt());
-
-        // force loitering above home
-        next_WP.lat = home.lat;
-        next_WP.lng = home.lng;
-
-        // If failsafe OR auto approach altitude is set
-        // we will go into automatic land, (g.rtl_approach_alt) is the lowest point
-        // -1 means disable feature
-        if(ap.failsafe || g.rtl_approach_alt >= 0)
-            loiter_timer = millis();
-        else
-            loiter_timer = 0;
-    }
-}
-
 static bool check_missed_wp()
 {
     int32_t temp;

ArduCopter/system.pde

@@ -410,14 +410,9 @@ static void set_mode(byte mode)
     motors.auto_armed(g.rc_3.control_in > 0);
     set_auto_armed(g.rc_3.control_in > 0);
 
-    // do not auto_land if we are leaving RTL
-    loiter_timer = 0;
-
     // if we change modes, we must clear landed flag
     set_land_complete(false);
 
-    // have we achieved the proper altitude before RTL is enabled
-    set_rtl_reached_alt(false);
     // debug to Serial terminal
     //cliSerial->println(flight_mode_strings[control_mode]);
 
@@ -528,12 +523,7 @@ static void set_mode(byte mode)
     case RTL:
     	ap.manual_throttle = false;
     	ap.manual_attitude = false;
-        yaw_mode        = RTL_YAW;
+        do_RTL();
-        roll_pitch_mode = RTL_RP;
-        set_throttle_mode(RTL_THR);
-        set_rtl_reached_alt(false);
-        set_next_WP(&current_loc);
-        set_new_altitude(get_RTL_alt());
         break;
 
     case OF_LOITER:

ArduCopter/test.pde

@@ -739,10 +739,10 @@ test_wp(uint8_t argc, const Menu::arg *argv)
 
     // save the alitude above home option
     cliSerial->printf_P(PSTR("Hold alt "));
-    if(g.RTL_altitude < 0) {
+    if(g.rtl_altitude < 0) {
         cliSerial->printf_P(PSTR("\n"));
     }else{
-        cliSerial->printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100);
+        cliSerial->printf_P(PSTR("of %dm\n"), (int)g.rtl_altitude / 100);
     }
 
     cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total);
@@ -1068,7 +1068,7 @@ test_logging(uint8_t argc, const Menu::arg *argv)
  *
  *       }
  *
- *       g.RTL_altitude.set_and_save(300);
+ *       g.rtl_altitude.set_and_save(300);
  *       g.command_total.set_and_save(4);
  *       g.waypoint_radius.set_and_save(3);
  *