Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: support DO_RETURN_PATH_START mission item and command #28574

Merged
merged 1 commit into from
Dec 2, 2024
Merged

Plane: support DO_RETURN_PATH_START mission item and command #28574

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions ArduPlane/AP_Arming.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()) {
Expand Down
20 changes: 18 additions & 2 deletions ArduPlane/GCS_Mavlink.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
IamPete1 marked this conversation as resolved.
Show resolved Hide resolved
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;

Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)),

Expand Down
2 changes: 2 additions & 0 deletions ArduPlane/commands_logic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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:
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/defines.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
17 changes: 15 additions & 2 deletions ArduPlane/mode_rtl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

is this a separate bug fix? or related somehow?

Copy link
Member Author

@IamPete1 IamPete1 Nov 13, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Related in that the same pattern is in the new case. If the mode change failed then mission was left with force resume true. This means the next time you enter auto you will resume the mission and not restart ignoring MIS_RESTART 1 if it is set.

edit: I'm sure you spotted it but we set set_force_resume(true) before we try and change mode up on line 113

}

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

Expand Down
Loading