You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
2.4 KiB
61 lines
2.4 KiB
/// @file AP_Mission_ChangeDetector.cpp |
|
/// @brief Detects changes in the next few nav commands in the mission |
|
|
|
#include "AP_Mission_ChangeDetector.h" |
|
|
|
extern const AP_HAL::HAL& hal; |
|
|
|
// detect external changes to mission |
|
bool AP_Mission_ChangeDetector::check_for_mission_change() |
|
{ |
|
AP_Mission *mission = AP::mission(); |
|
if (mission == nullptr) { |
|
return false; |
|
} |
|
|
|
// check if mission has been updated |
|
const uint32_t change_time_ms = mission->last_change_time_ms(); |
|
const bool update_time_changed = (change_time_ms != mis_change_detect.last_change_time_ms); |
|
|
|
// check if active command index has changed |
|
const uint16_t curr_cmd_idx = mission->get_current_nav_index(); |
|
const bool curr_cmd_idx_changed = (curr_cmd_idx != mis_change_detect.curr_cmd_index); |
|
|
|
// no changes if neither mission update time nor active command index has changed |
|
if (!update_time_changed && !curr_cmd_idx_changed) { |
|
return false; |
|
} |
|
|
|
// the mission has been updated (but maybe not changed) and/or the current command index has changed |
|
// check the contents of the next three commands to ensure they have not changed |
|
// and update storage so we can detect future changes |
|
|
|
bool cmds_changed = false; // true if upcoming command contents have changed |
|
|
|
// retrieve cmds from mission and compare with mis_change_detect |
|
uint8_t num_cmds = 0; |
|
uint16_t cmd_idx = curr_cmd_idx; |
|
AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; |
|
while ((num_cmds < ARRAY_SIZE(cmd)) && mission->get_next_nav_cmd(cmd_idx, cmd[num_cmds])) { |
|
num_cmds++; |
|
if ((num_cmds > mis_change_detect.cmd_count) || (cmd[num_cmds-1] != mis_change_detect.cmd[num_cmds-1])) { |
|
cmds_changed = true; |
|
mis_change_detect.cmd[num_cmds-1] = cmd[num_cmds-1]; |
|
} |
|
cmd_idx = cmd[num_cmds-1].index+1; |
|
} |
|
|
|
// mission has changed if number of upcoming commands does not match mis_change_detect |
|
if (num_cmds != mis_change_detect.cmd_count) { |
|
cmds_changed = true; |
|
} |
|
|
|
// update mis_change_detect with last change time, command index and number of commands |
|
mis_change_detect.last_change_time_ms = change_time_ms; |
|
mis_change_detect.curr_cmd_index = curr_cmd_idx; |
|
mis_change_detect.cmd_count = num_cmds; |
|
|
|
// mission has changed if upcoming command contents have changed without the current command index changing |
|
return cmds_changed && !curr_cmd_idx_changed; |
|
} |
|
|
|
|