Plane: support DO_RETURN_PATH_START misison item and command

ArduPilot · Nov 10, 2024 · a5870b2 · a5870b2
1 parent 81a30e8
commit a5870b2
Show file tree

Hide file tree

Showing 6 changed files with 46 additions and 9 deletions.
diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp
@@ -433,9 +433,15 @@ bool AP_Arming_Plane::mission_checks(bool report)
 {
     // base checks
     bool ret = AP_Arming::mission_checks(report);
-    if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
-        ret = false;
-        check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
+    if (plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
+        if (plane.mission.contains_item(MAV_CMD_DO_LAND_START)) {
+            ret = false;
+            check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
+        }
+        if (plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) {
+            ret = false;
+            check_failed(ARMING_CHECK_MISSION, report, "DO_RETURN_PATH_START set and RTL_AUTOLAND disabled");
+        }
     }
 #if HAL_QUADPLANE_ENABLED
     if (plane.quadplane.available()) {

diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp
@@ -1056,11 +1056,27 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
     case MAV_CMD_DO_GO_AROUND:
         return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
 
+    case MAV_CMD_DO_RETURN_PATH_START:
+        // attempt to rejoin after the next DO_RETURN_PATH_START command in the mission
+        if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
+            plane.mission.set_force_resume(true);
+            if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
+                return MAV_RESULT_ACCEPTED;
+            }
+            // mode change failed, revert force resume flag
+            plane.mission.set_force_resume(false);
+        }
+        return MAV_RESULT_FAILED;
+
     case MAV_CMD_DO_LAND_START:
         // attempt to switch to next DO_LAND_START command in the mission
         if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
-            plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
-            return MAV_RESULT_ACCEPTED;
+            plane.mission.set_force_resume(true);
+            if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
+                return MAV_RESULT_ACCEPTED;
+            }
+            // mode change failed, revert force resume flag
+            plane.mission.set_force_resume(false);
         }
         return MAV_RESULT_FAILED;
 

diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
@@ -737,7 +737,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Param: RTL_AUTOLAND
     // @DisplayName: RTL auto land
     // @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).
-    // @Values: 0:Disable,1:Fly HOME then land,2:Go directly to landing sequence, 3:OnlyForGoAround
+    // @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item
     // @User: Standard
     GSCALAR(rtl_autoland,         "RTL_AUTOLAND",   float(RtlAutoland::RTL_DISABLE)),
 

diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
@@ -138,6 +138,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         }
         break;
 
+    case MAV_CMD_DO_RETURN_PATH_START:
     case MAV_CMD_DO_LAND_START:
         break;
 
@@ -304,6 +305,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd)        // Ret
     case MAV_CMD_DO_CHANGE_SPEED:
     case MAV_CMD_DO_SET_HOME:
     case MAV_CMD_DO_INVERTED_FLIGHT:
+    case MAV_CMD_DO_RETURN_PATH_START:
     case MAV_CMD_DO_LAND_START:
     case MAV_CMD_DO_FENCE_ENABLE:
     case MAV_CMD_DO_AUTOTUNE_ENABLE:

diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
@@ -61,8 +61,8 @@ enum class RtlAutoland {
     RTL_THEN_DO_LAND_START = 1,
     RTL_IMMEDIATE_DO_LAND_START = 2,
     NO_RTL_GO_AROUND = 3,
+    DO_RETURN_PATH_START = 4,
 };
-
 
 enum ChannelMixing {
     MIXING_DISABLED = 0,

diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp
@@ -106,8 +106,7 @@ void ModeRTL::navigate()
         if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) ||
             (plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START &&
             plane.reached_loiter_target() && 
-            labs(plane.calc_altitude_error_cm()) < 1000))
-            {
+            labs(plane.calc_altitude_error_cm()) < 1000)) {
                 // we've reached the RTL point, see if we have a landing sequence
                 if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
                     // switch from RTL -> AUTO
@@ -116,12 +115,26 @@ void ModeRTL::navigate()
                         // return here so we don't change the radius and don't run the rtl update_loiter()
                         return;
                     }
+                    // mode change failed, revert force resume flag
+                    plane.mission.set_force_resume(false);
                 }
 
                 // prevent running the expensive jump_to_landing_sequence
                 // on every loop
                 plane.auto_state.checked_for_autoland = true;
+
+        } else if (plane.g.rtl_autoland == RtlAutoland::DO_RETURN_PATH_START) {
+            if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
+                plane.mission.set_force_resume(true);
+                if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) {
+                    // return here so we don't change the radius and don't run the rtl update_loiter()
+                    return;
+                }
+                // mode change failed, revert force resume flag
+                plane.mission.set_force_resume(false);
             }
+            plane.auto_state.checked_for_autoland = true;
+        }
     }
 }