AP_Mission: add ChangeDetector

Detects changes in the next few nav commands in the mission so SCurves and Splines can handle them elegantly

libraries/AP_Mission/AP_Mission_ChangeDetector.cpp

@@ -0,0 +1,61 @@
+/// @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;
+}
+

libraries/AP_Mission/AP_Mission_ChangeDetector.h

@@ -0,0 +1,41 @@
+/// @file    AP_Mission_ChangeDetector.h
+/// @brief   Detects changes in the next few nav commands in the mission
+
+/*
+ *   The AP_Mission_ChangeDetector library:
+ *   - records the index of the active nav command
+ *   - maintains a copy of the next few navigation commands
+ *   - checks for changes in either the active command or the next few nav commands
+ *
+ *   Detecting changes in the next few nav commands is critical for SCurves and splines
+ *   which plan the path through the corners
+ */
+#pragma once
+
+#include "AP_Mission.h"
+
+#if HAL_MISSION_ENABLED
+
+/// @class    AP_Mission_ChangeDetector
+/// @brief    Mission command change detector
+class AP_Mission_ChangeDetector
+{
+
+public:
+
+    // check for changes to mission. returns true if mission has been changed since last check
+    bool check_for_mission_change() WARN_IF_UNUSED;
+
+private:
+
+    // number of upcoming commands to monitor for changes
+    static const uint8_t mis_change_detect_cmd_max = 3;
+    struct {
+        uint32_t last_change_time_ms;       // local copy of last time mission was changed
+        uint16_t curr_cmd_index;            // local copy of AP_Mission's current command index
+        uint8_t cmd_count;                  // number of commands in the cmd array
+        AP_Mission::Mission_Command cmd[mis_change_detect_cmd_max]; // local copy of the next few mission commands
+    } mis_change_detect;
+};
+
+#endif // HAL_MISSION_ENABLED