AP_Mission: add restart_current_nav_cmd

libraries/AP_Mission/AP_Mission.cpp

@@ -517,6 +517,24 @@ bool AP_Mission::set_current_cmd(uint16_t index, bool rewind)
     return true;
 }
 
+// restart current navigation command.  Used to handle external changes to mission
+// returns true on success, false if mission is not running or current nav command is invalid
+bool AP_Mission::restart_current_nav_cmd()
+{
+    // return immediately if mission is not running
+    if (_flags.state != MISSION_RUNNING) {
+        return false;
+    }
+
+    // return immediately if nav command index is invalid
+    const uint16_t nav_cmd_index = get_current_nav_index();
+    if ((nav_cmd_index == 0) || (nav_cmd_index >= num_commands())) {
+        return false;
+    }
+
+    return set_current_cmd(_nav_cmd.index);
+}
+
 // returns false on any issue at all.
 bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet)
 {

libraries/AP_Mission/AP_Mission.h

@@ -494,6 +494,10 @@ public:
     // set_current_cmd - jumps to command specified by index
     bool set_current_cmd(uint16_t index, bool rewind = false);
 
+    // restart current navigation command.  Used to handle external changes to mission
+    // returns true on success, false if current nav command has been deleted
+    bool restart_current_nav_cmd();
+
     /// load_cmd_from_storage - load command from storage
     ///     true is return if successful
     bool read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const;