Sub: Remove RTL

ArduSub/GCS_Mavlink.cpp

@@ -48,7 +48,6 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
     base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
     switch (control_mode) {
     case AUTO:
-    case RTL:
     case VELHOLD:
     case GUIDED:
     case CIRCLE:
@@ -168,7 +167,6 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
     case AUTO:
     case GUIDED:
     case VELHOLD:
-    case RTL:
     case CIRCLE:
     case SURFACE:
     case OF_LOITER:
@@ -1268,12 +1266,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             }
             break;
 
-        case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-            if (sub.set_mode(RTL, MODE_REASON_GCS_COMMAND)) {
-                result = MAV_RESULT_ACCEPTED;
-            }
-            break;
-
 // Not supported in sub
 //        case MAV_CMD_NAV_LAND:
 //            if (sub.set_mode(LAND, MODE_REASON_GCS_COMMAND)) {

ArduSub/Parameters.cpp

@@ -104,33 +104,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @Bitmask: 0:Roll,1:Pitch,2:Yaw
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
-    // @Param: RTL_ALT
-    // @DisplayName: RTL Altitude
-    // @Description: The minimum relative altitude the model will move to before Returning to Launch.  Set to zero to return at current altitude.
-    // @Units: Centimeters
-    // @Range: 0 8000
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(rtl_altitude,   "RTL_ALT",     RTL_ALT),
-
-	// @Param: RTL_CONE_SLOPE
-	// @DisplayName: RTL cone slope
-	// @Description: Defines a cone above home which determines maximum climb
-	// @Range: 0.5 10.0
-	// @Increment: .1
-	// @Values: 0:Disabled,1:Shallow,3:Steep
-	// @User: Standard
-	GSCALAR(rtl_cone_slope,   "RTL_CONE_SLOPE",     RTL_CONE_SLOPE_DEFAULT),
-
-    // @Param: RTL_SPEED
-    // @DisplayName: RTL speed
-    // @Description: Defines the speed in cm/s which the aircraft will attempt to maintain horizontally while flying home. If this is set to zero, WPNAV_SPEED will be used instead.
-    // @Units: cm/s
-    // @Range: 0 2000
-    // @Increment: 50
-    // @User: Standard
-    GSCALAR(rtl_speed_cms,   "RTL_SPEED",     0),
-
     // @Param: RNGFND_GAIN
     // @DisplayName: Rangefinder gain
     // @Description: Used to adjust the speed with which the target altitude is changed when objects are sensed below the copter
@@ -232,24 +205,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(compass_enabled,        "MAG_ENABLE",   MAGNETOMETER),
 
-    // @Param: RTL_ALT_FINAL
-    // @DisplayName: RTL Final Altitude
-    // @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 zero to land.
-    // @Units: Centimeters
-    // @Range: -1 1000
-    // @Increment: 1
-    // @User: Standard
-    GSCALAR(rtl_alt_final,  "RTL_ALT_FINAL", RTL_ALT_FINAL),
-
-    // @Param: RTL_CLIMB_MIN
-    // @DisplayName: RTL minimum climb
-    // @Description: The vehicle will climb this many cm during the initial climb portion of the RTL
-    // @Units: Centimeters
-    // @Range: 0 3000
-    // @Increment: 10
-    // @User: Standard
-    GSCALAR(rtl_climb_min,  "RTL_CLIMB_MIN",    RTL_CLIMB_MIN_DEFAULT),
-
     // @Param: WP_YAW_BEHAVIOR
     // @DisplayName: Yaw behaviour during missions
     // @Description: Determines how the autopilot controls the yaw during missions and RTL
