diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index a15507de75..871b2032b5 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1419,10 +1419,8 @@ static void update_navigation() nav_controller->reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence - if (!auto_state.checked_for_autoland && - mission.jump_to_landing_sequence()) { - gcs_send_text_P(SEVERITY_LOW, PSTR("Starting auto landing")); - set_mode(AUTO); + if (!auto_state.checked_for_autoland) { + jump_to_landing_sequence(); } // prevent running the expensive jump_to_landing_sequence // on every loop diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index d26b932425..200b296629 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1111,12 +1111,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_DO_LAND_START: result = MAV_RESULT_FAILED; - + // attempt to switch to next DO_LAND_START command in the mission - if (mission.jump_to_landing_sequence()) { - set_mode(AUTO); + if (jump_to_landing_sequence()) { result = MAV_RESULT_ACCEPTED; - } + } break; case MAV_CMD_DO_FENCE_ENABLE: diff --git a/ArduPlane/landing.pde b/ArduPlane/landing.pde index 675acc1c3e..147e9ab9a6 100644 --- a/ArduPlane/landing.pde +++ b/ArduPlane/landing.pde @@ -115,3 +115,26 @@ static void setup_landing_glide_slope(void) // stay within the range of the start and end locations in altitude constrain_target_altitude_location(loc, prev_WP_loc); } + +/* find the nearest landing sequence starting point (DO_LAND_START) and + * switch to that mission item. Returns false if no DO_LAND_START available.*/ +static bool jump_to_landing_sequence(void) { + uint16_t land_idx = mission.get_landing_sequence_start(); + if (land_idx != 0) { + + set_mode(AUTO); + + if (mission.set_current_cmd(land_idx)) { + //if the mission has ended it has to be restarted + if (mission.state() == AP_Mission::MISSION_STOPPED) { + mission.resume(); + } + + gcs_send_text_P(SEVERITY_LOW, PSTR("Landing sequence begun.")); + return true; + } + } + + gcs_send_text_P(SEVERITY_HIGH, PSTR("Unable to start landing sequence.")); + return false; +}