From 5e7463fdcdc6b1acfa25666fa111928d7201aab2 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Wed, 13 Nov 2024 19:41:44 +0000 Subject: [PATCH] AP_Mission: `DO_RETURN_PATH_START` limit worst case runtime --- libraries/AP_Mission/AP_Mission.cpp | 16 +++++++++++----- libraries/AP_Mission/AP_Mission.h | 2 +- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 22b4ce2950..d27beca79e 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -2446,16 +2446,22 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location ¤t_loc) uint16_t landing_start_index = 0; float min_distance = -1; + // This defines the maximum number of waypoints that will be searched, this limits the worst case runtime + uint16_t search_remaining = 1000; + // Go through mission and check each DO_RETURN_PATH_START for (uint16_t i = 1; i < num_commands(); i++) { - Mission_Command tmp; - if (read_cmd_from_storage(i, tmp) && (tmp.id == MAV_CMD_DO_RETURN_PATH_START)) { + if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) { uint16_t tmp_index; float tmp_distance; - if (distance_to_mission_leg(i, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){ + if (distance_to_mission_leg(i, search_remaining, tmp_distance, tmp_index, current_loc) && (min_distance < 0 || tmp_distance <= min_distance)){ min_distance = tmp_distance; landing_start_index = tmp_index; } + if (search_remaining == 0) { + // Run out of time to search, stop and return the best so far + break; + } } } @@ -2626,7 +2632,7 @@ reset_do_jump_tracking: // Approximate the distance travelled to return to the mission path. DO_JUMP commands are observed in look forward. // Stop searching once reaching a landing or do-land-start -bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc) +bool AP_Mission::distance_to_mission_leg(uint16_t start_index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc) { Location prev_loc; Mission_Command temp_cmd; @@ -2642,7 +2648,7 @@ bool AP_Mission::distance_to_mission_leg(uint16_t start_index, float &rejoin_dis // run through remainder of mission to approximate a distance to landing uint16_t index = start_index; - for (uint8_t i=0; i 0; search_remaining--) { // search until the end of the mission command list for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) { if (get_next_cmd(cmd_index, temp_cmd, true, false)) { diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 8926749d43..f3a1c8a043 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -869,7 +869,7 @@ private: // Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward. // Stop searching once reaching a landing or do-land-start - bool distance_to_mission_leg(uint16_t index, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc); + bool distance_to_mission_leg(uint16_t index, uint16_t &search_remaining, float &rejoin_distance, uint16_t &rejoin_index, const Location& current_loc); // calculate the location of a resume cmd wp bool calc_rewind_pos(Mission_Command& rewind_cmd);