AP_Mission: Skip DO_JUMP Mission Items that point to itself

Fixes the case where MAV_CMD_DO_JUMP pointing to itself will skip the rest of the mission.
This fixes the failure that results from the autotest added in previous commit.
This commit is contained in:
James O'Shannessy 2024-11-13 13:59:25 +11:00
parent 0219c487e4
commit 87c6372ac8
1 changed files with 8 additions and 0 deletions

View File

@ -2175,6 +2175,14 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
return false;
}
// handle jump item that points to itself
if (temp_cmd.content.jump.target == cmd_index) {
// This jump points to itself - move on to next mission item
// To-Do: log an error?
cmd_index++;
continue;
}
// check for endless loops
if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
// we have somehow reached this jump command twice and there is no chance it will complete