Line data    Source code

       1             : /// @file    AP_Mission_ChangeDetector.cpp
       2             : /// @brief   Detects changes in the next few nav commands in the mission
       3             : 
       4             : #include "AP_Mission_ChangeDetector.h"
       5             : 
       6             : #if AP_MISSION_ENABLED
       7             : 
       8             : // detect external changes to mission
       9     3574164 : bool AP_Mission_ChangeDetector::check_for_mission_change()
      10             : {
      11     3574164 :     AP_Mission &mission = AP::mission();
      12             : 
      13             :     // check if mission has been updated
      14     3574164 :     const uint32_t change_time_ms = mission.last_change_time_ms();
      15     3574164 :     const bool update_time_changed = (change_time_ms != mis_change_detect.last_change_time_ms);
      16             : 
      17             :     // check if active command index has changed
      18     3574164 :     const uint16_t curr_cmd_idx = mission.get_current_nav_index();
      19     3574164 :     const bool curr_cmd_idx_changed = (curr_cmd_idx != mis_change_detect.curr_cmd_index);
      20             : 
      21             :     // no changes if neither mission update time nor active command index has changed
      22     3574164 :     if (!update_time_changed && !curr_cmd_idx_changed) {
      23     3573482 :         return false;
      24             :     }
      25             : 
      26             :     // the mission has been updated (but maybe not changed) and/or the current command index has changed
      27             :     // check the contents of the next three commands to ensure they have not changed
      28             :     // and update storage so we can detect future changes
      29             : 
      30         682 :     bool cmds_changed = false;  // true if upcoming command contents have changed
      31             : 
      32             :     // retrieve cmds from mission and compare with mis_change_detect
      33         682 :     uint8_t num_cmds = 0;
      34         682 :     uint16_t cmd_idx = curr_cmd_idx;
      35        2728 :     AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max];
      36        2303 :     while ((num_cmds < ARRAY_SIZE(cmd)) && mission.get_next_nav_cmd(cmd_idx, cmd[num_cmds])) {
      37        1621 :         num_cmds++;
      38        1621 :         if ((num_cmds > mis_change_detect.cmd_count) || (cmd[num_cmds-1] != mis_change_detect.cmd[num_cmds-1])) {
      39        1621 :             cmds_changed = true;
      40        1621 :             mis_change_detect.cmd[num_cmds-1] = cmd[num_cmds-1];
      41             :         }
      42        1621 :         cmd_idx = cmd[num_cmds-1].index+1;
      43             :     }
      44             : 
      45             :     // mission has changed if number of upcoming commands does not match mis_change_detect
      46         682 :     if (num_cmds != mis_change_detect.cmd_count) {
      47         386 :         cmds_changed = true;
      48             :     }
      49             : 
      50             :     // update mis_change_detect with last change time, command index and number of commands
      51         682 :     mis_change_detect.last_change_time_ms = change_time_ms;
      52         682 :     mis_change_detect.curr_cmd_index = curr_cmd_idx;
      53         682 :     mis_change_detect.cmd_count = num_cmds;
      54             : 
      55             :     // mission has changed if upcoming command contents have changed without the current command index changing
      56         682 :     return cmds_changed && !curr_cmd_idx_changed;
      57             : }
      58             : 
      59             : #endif  // AP_MISSION_ENABLED