@@ -257,15 +212,6 @@ const AP_Param::Info Sub::var_info[] = {
     // @User: Standard
     GSCALAR(wp_yaw_behavior,  "WP_YAW_BEHAVIOR",    WP_YAW_BEHAVIOR_DEFAULT),
 
-    // @Param: RTL_LOIT_TIME
-    // @DisplayName: RTL loiter time
-    // @Description: Time (in milliseconds) to loiter above home before beginning final descent
-    // @Units: ms
-    // @Range: 0 60000
-    // @Increment: 1000
-    // @User: Standard
-    GSCALAR(rtl_loiter_time,      "RTL_LOIT_TIME",    RTL_LOITER_TIME),
-
     // @Param: PILOT_VELZ_MAX
     // @DisplayName: Pilot maximum vertical speed
     // @Description: The maximum vertical velocity the pilot may request in cm/s

ArduSub/Parameters.h

@@ -144,13 +144,6 @@ public:
         k_param_gcs3,
         k_param_gcs_pid_mask,    // 126
 
-        //
-        // 135 : reserved for Solo until features merged with master
-        //
-        k_param_rtl_speed_cms = 135,
-        k_param_fs_batt_curr_rtl,
-		k_param_rtl_cone_slope, // 137
-
         //
         // 140: Sensor parameters
         //
@@ -160,13 +153,6 @@ public:
         k_param_ch7_option,
         k_param_ahrs, // AHRS group // 159
 
-        //
-        // 160: Navigation parameters
-        //
-        k_param_rtl_altitude = 160,
-        k_param_rtl_loiter_time,
-        k_param_rtl_alt_final,
-
         //
         // Camera and mount parameters
         //
@@ -224,7 +210,6 @@ public:
         k_param_autotune_aggressiveness,
         k_param_pi_vel_xy,
         k_param_fs_ekf_action,
-        k_param_rtl_climb_min,
         k_param_rpm_sensor,
         k_param_autotune_min_d, // 251
         k_param_DataFlash = 253, // 253 - Logging Group
@@ -290,9 +275,6 @@ public:
     AP_Float        throttle_filt;
     AP_Int16        throttle_behavior;
 
-    AP_Int16        rtl_altitude;
-    AP_Int16        rtl_speed_cms;
-    AP_Float        rtl_cone_slope;
     AP_Float        rangefinder_gain;
 
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled
@@ -312,15 +294,12 @@ public:
     AP_Int16        gps_hdop_good;              // GPS Hdop value at or below this value represent a good position
 
     AP_Int8         compass_enabled;
-    AP_Int16        rtl_alt_final;
-    AP_Int16        rtl_climb_min;              // rtl minimum climb in cm
 
     AP_Int8         wp_yaw_behavior;            // controls how the autopilot controls yaw during missions
     AP_Int8         rc_feel_rp;                 // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
     
     // Waypoints
     //
-    AP_Int32        rtl_loiter_time;
     AP_Int16        pilot_velocity_z_max;        // maximum vertical velocity the pilot may request
     AP_Int16        pilot_accel_z;               // vertical acceleration the pilot may request
 

ArduSub/Sub.cpp

@@ -36,8 +36,6 @@ Sub::Sub(void) :
     wp_distance(0),
     auto_mode(Auto_WP),
     guided_mode(Guided_WP),
-    rtl_state(RTL_InitialClimb),
-    rtl_state_complete(false),
     circle_pilot_yaw_override(false),
     simple_cos_yaw(1.0f),
     simple_sin_yaw(0.0f),
@@ -73,7 +71,6 @@ Sub::Sub(void) :
     pmTest1(0),
     fast_loopTimer(0),
     mainLoop_count(0),
-    rtl_loiter_start_time(0),
     auto_trim_counter(0),
     ServoRelayEvents(relay),
 #if CAMERA == ENABLED

ArduSub/Sub.h

@@ -331,20 +331,6 @@ private:
     // Guided
     GuidedMode guided_mode;  // controls which controller is run (pos or vel)
 
-    // 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;
-
     // Circle
     bool circle_pilot_yaw_override; // true if pilot is overriding yaw
 
@@ -444,8 +430,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;
 
     // Used to exit the roll and pitch auto trim function
     uint8_t auto_trim_counter;
@@ -614,11 +598,9 @@ private:
     bool far_from_EKF_origin(const Location& loc);
     void set_system_time_from_GPS();
     void exit_mission();
-    void do_RTL(void);
     bool verify_land();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();
-    bool verify_RTL();
     bool verify_wait_delay();
     bool verify_within_distance();
     bool verify_yaw();
@@ -639,8 +621,6 @@ private:
     void auto_land_start();
     void auto_land_start(const Vector3f& destination);
     void auto_land_run();
-    void auto_rtl_start();
-    void auto_rtl_run();
     void auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m);
     void auto_circle_start();
     void auto_circle_run();
