Copter: FlightMode - convert RTL flight mode

ArduCopter/Copter.cpp

@@ -33,8 +33,6 @@ Copter::Copter(void)
     home_bearing(0),
     home_distance(0),
     wp_distance(0),
-    rtl_state(RTL_InitialClimb),
-    rtl_state_complete(false),
     smart_rtl_state(SmartRTL_PathFollow),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
@@ -62,7 +60,6 @@ Copter::Copter(void)
     pmTest1(0),
     fast_loopTimer(0),
     mainLoop_count(0),
-    rtl_loiter_start_time(0),
     auto_trim_counter(0),
     in_mavlink_delay(false),
     param_loader(var_info)

ArduCopter/Copter.h

@@ -392,20 +392,6 @@ private:
         float descend_max; // centimetres
     } nav_payload_place;
 
-    // RTL
-    RTLState rtl_state;  // records state of rtl (initial climb, returning home, etc)
-    bool rtl_state_complete; // set to true if the current state is completed
-
-    struct {
-        // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
-        Location_Class origin_point;
-        Location_Class climb_target;
-        Location_Class return_target;
-        Location_Class descent_target;
-        bool land;
-        bool terrain_used;
-    } rtl_path;
-
     // SmartRTL
     SmartRTLState smart_rtl_state;  // records state of SmartRTL
 
@@ -532,8 +518,6 @@ private:
     uint32_t fast_loopTimer;
     // Counter of main loop executions.  Used for performance monitoring and failsafe processing
     uint16_t mainLoop_count;
-    // Loiter timer - Records how long we have been in loiter
-    uint32_t rtl_loiter_start_time;
     // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
     uint32_t arm_time_ms;
 
@@ -862,20 +846,6 @@ private:
     bool throw_height_good();
     bool throw_position_good();
 
-    bool rtl_init(bool ignore_checks);
-    void rtl_restart_without_terrain();
-    void rtl_run(bool disarm_on_land=true);
-    void rtl_climb_start();
-    void rtl_return_start();
-    void rtl_climb_return_run();
-    void rtl_loiterathome_start();
-    void rtl_loiterathome_run();
-    void rtl_descent_start();
-    void rtl_descent_run();
-    void rtl_land_start();
-    void rtl_land_run(bool disarm_on_land);
-    void rtl_build_path(bool terrain_following_allowed);
-    void rtl_compute_return_target(bool terrain_following_allowed);
     bool smart_rtl_init(bool ignore_checks);
     void smart_rtl_exit();
     void smart_rtl_run();
@@ -884,6 +854,7 @@ private:
     void smart_rtl_pre_land_position_run();
     void smart_rtl_land();
     void smart_rtl_save_position();
+
     bool sport_init(bool ignore_checks);
     void sport_run();
     void crash_check();
@@ -1102,6 +1073,8 @@ private:
 
     Copter::FlightMode_LOITER flightmode_loiter{*this};
 
+    Copter::FlightMode_RTL flightmode_rtl{*this};
+
 #if FRAME_CONFIG == HELI_FRAME
     Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this};
 #else

ArduCopter/FlightMode.h

@@ -510,3 +510,70 @@ private:
     void gps_run();
     void nogps_run();
 };
+
+
+
+class FlightMode_RTL : public FlightMode {
+
+public:
+
+    FlightMode_RTL(Copter &copter) :
+        Copter::FlightMode(copter)
+        { }
+
+    bool init(bool ignore_checks) override;
+    void run() override { // should be called at 100hz or more
+        return run(true);
+    }
+    void run(bool disarm_on_land); // should be called at 100hz or more
+
+    bool requires_GPS() const override { return true; }
+    bool has_manual_throttle() const override { return false; }
+    bool allows_arming(bool from_gcs) const override { return true; };
+    bool is_autopilot() const override { return true; }
+
+    RTLState state() { return _state; }
+
+    // this should probably not be exposed
+    bool state_complete() { return _state_complete; }
+
+    bool landing_gear_should_be_deployed();
+
+    void restart_without_terrain();
+
+protected:
+
+    const char *name() const override { return "RTL"; }
+    const char *name4() const override { return "RTL "; }
+
+private:
+
+    void climb_start();
+    void return_start();
+    void climb_return_run();
+    void loiterathome_start();
+    void loiterathome_run();
+    void descent_start();
+    void descent_run();
+    void land_start();
+    void land_run(bool disarm_on_land);
+    void build_path(bool terrain_following_allowed);
+    void compute_return_target(bool terrain_following_allowed);
+
+    // RTL
+    RTLState _state = RTL_InitialClimb;  // records state of rtl (initial climb, returning home, etc)
+    bool _state_complete = false; // set to true if the current state is completed
+
+    struct {
+        // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
+        Location_Class origin_point;
+        Location_Class climb_target;
+        Location_Class return_target;
+        Location_Class descent_target;
+        bool land;
+        bool terrain_used;
+    } rtl_path;
+
+    // Loiter timer - Records how long we have been in loiter
+    uint32_t _loiter_start_time = 0;
+};

