Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: `DO_RETURN_PATH_START` limit worst case runtime

This commit is contained in:
Iampete1 2024-11-13 19:41:44 +00:00 committed by Andrew Tridgell
parent 840507970e
commit 5e7463fdcd
2 changed files with 12 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Mission/AP_Mission.cpp
View File

@ -2446,16 +2446,22 @@ bool AP_Mission::jump_to_closest_mission_leg(const Location &current_loc)
uint16_t landing_start_index = 0; uint16_t landing_start_index = 0;
float min_distance = -1; 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 // Go through mission and check each DO_RETURN_PATH_START
for (uint16_t i = 1; i < num_commands(); i++) { for (uint16_t i = 1; i < num_commands(); i++) {
Mission_Command tmp; if (get_command_id(i) == uint16_t(MAV_CMD_DO_RETURN_PATH_START)) {
if (read_cmd_from_storage(i, tmp) && (tmp.id == MAV_CMD_DO_RETURN_PATH_START)) {
uint16_t tmp_index; uint16_t tmp_index;
float tmp_distance; 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; min_distance = tmp_distance;
landing_start_index = tmp_index; 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. // 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 // 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; Location prev_loc;
Mission_Command temp_cmd; 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 // run through remainder of mission to approximate a distance to landing
uint16_t index = start_index; uint16_t index = start_index;
for (uint8_t i=0; i<UINT8_MAX; i++) { for (; search_remaining > 0; search_remaining--) {
// search until the end of the mission command list // search until the end of the mission command list
for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) { for (uint16_t cmd_index = index; cmd_index <= (unsigned)_cmd_total; cmd_index++) {
if (get_next_cmd(cmd_index, temp_cmd, true, false)) { if (get_next_cmd(cmd_index, temp_cmd, true, false)) {

2
libraries/AP_Mission/AP_Mission.h
View File

@ -869,7 +869,7 @@ private:
// Approximate the distance traveled to return to the mission path. DO_JUMP commands are observed in look forward. // 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 // 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 // calculate the location of a resume cmd wp
bool calc_rewind_pos(Mission_Command& rewind_cmd); bool calc_rewind_pos(Mission_Command& rewind_cmd);