@@ -704,20 +684,6 @@ private:
     bool poshold_init(bool ignore_checks);
     void poshold_run();
 
-    bool rtl_init(bool ignore_checks);
-    void rtl_restart_without_terrain();
-    void rtl_run();
-    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();
-    void rtl_build_path(bool terrain_following_allowed);
-    void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed);
     bool transect_init(bool ignore_checks);
     void transect_run();
     bool stabilize_init(bool ignore_checks);

ArduSub/arming_checks.cpp

@@ -482,13 +482,6 @@ bool Sub::pre_arm_terrain_check(bool display_failure)
         return true;
     }
 
-    // check if terrain following is enabled, using a range finder but RTL_ALT is higher than rangefinder's max range
-    // To-Do: modify RTL return path to fly at or above the RTL_ALT and remove this check
-    if ((rangefinder.num_sensors() > 0) && (g.rtl_altitude > rangefinder.max_distance_cm())) {
-        gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RTL_ALT above rangefinder max range");
-        return false;
-    }
-
     // show terrain statistics
     uint16_t terr_pending, terr_loaded;
     terrain.get_statistics(terr_pending, terr_loaded);

ArduSub/commands_logic.cpp

@@ -35,10 +35,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         do_loiter_time(cmd);
         break;
 
-    case MAV_CMD_NAV_RETURN_TO_LAUNCH:             //20
-        do_RTL();
-        break;
-
     case MAV_CMD_NAV_SPLINE_WAYPOINT:           // 82  Navigate to Waypoint using spline
         do_spline_wp(cmd);
         break;
@@ -189,9 +185,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LOITER_TIME:
         return verify_loiter_time();
 
-    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
-        return verify_RTL();
-
     case MAV_CMD_NAV_SPLINE_WAYPOINT:
         return verify_spline_wp(cmd);
 
@@ -252,17 +245,6 @@ void Sub::exit_mission()
     }
 }
 
-/********************************************************************************/
-//
-/********************************************************************************/
-
-// do_RTL - start Return-to-Launch
-void Sub::do_RTL(void)
-{
-    // start rtl in auto flight mode
-    auto_rtl_start();
-}
-
 /********************************************************************************/
 //	Nav (Must) commands
 /********************************************************************************/
@@ -635,14 +617,6 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
     return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
 }
 
-// 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
-bool Sub::verify_RTL()
-{
-    return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
-}
-
 // verify_spline_wp - check if we have reached the next way point using spline
 bool Sub::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
 {

ArduSub/control_auto.cpp

@@ -66,10 +66,6 @@ void Sub::auto_run()
         auto_land_run();
         break;
 
-    case Auto_RTL:
-        auto_rtl_run();
-        break;
-
     case Auto_Circle:
         auto_circle_run();
         break;
@@ -437,23 +433,6 @@ void Sub::auto_land_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());
 }
 
-// auto_rtl_start - initialises RTL in AUTO flight mode
-void Sub::auto_rtl_start()
-{
-    auto_mode = Auto_RTL;
-
-    // call regular rtl flight mode initialisation and ask it to ignore checks
-    rtl_init(true);
-}
-
-// auto_rtl_run - rtl in AUTO flight mode
-//      called by auto_run at 100hz or more
-void Sub::auto_rtl_run()
-{
-    // call regular rtl flight mode run function
-    rtl_run();
-}
-
 // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
 //  we assume the caller has set the circle's circle with circle_nav.set_center()
 //  we assume the caller has performed all required GPS_ok checks

ArduSub/defines.h