ArduCopter/commands_logic.cpp

@@ -970,7 +970,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
 // returns true with RTL has completed successfully
 bool Copter::verify_RTL()
 {
-    return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
+    return (flightmode_rtl.state_complete() && (flightmode_rtl.state() == RTL_FinalDescent || flightmode_rtl.state() == RTL_Land));
 }
 
 // verify_spline_wp - check if we have reached the next way point using spline

ArduCopter/control_auto.cpp

@@ -417,7 +417,7 @@ bool Copter::FlightMode_AUTO::landing_gear_should_be_deployed()
     case Auto_Land:
         return true;
     case Auto_RTL:
-        switch(_copter.rtl_state) {
+        switch(_copter.flightmode_rtl.state()) {
         case RTL_LoiterAtHome:
         case RTL_Land:
         case RTL_FinalDescent:
@@ -438,7 +438,7 @@ void Copter::FlightMode_AUTO::rtl_start()
     _mode = Auto_RTL;
 
     // call regular rtl flight mode initialisation and ask it to ignore checks
-    _copter.rtl_init(true);
+    _copter.flightmode_rtl.init(true);
 }
 
 // auto_rtl_run - rtl in AUTO flight mode
@@ -446,7 +446,7 @@ void Copter::FlightMode_AUTO::rtl_start()
 void Copter::FlightMode_AUTO::rtl_run()
 {
     // call regular rtl flight mode run function
-    _copter.rtl_run(false);
+    _copter.flightmode_rtl.run(false);
 }
 
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location

ArduCopter/control_rtl.cpp

@@ -8,13 +8,13 @@
  */
 
 // rtl_init - initialise rtl controller
-bool Copter::rtl_init(bool ignore_checks)
+bool Copter::FlightMode_RTL::init(bool ignore_checks)
 {
-    if (position_ok() || ignore_checks) {
+    if (_copter.position_ok() || ignore_checks) {
         // initialise waypoint and spline controller
         wp_nav->wp_and_spline_init();
-        rtl_build_path(!failsafe.terrain);
+        build_path(!_copter.failsafe.terrain);
-        rtl_climb_start();
+        climb_start();
         return true;
     }else{
         return false;
@@ -22,35 +22,35 @@ bool Copter::rtl_init(bool ignore_checks)
 }
 
 // re-start RTL with terrain following disabled
-void Copter::rtl_restart_without_terrain()
+void Copter::FlightMode_RTL::restart_without_terrain()
 {
     // log an error
-    Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
+    _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL);
     if (rtl_path.terrain_used) {
-        rtl_build_path(false);
+        build_path(false);
-        rtl_climb_start();
+        climb_start();
         gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
     }
 }
 
 // rtl_run - runs the return-to-launch controller
 // should be called at 100hz or more
-void Copter::rtl_run(bool disarm_on_land)
+void Copter::FlightMode_RTL::run(bool disarm_on_land)
 {
     // check if we need to move to next state
-    if (rtl_state_complete) {
+    if (_state_complete) {
-        switch (rtl_state) {
+        switch (_state) {
         case RTL_InitialClimb:
-            rtl_return_start();
+            return_start();
             break;
         case RTL_ReturnHome:
-            rtl_loiterathome_start();
+            loiterathome_start();
             break;
         case RTL_LoiterAtHome:
-            if (rtl_path.land || failsafe.radio) {
+            if (rtl_path.land || _copter.failsafe.radio) {
-                rtl_land_start();
+                land_start();
             }else{
-                rtl_descent_start();
+                descent_start();
             }
             break;
         case RTL_FinalDescent:
@@ -63,35 +63,35 @@ void Copter::rtl_run(bool disarm_on_land)
     }
 
     // call the correct run function
-    switch (rtl_state) {
+    switch (_state) {
 
     case RTL_InitialClimb:
-        rtl_climb_return_run();
+        climb_return_run();
         break;
 
     case RTL_ReturnHome:
-        rtl_climb_return_run();
+        climb_return_run();
         break;
 
     case RTL_LoiterAtHome:
-        rtl_loiterathome_run();
+        loiterathome_run();
         break;
 
     case RTL_FinalDescent:
-        rtl_descent_run();
+        descent_run();
         break;
 
     case RTL_Land:
-        rtl_land_run(disarm_on_land);
+        land_run(disarm_on_land);
         break;
     }
 }
 
 // rtl_climb_start - initialise climb to RTL altitude
-void Copter::rtl_climb_start()
+void Copter::FlightMode_RTL::climb_start()
 {
-    rtl_state = RTL_InitialClimb;
+    _state = RTL_InitialClimb;
-    rtl_state_complete = false;
+    _state_complete = false;
 
     // RTL_SPEED == 0 means use WPNAV_SPEED
     if (g.rtl_speed_cms != 0) {
@@ -101,8 +101,8 @@ void Copter::rtl_climb_start()
     // set the destination
     if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
         // this should not happen because rtl_build_path will have checked terrain data was available
-        Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
+        _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
-        set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
+        _copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
         return;
     }
     wp_nav->set_fast_waypoint(true);
@@ -112,23 +112,23 @@ void Copter::rtl_climb_start()
 }
 
 // rtl_return_start - initialise return to home
-void Copter::rtl_return_start()
+void Copter::FlightMode_RTL::return_start()
 {
-    rtl_state = RTL_ReturnHome;
+    _state = RTL_ReturnHome;
-    rtl_state_complete = false;
+    _state_complete = false;
 
     if (!wp_nav->set_wp_destination(rtl_path.return_target)) {
         // failure must be caused by missing terrain data, restart RTL
-        rtl_restart_without_terrain();
+        restart_without_terrain();
     }
 
     // initialise yaw to point home (maybe)
-    set_auto_yaw_mode(get_default_auto_yaw_mode(true));
+    set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(true));
 }
 
 // rtl_climb_return_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
-void Copter::rtl_climb_return_run()
+void Copter::FlightMode_RTL::climb_return_run()
 {
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
@@ -148,7 +148,7 @@ void Copter::rtl_climb_return_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -160,13 +160,13 @@ void Copter::rtl_climb_return_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    failsafe_terrain_set_status(wp_nav->update_wpnav());
+    _copter.failsafe_terrain_set_status(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) {
+    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -175,18 +175,18 @@ void Copter::rtl_climb_return_run()
     }
 
     // check if we've completed this stage of RTL
-    rtl_state_complete = wp_nav->reached_wp_destination();
+    _state_complete = wp_nav->reached_wp_destination();
 }
 
 // rtl_loiterathome_start - initialise return to home
-void Copter::rtl_loiterathome_start()
+void Copter::FlightMode_RTL::loiterathome_start()
 {
-    rtl_state = RTL_LoiterAtHome;
+    _state = RTL_LoiterAtHome;
-    rtl_state_complete = false;
+    _state_complete = false;
-    rtl_loiter_start_time = millis();
+    _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) {
+    if(_copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) {
         set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW);
     } else {
         set_auto_yaw_mode(AUTO_YAW_HOLD);
@@ -195,7 +195,7 @@ void Copter::rtl_loiterathome_start()
 
 // 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
-void Copter::rtl_loiterathome_run()
+void Copter::FlightMode_RTL::loiterathome_run()
 {
     // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
@@ -215,7 +215,7 @@ void Copter::rtl_loiterathome_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -227,13 +227,13 @@ void Copter::rtl_loiterathome_run()
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
     // run waypoint controller
-    failsafe_terrain_set_status(wp_nav->update_wpnav());
+    _copter.failsafe_terrain_set_status(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) {
+    if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) {
         // roll & pitch from waypoint controller, yaw rate from pilot
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
     }else{
@@ -242,24 +242,24 @@ void Copter::rtl_loiterathome_run()
     }
 
     // check if we've completed this stage of RTL
-    if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
+    if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) {
-        if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
+        if (_copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) {
             // check if heading is within 2 degrees of heading when vehicle was armed
-            if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {
+            if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.initial_armed_bearing)) <= 200) {
-                rtl_state_complete = true;
+                _state_complete = true;
             }
         } else {
             // we have loitered long enough
-            rtl_state_complete = true;
+            _state_complete = true;
         }
     }
 }
 
 // rtl_descent_start - initialise descent to final alt
-void Copter::rtl_descent_start()
+void Copter::FlightMode_RTL::descent_start()
 {
-    rtl_state = RTL_FinalDescent;
+    _state = RTL_FinalDescent;
-    rtl_state_complete = false;
+    _state_complete = false;
 
     // Set wp navigation target to above home
     wp_nav->init_loiter_target(wp_nav->get_wp_destination());
@@ -273,7 +273,7 @@ void Copter::rtl_descent_start()
 
 // rtl_descent_run - implements the final descent to the RTL_ALT
 //      called by rtl_run at 100hz or more
-void Copter::rtl_descent_run()
+void Copter::FlightMode_RTL::descent_run()
 {
     int16_t roll_control = 0, pitch_control = 0;
     float target_yaw_rate = 0;
@@ -295,12 +295,12 @@ void Copter::rtl_descent_run()
     }
 
     // process pilot's input
-    if (!failsafe.radio) {
+    if (!_copter.failsafe.radio) {
-        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
+        if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
             Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
             // exit land if throttle is high
-            if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
+            if (!_copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
-                set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
+                _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
             }
         }
 
@@ -334,14 +334,14 @@ void Copter::rtl_descent_run()
     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
 
     // check if we've reached within 20cm of final altitude
-    rtl_state_complete = labs(rtl_path.descent_target.alt - current_loc.alt) < 20;
+    _state_complete = labs(rtl_path.descent_target.alt - _copter.current_loc.alt) < 20;
 }
 
 // rtl_loiterathome_start - initialise controllers to loiter over home
-void Copter::rtl_land_start()
+void Copter::FlightMode_RTL::land_start()
 {
-    rtl_state = RTL_Land;
+    _state = RTL_Land;
-    rtl_state_complete = false;
+    _state_complete = false;
 
     // Set wp navigation target to above home
     wp_nav->init_loiter_target(wp_nav->get_wp_destination());
@@ -356,9 +356,22 @@ void Copter::rtl_land_start()
     set_auto_yaw_mode(AUTO_YAW_HOLD);
 }
 
+bool Copter::FlightMode_RTL::landing_gear_should_be_deployed()
+{
+    switch(_state) {
+    case RTL_LoiterAtHome:
+    case RTL_Land:
+    case RTL_FinalDescent:
+        return true;
+    default:
+        return false;
+    }
+    return false;
+}
+
 // rtl_returnhome_run - return home
 //      called by rtl_run at 100hz or more
-void Copter::rtl_land_run(bool disarm_on_land)
+void Copter::FlightMode_RTL::land_run(bool disarm_on_land)
 {
     // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
     if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
@@ -376,25 +389,25 @@ void Copter::rtl_land_run(bool disarm_on_land)
 
         // disarm when the landing detector says we've landed
         if (ap.land_complete && disarm_on_land) {
-            init_disarm_motors();
+            _copter.init_disarm_motors();
         }
 
         // check if we've completed this stage of RTL
-        rtl_state_complete = ap.land_complete;
+        _state_complete = ap.land_complete;
         return;
     }
 
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
-    land_run_horizontal_control();
+    _copter.land_run_horizontal_control();
-    land_run_vertical_control();
+    _copter.land_run_vertical_control();
 
     // check if we've completed this stage of RTL
-    rtl_state_complete = ap.land_complete;
+    _state_complete = ap.land_complete;
 }
 
-void Copter::rtl_build_path(bool terrain_following_allowed)
+void Copter::FlightMode_RTL::build_path(bool terrain_following_allowed)
 {
     // origin point is our stopping point
     Vector3f stopping_point;
@@ -404,7 +417,7 @@ void Copter::rtl_build_path(bool terrain_following_allowed)
     rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME);
 
     // compute return target
-    rtl_compute_return_target(terrain_following_allowed);
+    compute_return_target(terrain_following_allowed);
 
     // climb target is above our origin point at the return altitude
     rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
@@ -419,28 +432,28 @@ void Copter::rtl_build_path(bool terrain_following_allowed)
 // compute the return target - home or rally point
 //   return altitude in cm above home at which vehicle should return home
 //   return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
-void Copter::rtl_compute_return_target(bool terrain_following_allowed)
+void Copter::FlightMode_RTL::compute_return_target(bool terrain_following_allowed)
 {
     // set return target to nearest rally point or home position (Note: alt is absolute)
 #if AC_RALLY == ENABLED
-    rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt);
+    rtl_path.return_target = _copter.rally.calc_best_rally_or_home_location(_copter.current_loc, ahrs.get_home().alt);
 #else
     rtl_path.return_target = ahrs.get_home();
 #endif
 
     // curr_alt is current altitude above home or above terrain depending upon use_terrain
-    int32_t curr_alt = current_loc.alt;
+    int32_t curr_alt = _copter.current_loc.alt;
 
     // decide if we should use terrain altitudes
-    rtl_path.terrain_used = terrain_use() && terrain_following_allowed;
+    rtl_path.terrain_used = _copter.terrain_use() && terrain_following_allowed;
     if (rtl_path.terrain_used) {
         // attempt to retrieve terrain alt for current location, stopping point and origin
         int32_t origin_terr_alt, return_target_terr_alt;
         if (!rtl_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
             !rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
-            !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
+            !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
             rtl_path.terrain_used = false;
-            Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
+            _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
         }
     }
 
@@ -478,10 +491,10 @@ void Copter::rtl_compute_return_target(bool terrain_following_allowed)
     //       if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to
     //       the vehicle not climbing at all as RTL begins.  This can be overly conservative and it might be better
     //       to apply the fence alt limit independently on the origin_point and return_target
-    if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
+    if ((_copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
         // get return target as alt-above-home so it can be compared to fence's alt
         if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) {
-            float fence_alt = fence.get_safe_alt_max()*100.0f;
+            float fence_alt = _copter.fence.get_safe_alt_max()*100.0f;
             if (target_alt > fence_alt) {
                 // reduce target alt to the fence alt
                 rtl_path.return_target.alt -= (target_alt - fence_alt);

ArduCopter/events.cpp

@@ -178,7 +178,7 @@ void Copter::failsafe_terrain_on_event()
     if (should_disarm_on_failsafe()) {
         init_disarm_motors();
     } else if (control_mode == RTL) {
-        rtl_restart_without_terrain();
+        flightmode_rtl.restart_without_terrain();
     } else {
         set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
     }

ArduCopter/flight_mode.cpp

@@ -96,7 +96,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
             break;
 
         case RTL:
-            success = rtl_init(ignore_checks);
+            success = flightmode_rtl.init(ignore_checks);
+            if (success) {
+                flightmode = &flightmode_rtl;
+            }
             break;
 
         case DRIFT:
@@ -209,10 +212,6 @@ void Copter::update_flight_mode()
 
     switch (control_mode) {
 
-        case RTL:
-            rtl_run();
-            break;
-
         case DRIFT:
             drift_run();
             break;
@@ -323,7 +322,6 @@ bool Copter::mode_requires_GPS()
         return flightmode->requires_GPS();
     }
     switch (control_mode) {
-        case RTL:
         case SMART_RTL:
         case DRIFT:
         case POSHOLD:
@@ -373,7 +371,6 @@ void Copter::notify_flight_mode()
         return;
     }
     switch (control_mode) {
-        case RTL:
         case AVOID_ADSB:
         case GUIDED_NOGPS:
         case SMART_RTL:
@@ -388,9 +385,6 @@ void Copter::notify_flight_mode()
 
     // set flight mode string
     switch (control_mode) {
-        case RTL:
-            notify.set_flight_mode_str("RTL ");
-            break;
         case DRIFT:
             notify.set_flight_mode_str("DRIF");
             break;

ArduCopter/heli.cpp

@@ -23,8 +23,8 @@ void Copter::heli_init()
 // should be called at 50hz
 void Copter::check_dynamic_flight(void)
 {
-    if (!motor->armed() || !motors->rotor_runup_complete() ||
+    if (!motors->armed() || !motors->rotor_runup_complete() ||
-        control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) {
+        control_mode == LAND || (control_mode==RTL && flightmode_rtl.state() == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) {
         heli_dynamic_flight_counter = 0;
         heli_flags.dynamic_flight = false;
         return;
@@ -109,7 +109,7 @@ void Copter::heli_update_landing_swash()
 
         case RTL:
         case SMART_RTL:
-            if (rtl_state == RTL_Land) {
+            if (flightmode_rtl.state() == RTL_Land) {
                 motors->set_collective_for_landing(true);
             }else{
                 motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);

ArduCopter/landing_gear.cpp

@@ -15,7 +15,7 @@ void Copter::landinggear_update()
     // if we are doing an automatic landing procedure, force the landing gear to deploy.
     // To-Do: should we pause the auto-land procedure to give time for gear to come down?
     if (control_mode == LAND ||
-       (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
+       (control_mode == RTL && flightmode_rtl.landing_gear_should_be_deployed()) ||
        (control_mode == AUTO && flightmode_auto.landing_gear_should_be_deployed())) {
         landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed);
     }