diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 785ba6eab4..bf5f9b04ab 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -802,7 +802,10 @@ void Plane::update_navigation() reached_loiter_target() && labs(altitude_error_cm) < 1000) { // we've reached the RTL point, see if we have a landing sequence - jump_to_landing_sequence(); + if (landing.jump_to_landing_sequence()) { + // switch from RTL -> AUTO + set_mode(AUTO, MODE_REASON_UNKNOWN); + } // prevent running the expensive jump_to_landing_sequence // on every loop @@ -811,7 +814,10 @@ void Plane::update_navigation() else if (g.rtl_autoland == 2 && !landing.checked_for_autoland) { // Go directly to the landing sequence - jump_to_landing_sequence(); + if (landing.jump_to_landing_sequence()) { + // switch from RTL -> AUTO + set_mode(AUTO, MODE_REASON_UNKNOWN); + } // prevent running the expensive jump_to_landing_sequence // on every loop diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 24a3666d97..c0a6a4e5de 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1335,7 +1335,8 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) result = MAV_RESULT_FAILED; // attempt to switch to next DO_LAND_START command in the mission - if (plane.jump_to_landing_sequence()) { + if (plane.landing.jump_to_landing_sequence()) { + plane.set_mode(AUTO, MODE_REASON_UNKNOWN); result = MAV_RESULT_ACCEPTED; } break; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 0c69a02b6b..e1dcb42c37 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -918,7 +918,6 @@ private: void disarm_if_autoland_complete(); void setup_landing_glide_slope(void); void adjust_landing_slope_for_rangefinder_bump(void); - bool jump_to_landing_sequence(void); float tecs_hgt_afe(void); void set_nav_controller(void); void loiter_angle_reset(void); @@ -1084,7 +1083,6 @@ private: bool verify_command_callback(const AP_Mission::Mission_Command& cmd); void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); void run_cli(AP_HAL::UARTDriver *port); - bool restart_landing_sequence(); void log_init(); void init_capabilities(void); void dataflash_periodic(void); diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp index 13852e6db8..7a0613855e 100644 --- a/ArduPlane/landing.cpp +++ b/ArduPlane/landing.cpp @@ -27,7 +27,7 @@ bool Plane::verify_land() if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) { next_WP_loc = current_loc; mission.stop(); - bool success = restart_landing_sequence(); + bool success = landing.restart_landing_sequence(); mission.resume(); if (!success) { // on a restart failure lets RTL or else the plane may fly away with nowhere to go! @@ -298,85 +298,7 @@ void Plane::setup_landing_glide_slope(void) constrain_target_altitude_location(loc, prev_WP_loc); } -/* - Restart a landing by first checking for a DO_LAND_START and - jump there. Otherwise decrement waypoint so we would re-start - from the top with same glide slope. Return true if successful. - */ -bool Plane::restart_landing_sequence() -{ - if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) { - return false; - } - - uint16_t do_land_start_index = mission.get_landing_sequence_start(); - uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index(); - bool success = false; - uint16_t current_index = mission.get_current_nav_index(); - AP_Mission::Mission_Command cmd; - - if (mission.read_cmd_from_storage(current_index+1,cmd) && - cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT && - (cmd.p1 == 0 || cmd.p1 == 1) && - mission.set_current_cmd(current_index+1)) - { - // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100); - success = true; - } - else if (do_land_start_index != 0 && - mission.set_current_cmd(do_land_start_index)) - { - // look for a DO_LAND_START and use that index - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); - success = true; - } - else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE && - mission.set_current_cmd(prev_cmd_with_wp_index)) - { - // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then - // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope - gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); - success = true; - } else { - gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); - success = false; - } - if (success) { - // exit landing stages if we're no longer executing NAV_LAND - update_flight_stage(); - } - return success; -} - -/* - find the nearest landing sequence starting point (DO_LAND_START) and - switch to that mission item. Returns false if no DO_LAND_START - available. - */ -bool Plane::jump_to_landing_sequence(void) -{ - uint16_t land_idx = mission.get_landing_sequence_start(); - if (land_idx != 0) { - if (mission.set_current_cmd(land_idx)) { - - // in case we're in RTL - set_mode(AUTO, MODE_REASON_UNKNOWN); - - //if the mission has ended it has to be restarted - if (mission.state() == AP_Mission::MISSION_STOPPED) { - mission.resume(); - } - - gcs_send_text(MAV_SEVERITY_INFO, "Landing sequence start"); - return true; - } - } - - gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start landing sequence"); - return false; -} /* the height above field elevation that we pass to TECS diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 7fdbe417ac..8693b272c3 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -545,7 +545,10 @@ void Plane::exit_mode(enum FlightMode mode) if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) { - restart_landing_sequence(); + landing.restart_landing_sequence(); + + // exit landing stages if we're no longer executing NAV_LAND + update_flight_stage(); } } auto_state.started_flying_in_auto_ms = 0; diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 22ed1fe213..b82d4bddbc 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -18,6 +18,7 @@ */ #include "AP_Landing.h" +#include // table of user settable parameters const AP_Param::GroupInfo AP_Landing::var_info[] = { @@ -26,3 +27,78 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = { }; + +/* + Restart a landing by first checking for a DO_LAND_START and + jump there. Otherwise decrement waypoint so we would re-start + from the top with same glide slope. Return true if successful. + */ +bool AP_Landing::restart_landing_sequence() +{ + if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) { + return false; + } + + uint16_t do_land_start_index = mission.get_landing_sequence_start(); + uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index(); + bool success = false; + uint16_t current_index = mission.get_current_nav_index(); + AP_Mission::Mission_Command cmd; + + if (mission.read_cmd_from_storage(current_index+1,cmd) && + cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT && + (cmd.p1 == 0 || cmd.p1 == 1) && + mission.set_current_cmd(current_index+1)) + { + // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100); + success = true; + } + else if (do_land_start_index != 0 && + mission.set_current_cmd(do_land_start_index)) + { + // look for a DO_LAND_START and use that index + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index); + success = true; + } + else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE && + mission.set_current_cmd(prev_cmd_with_wp_index)) + { + // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then + // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index); + success = true; + } else { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Unable to restart landing sequence"); + success = false; + } + + return success; +} + + +/* + find the nearest landing sequence starting point (DO_LAND_START) and + switch to that mission item. Returns false if no DO_LAND_START + available. + */ +bool AP_Landing::jump_to_landing_sequence(void) +{ + uint16_t land_idx = mission.get_landing_sequence_start(); + if (land_idx != 0) { + 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_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing sequence start"); + return true; + } + } + + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Unable to start landing sequence"); + return false; +} + diff --git a/libraries/AP_Landing/AP_Landing.h b/libraries/AP_Landing/AP_Landing.h index 3e960b284a..6227666696 100644 --- a/libraries/AP_Landing/AP_Landing.h +++ b/libraries/AP_Landing/AP_Landing.h @@ -30,6 +30,9 @@ public: AP_Param::setup_object_defaults(this, var_info); } + bool restart_landing_sequence(); + bool jump_to_landing_sequence(void); + static const struct AP_Param::GroupInfo var_info[]; // Flag to indicate if we have landed.