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

Plane: Fly a planned abort sequence if available

This commit is contained in:
Michael du Breuil 2018-12-05 16:28:27 -07:00 committed by Andrew Tridgell
parent 670eaf8782
commit e8885ee1a5
1 changed files with 31 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduPlane/GCS_Mavlink.cpp
View File

@ -952,21 +952,38 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
return MAV_RESULT_FAILED;
case MAV_CMD_DO_GO_AROUND:
if (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Initiate an aborted landing. This will trigger a pitch-up and
// climb-out to a safe altitude holding heading then one of the
// following actions will occur, check for in this order:
// - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
// increment mission index to execute it
// - else if DO_LAND_START is available, jump to it
// - else decrement the mission index to repeat the landing approach
{
uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
bool is_in_landing = (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) ||
(mission_id == MAV_CMD_NAV_LAND) ||
(mission_id == MAV_CMD_NAV_VTOL_LAND);
if (is_in_landing) {
// fly a user planned abort pattern if available
if (plane.mission.jump_to_abort_landing_sequence()) {
return MAV_RESULT_ACCEPTED;
}
if (!is_zero(packet.param1)) {
plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
}
if (plane.landing.request_go_around()) {
plane.auto_state.next_wp_crosstrack = false;
return MAV_RESULT_ACCEPTED;
// only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
// shooting a quadplane approach
if ((!plane.quadplane.available()) ||
((!plane.quadplane.in_vtol_auto()) &&
(!(plane.quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)))) {
// Initiate an aborted landing. This will trigger a pitch-up and
// climb-out to a safe altitude holding heading then one of the
// following actions will occur, check for in this order:
// - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
// increment mission index to execute it
// - else if DO_LAND_START is available, jump to it
// - else decrement the mission index to repeat the landing approach
if (!is_zero(packet.param1)) {
plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
}
if (plane.landing.request_go_around()) {
plane.auto_state.next_wp_crosstrack = false;
return MAV_RESULT_ACCEPTED;
}
}
}
}
return MAV_RESULT_FAILED;