diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 3c48cebdd63b9..fe900d307db9a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -1062,3 +1062,12 @@ uint16_t Mode::get_pilot_speed_dn() { return copter.get_pilot_speed_dn(); } + +// Return stopping point as a location with above origin alt frame +Location Mode::get_stopping_point() const +{ + Vector3p stopping_point_NEU; + copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy()); + copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z); + return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN }; +} diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9d6526b46d545..2e33f121c3026 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -197,6 +197,9 @@ class Mode { void zero_throttle_and_hold_attitude(); void make_safe_ground_handling(bool force_throttle_unlimited = false); + // Return stopping point as a location with above origin alt frame + Location get_stopping_point() const; + // functions to control normal landing. pause_descent is true if vehicle should not descend void land_run_horizontal_control(); void land_run_vertical_control(bool pause_descent = false); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 12233d9f4877b..cd3060fa0f7fb 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -216,7 +216,7 @@ bool ModeAuto::allows_weathervaning() const // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason) { - if (mission.jump_to_landing_sequence()) { + if (mission.jump_to_landing_sequence(get_stopping_point())) { mission.set_force_resume(true); // if not already in auto switch to auto if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index b8a6cd9bbe7af..d3250e4265642 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -387,10 +387,7 @@ void ModeRTL::land_run(bool disarm_on_land) void ModeRTL::build_path() { // origin point is our stopping point - Vector3p stopping_point; - pos_control->get_stopping_point_xy_cm(stopping_point.xy()); - pos_control->get_stopping_point_z_cm(stopping_point.z); - rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN); + rtl_path.origin_point = get_stopping_point(); rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); // compute return target diff --git a/ArduPlane/AP_Arming.cpp b/ArduPlane/AP_Arming.cpp index d7ecaa03fad0a..061cb476a07bd 100644 --- a/ArduPlane/AP_Arming.cpp +++ b/ArduPlane/AP_Arming.cpp @@ -437,7 +437,7 @@ bool AP_Arming_Plane::mission_checks(bool report) { // base checks bool ret = AP_Arming::mission_checks(report); - if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { + if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) { ret = false; check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled"); } diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 97748b7f0d457..3a9dc4f70a735 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -701,7 +701,7 @@ bool Plane::trigger_land_abort(const float climb_to_alt_m) plane.is_land_command(mission_id); if (is_in_landing) { // fly a user planned abort pattern if available - if (plane.mission.jump_to_abort_landing_sequence()) { + if (plane.have_position && plane.mission.jump_to_abort_landing_sequence(plane.current_loc)) { return true; } diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 24e832ee8b0a3..751544d915bee 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1065,7 +1065,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_LAND_START: // attempt to switch to next DO_LAND_START command in the mission - if (plane.mission.jump_to_landing_sequence()) { + if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) { plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND); return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index a699625779e02..193f4cf1cb232 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -269,14 +269,14 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) already_landing = true; } #endif - if (!already_landing) { + if (!already_landing && plane.have_position) { // never stop a landing if we were already committed - if (plane.mission.is_best_land_sequence()) { + if (plane.mission.is_best_land_sequence(plane.current_loc)) { // continue mission as it will reach a landing in less distance plane.mission.set_in_landing_sequence_flag(true); break; } - if (plane.mission.jump_to_landing_sequence()) { + if (plane.mission.jump_to_landing_sequence(plane.current_loc)) { plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE); break; } @@ -293,7 +293,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action) #endif if (!already_landing) { // never stop a landing if we were already committed - if (g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && plane.mission.is_best_land_sequence()) { + if ((g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) && plane.have_position && plane.mission.is_best_land_sequence(plane.current_loc)) { // continue mission as it will reach a landing in less distance plane.mission.set_in_landing_sequence_flag(true); break; diff --git a/ArduPlane/mode_rtl.cpp b/ArduPlane/mode_rtl.cpp index ee851089e2087..179a8e9b477b4 100644 --- a/ArduPlane/mode_rtl.cpp +++ b/ArduPlane/mode_rtl.cpp @@ -102,7 +102,7 @@ void ModeRTL::navigate() labs(plane.altitude_error_cm) < 1000)) { // we've reached the RTL point, see if we have a landing sequence - if (plane.mission.jump_to_landing_sequence()) { + if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) { // switch from RTL -> AUTO plane.mission.set_force_resume(true); if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) { diff --git a/libraries/AP_Landing/AP_Landing.cpp b/libraries/AP_Landing/AP_Landing.cpp index 51ab12a63bf77..16cb32630e30b 100644 --- a/libraries/AP_Landing/AP_Landing.cpp +++ b/libraries/AP_Landing/AP_Landing.cpp @@ -474,7 +474,11 @@ bool AP_Landing::restart_landing_sequence() return false; } - uint16_t do_land_start_index = mission.get_landing_sequence_start(); + uint16_t do_land_start_index = 0; + Location loc; + if (ahrs.get_location(loc)) { + do_land_start_index = mission.get_landing_sequence_start(loc); + } 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(); diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 7f5d5726d4066..4f9f07ad1c239 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2317,14 +2317,8 @@ void AP_Mission::check_eeprom_version() // find the nearest landing sequence starting point (DO_LAND_START) and // return its index. Returns 0 if no appropriate DO_LAND_START point can // be found. -uint16_t AP_Mission::get_landing_sequence_start() +uint16_t AP_Mission::get_landing_sequence_start(const Location ¤t_loc) { - Location current_loc; - - if (!AP::ahrs().get_location(current_loc)) { - return 0; - } - const Location::AltFrame current_loc_alt_frame = current_loc.get_alt_frame(); uint16_t landing_start_index = 0; float min_distance = -1; @@ -2369,9 +2363,9 @@ uint16_t AP_Mission::get_landing_sequence_start() switch to that mission item. Returns false if no DO_LAND_START available. */ -bool AP_Mission::jump_to_landing_sequence(void) +bool AP_Mission::jump_to_landing_sequence(const Location ¤t_loc) { - uint16_t land_idx = get_landing_sequence_start(); + uint16_t land_idx = get_landing_sequence_start(current_loc); if (land_idx != 0 && set_current_cmd(land_idx)) { //if the mission has ended it has to be restarted @@ -2389,29 +2383,25 @@ bool AP_Mission::jump_to_landing_sequence(void) } // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort -bool AP_Mission::jump_to_abort_landing_sequence(void) +bool AP_Mission::jump_to_abort_landing_sequence(const Location ¤t_loc) { - Location current_loc; - uint16_t abort_index = 0; - if (AP::ahrs().get_location(current_loc)) { - float min_distance = FLT_MAX; + float min_distance = FLT_MAX; - const auto count = num_commands(); - for (uint16_t i = 1; i < count; i++) { - if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) { - continue; - } - Mission_Command tmp; - if (!read_cmd_from_storage(i, tmp)) { - continue; - } - if (tmp.id == MAV_CMD_DO_GO_AROUND) { - float tmp_distance = tmp.content.location.get_distance(current_loc); - if (tmp_distance < min_distance) { - min_distance = tmp_distance; - abort_index = i; - } + const auto count = num_commands(); + for (uint16_t i = 1; i < count; i++) { + if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) { + continue; + } + Mission_Command tmp; + if (!read_cmd_from_storage(i, tmp)) { + continue; + } + if (tmp.id == MAV_CMD_DO_GO_AROUND) { + float tmp_distance = tmp.content.location.get_distance(current_loc); + if (tmp_distance < min_distance) { + min_distance = tmp_distance; + abort_index = i; } } } @@ -2434,7 +2424,7 @@ bool AP_Mission::jump_to_abort_landing_sequence(void) } // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan -bool AP_Mission::is_best_land_sequence(void) +bool AP_Mission::is_best_land_sequence(const Location ¤t_loc) { // check if there is even a running mission to interupt if (_flags.state != MISSION_RUNNING) { @@ -2457,19 +2447,12 @@ bool AP_Mission::is_best_land_sequence(void) // go through the mission for the nearest DO_LAND_START first as this is the most probable route // to a landing with the minimum number of WP. - uint16_t do_land_start_index = get_landing_sequence_start(); + uint16_t do_land_start_index = get_landing_sequence_start(current_loc); if (do_land_start_index == 0) { // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained return false; } - // get our current location - Location current_loc; - if (!AP::ahrs().get_location(current_loc)) { - // we don't know where we are!! - return false; - } - // get distance to landing if travelled to nearest DO_LAND_START via RTL float dist_via_do_land; if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) { @@ -2927,6 +2910,30 @@ void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd, #endif } +// Helpers to fill in location for scripting +#if AP_SCRIPTING_ENABLED +bool AP_Mission::jump_to_landing_sequence(void) +{ + Location loc; + if (AP::ahrs().get_location(loc)) { + return jump_to_landing_sequence(loc); + } + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence"); + return false; +} + +bool AP_Mission::jump_to_abort_landing_sequence(void) +{ + Location loc; + if (AP::ahrs().get_location(loc)) { + return jump_to_abort_landing_sequence(loc); + } + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence"); + return false; +} +#endif // AP_SCRIPTING_ENABLED + + // singleton instance AP_Mission *AP_Mission::_singleton; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index f332594c09945..80d1991b5a936 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -665,18 +665,24 @@ class AP_Mission // find the nearest landing sequence starting point (DO_LAND_START) and // return its index. Returns 0 if no appropriate DO_LAND_START point can // be found. - uint16_t get_landing_sequence_start(); + uint16_t get_landing_sequence_start(const Location ¤t_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. - bool jump_to_landing_sequence(void); + bool jump_to_landing_sequence(const Location ¤t_loc); // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort + bool jump_to_abort_landing_sequence(const Location ¤t_loc); + + // Scripting helpers for the above functions to fill in the location +#if AP_SCRIPTING_ENABLED + bool jump_to_landing_sequence(void); bool jump_to_abort_landing_sequence(void); +#endif // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan - bool is_best_land_sequence(void); + bool is_best_land_sequence(const Location ¤t_loc); // set in_landing_sequence flag void set_in_landing_sequence_flag(bool flag)