@@ -42,7 +42,10 @@ enum aux_sw_func {
     AUXSW_DO_NOTHING =           0, // aux switch disabled
 //    AUXSW_FLIP =                 2, // flip
     AUXSW_SIMPLE_MODE =          3, // change to simple mode
-    AUXSW_RTL =                  4, // change to RTL flight mode
+
+// No RTL mode for Sub
+//    AUXSW_RTL =                  4, // change to RTL flight mode
+
     AUXSW_SAVE_TRIM =            5, // save current position as level
     AUXSW_SAVE_WP =              7, // save mission waypoint or RTL if in auto mode
     AUXSW_CAMERA_TRIGGER =       9, // trigger camera servo or relay
@@ -112,7 +115,7 @@ enum control_mode_t {
     AUTO =          3,  // not implemented in sub // fully automatic waypoint control using mission commands
     GUIDED =        4,  // not implemented in sub // fully automatic fly to coordinate or fly at velocity/direction using GCS immediate commands
     VELHOLD =       5,  // automatic x/y velocity control and automatic depth/throttle
-    RTL =           6,  // not implemented in sub // automatic return to launching point
+//    RTL =           6,  // not implemented in sub // automatic return to launching point
     CIRCLE =        7,  // not implemented in sub // automatic circular flight with automatic throttle
     SURFACE =       9,  // automatically return to surface, pilot maintains horizontal control
     OF_LOITER =    10,  // deprecated
@@ -207,7 +210,7 @@ enum AutoMode {
     Auto_TakeOff,
     Auto_WP,
     Auto_Land,
-    Auto_RTL,
+//    Auto_RTL,
     Auto_CircleMoveToEdge,
     Auto_Circle,
     Auto_Spline,

ArduSub/events.cpp

@@ -220,7 +220,7 @@ void Sub::failsafe_gcs_check()
 void Sub::failsafe_terrain_check()
 {
     // trigger with 5 seconds of failures while in AUTO mode
-    bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL);
+    bool valid_mode = (control_mode == AUTO || control_mode == GUIDED);
     bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
     bool trigger_event = valid_mode && timeout;
 

ArduSub/flight_mode.cpp

@@ -59,10 +59,6 @@ bool Sub::set_mode(control_mode_t mode, mode_reason_t reason)
             success = surface_init(ignore_checks);
             break;
 
-        case RTL:
-            success = rtl_init(ignore_checks);
-            break;
-
 #if TRANSECT_ENABLED == ENABLED
         case TRANSECT:
             success = transect_init(ignore_checks);
@@ -162,10 +158,6 @@ void Sub::update_flight_mode()
             surface_run();
             break;
 
-        case RTL:
-            rtl_run();
-            break;
-
 #if TRANSECT_ENABLED == ENABLED
         case TRANSECT:
             transect_run();
@@ -226,7 +218,6 @@ bool Sub::mode_requires_GPS(control_mode_t mode) {
         case AUTO:
         case GUIDED:
         case VELHOLD:
-        case RTL:
         case CIRCLE:
         case POSHOLD:
         case TRANSECT:
@@ -266,7 +257,6 @@ void Sub::notify_flight_mode(control_mode_t mode) {
     switch(mode) {
         case AUTO:
         case GUIDED:
-        case RTL:
         case CIRCLE:
         case SURFACE:
             // autopilot modes
@@ -303,9 +293,6 @@ void Sub::print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode)
     case VELHOLD:
         port->print("VELHOLD");
         break;
-    case RTL:
-        port->print("RTL");
-        break;
     case CIRCLE:
         port->print("CIRCLE");
         break;

ArduSub/navigation.cpp

@@ -28,7 +28,7 @@ void Sub::calc_wp_distance()
     // get target from loiter or wpinav controller
     if (control_mode == CIRCLE) {
         wp_distance = wp_nav.get_loiter_distance_to_target();
-    }else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
+    }else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_distance = wp_nav.get_wp_distance_to_destination();
     }else{
         wp_distance = 0;
@@ -41,7 +41,7 @@ void Sub::calc_wp_bearing()
     // get target from loiter or wpinav controller
     if (control_mode == CIRCLE) {
         wp_bearing = wp_nav.get_loiter_bearing_to_target();
-    } else if (control_mode == AUTO || control_mode == RTL || (control_mode == GUIDED && guided_mode == Guided_WP)) {
+    } else if (control_mode == AUTO || (control_mode == GUIDED && guided_mode == Guided_WP)) {
         wp_bearing = wp_nav.get_wp_bearing_to_destination();
     } else {
         wp_bearing = 0;