Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: support DO_RETURN_PATH_START misison item and command

This commit is contained in:
Iampete1 2024-11-10 11:56:58 +00:00 committed by Andrew Tridgell
parent 445c03c69c
commit a79fcdbfcd
6 changed files with 46 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduPlane/AP_Arming.cpp
View File

@ -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()) {

20
ArduPlane/GCS_Mavlink.cpp
View File

@ -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;

2
ArduPlane/Parameters.cpp
View File

@ -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)),

2
ArduPlane/commands_logic.cpp
View File

@ -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:

2
ArduPlane/defines.h
View File

@ -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 {

17
ArduPlane/mode_rtl.cpp
View File

@ -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;
}
}
}