From a79fcdbfcdd9f21792acea39c72474e95bc80037 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 10 Nov 2024 11:56:58 +0000 Subject: [PATCH] Plane: support DO_RETURN_PATH_START misison item and command --- ArduPlane/AP_Arming.cpp | 12 +++++++++--- ArduPlane/GCS_Mavlink.cpp | 20 ++++++++++++++++++-- ArduPlane/Parameters.cpp | 2 +- ArduPlane/commands_logic.cpp | 2 ++ ArduPlane/defines.h | 2 +- ArduPlane/mode_rtl.cpp | 17 +++++++++++++++-- 6 files changed, 46 insertions(+), 9 deletions(-) diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index 643f93a6c1..fca35f8d08 100644 --- 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 index 298b4e47a5..7aaba22f70 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1072,11 +1072,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 index 04a5ba3a59..8dcd235220 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -746,7 +746,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 index 90c3e02480..2110b1455d 100644 --- 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 index a3da572611..561e1deb39 100644 --- 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, }; - // PID broadcast bitmask enum tuning_pid_bits { diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index b0d8cfbfd6..a6f2fef892 100644 --- 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; + } } }