mirror of https://github.com/ArduPilot/ardupilot
Plane: use abort landing logic with mavlink GO_AROUND cmd
- also enabled use of CMD_DO_GO_AROUND altitude param
This commit is contained in:
parent
d18c25a0f9
commit
0bfe235d6b
|
@ -836,6 +836,7 @@ void Plane::update_flight_stage(void)
|
||||||
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
|
} else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
|
||||||
|
|
||||||
if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) ||
|
if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) ||
|
||||||
|
auto_state.commanded_go_around ||
|
||||||
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
|
flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
|
||||||
// abort mode is sticky, it must complete while executing NAV_LAND
|
// abort mode is sticky, it must complete while executing NAV_LAND
|
||||||
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
|
set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);
|
||||||
|
|
|
@ -1384,9 +1384,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||||
//Not allowing go around at FLIGHT_LAND_FINAL stage on purpose --
|
//Not allowing go around at FLIGHT_LAND_FINAL stage on purpose --
|
||||||
//if plane is close to the ground a go around coudld be dangerous.
|
//if plane is close to the ground a go around coudld be dangerous.
|
||||||
if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
if (plane.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
|
||||||
//Just tell the autopilot we're done landing so it will
|
// Initiate an aborted landing. This will trigger a pitch-up and
|
||||||
//proceed to the next mission item. If there is no next mission
|
// climb-out to a safe altitude holding heading then one of the
|
||||||
//item the plane will head to home point and loiter.
|
// 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;
|
||||||
|
}
|
||||||
plane.auto_state.commanded_go_around = true;
|
plane.auto_state.commanded_go_around = true;
|
||||||
|
|
||||||
result = MAV_RESULT_ACCEPTED;
|
result = MAV_RESULT_ACCEPTED;
|
||||||
|
|
|
@ -16,15 +16,6 @@ bool Plane::verify_land()
|
||||||
// so we don't verify command completion. Instead we use this to
|
// so we don't verify command completion. Instead we use this to
|
||||||
// adjust final landing parameters
|
// adjust final landing parameters
|
||||||
|
|
||||||
// If a go around has been commanded, we are done landing. This will send
|
|
||||||
// the mission to the next mission item, which presumably is a mission
|
|
||||||
// segment with operations to perform when a landing is called off.
|
|
||||||
// If there are no commands after the land waypoint mission item then
|
|
||||||
// the plane will proceed to loiter about its home point.
|
|
||||||
if (auto_state.commanded_go_around) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
|
// when aborting a landing, mimic the verify_takeoff with steering hold. Once
|
||||||
// the altitude has been reached, restart the landing sequence
|
// the altitude has been reached, restart the landing sequence
|
||||||
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
|
||||||
|
|
Loading…
Reference in New Issue