diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 8cceb0e52a..d28efbfb58 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -836,6 +836,7 @@ void Plane::update_flight_stage(void) } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { if ((g.land_abort_throttle_enable && channel_throttle->control_in > 95) || + auto_state.commanded_go_around || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){ // abort mode is sticky, it must complete while executing NAV_LAND set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f3124a1318..e0af13a13d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1384,9 +1384,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //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.flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) { - //Just tell the autopilot we're done landing so it will - //proceed to the next mission item. If there is no next mission - //item the plane will head to home point and loiter. + // 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; + } plane.auto_state.commanded_go_around = true; result = MAV_RESULT_ACCEPTED; diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index f08a359c4a..6fe63ef22a 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -16,15 +16,6 @@ bool Plane::verify_land() // so we don't verify command completion. Instead we use this to // 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 // the altitude has been reached, restart the landing sequence if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {