Sub: Implement MAV_CMD_NAV_RETURN_TO_LAUNCH

ArduSub/Sub.h

@@ -766,6 +766,7 @@ private:
     bool do_guided(const AP_Mission::Mission_Command& cmd);
     void do_nav_wp(const AP_Mission::Mission_Command& cmd);
     void do_surface(const AP_Mission::Mission_Command& cmd);
+    void do_RTL(void);
     void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd);
     void do_circle(const AP_Mission::Mission_Command& cmd);
     void do_loiter_time(const AP_Mission::Mission_Command& cmd);
@@ -791,6 +792,7 @@ private:
 #endif
     bool verify_nav_wp(const AP_Mission::Mission_Command& cmd);
     bool verify_surface(const AP_Mission::Mission_Command& cmd);
+    bool verify_RTL(void);
     bool verify_circle(const AP_Mission::Mission_Command& cmd);
     bool verify_spline_wp(const AP_Mission::Mission_Command& cmd);
 #if NAV_GUIDED == ENABLED

ArduSub/commands_logic.cpp

@@ -23,6 +23,10 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
         do_surface(cmd);
         break;
 
+    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+        do_RTL();
+        break;
+
     case MAV_CMD_NAV_LOITER_UNLIM:              // 17 Loiter indefinitely
         do_loiter_unlimited(cmd);
         break;
@@ -179,6 +183,9 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
     case MAV_CMD_NAV_LAND:
         return verify_surface(cmd);
 
+    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+        return verify_RTL();
+
     case MAV_CMD_NAV_LOITER_UNLIM:
         return verify_loiter_unlimited();
 
@@ -323,6 +330,11 @@ void Sub::do_surface(const AP_Mission::Mission_Command& cmd)
     auto_wp_start(target_location);
 }
 
+void Sub::do_RTL()
+{
+    auto_wp_start(ahrs.get_home());
+}
+
 // do_loiter_unlimited - start loitering with no end conditions
 // note: caller should set yaw_mode
 void Sub::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
@@ -606,6 +618,10 @@ bool Sub::verify_surface(const AP_Mission::Mission_Command& cmd)
     return retval;
 }
 
+bool Sub::verify_RTL() {
+    return wp_nav.reached_wp_destination();
+}
+
 bool Sub::verify_loiter_unlimited()
 {
     return false;