From 3cbbd4ebb97ffd2ea2632e573dda200868280f0c Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 1 Mar 2014 10:19:38 +0900
Subject: [PATCH] Mission: add set current command method

---
 libraries/AP_Mission/AP_Mission.cpp | 59 +++++++++++++++++++++++++++++
 libraries/AP_Mission/AP_Mission.h   |  5 +--
 2 files changed, 61 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
index adbd21abc3..34ea0b5868 100644
--- a/libraries/AP_Mission/AP_Mission.cpp
+++ b/libraries/AP_Mission/AP_Mission.cpp
@@ -200,6 +200,65 @@ bool AP_Mission::get_next_nav_cmd(uint16_t start_index, Mission_Command& cmd)
     return false;
 }
 
+// set_current_cmd - jumps to command specified by index
+bool AP_Mission::set_current_cmd(uint8_t index)
+{
+    Mission_Command cmd;
+
+    // exit immediately if we're not running
+    // To-Do: allow setting command while mission is stopped and use the index provided when mission is started
+    if (_flags.state != MISSION_RUNNING) {
+        return false;
+    }
+
+    // sanity check index
+    if (index >= _cmd_total || index == 0) {
+        return false;
+    }
+
+    // stop the current running do command
+    _do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
+    _flags.do_cmd_loaded = false;
+    _flags.do_cmd_all_done = false;
+
+    // stop current nav cmd
+    _flags.nav_cmd_loaded = false;
+
+    // search until we find next nav command or reach end of command list
+    while (!_flags.nav_cmd_loaded) {
+        // get next command
+        if (!get_next_cmd(index, cmd, true)) {
+            // if we run out of nav commands mark mission as complete
+            complete();
+            // return true because we did what was requested
+            // which was apparently to jump to a command at the end of the mission
+            return true;
+        }
+
+        // check if navigation or "do" command
+        if (is_nav_cmd(cmd)) {
+            // save previous nav command index
+            _prev_nav_cmd_index = _nav_cmd.index;
+            // set current navigation command and start it
+            _nav_cmd = cmd;
+            _flags.nav_cmd_loaded = true;
+            _cmd_start_fn(_nav_cmd);
+        }else{
+            // set current do command and start it (if not already set)
+            if (!_flags.do_cmd_loaded) {
+                _do_cmd = cmd;
+                _flags.do_cmd_loaded = true;
+                _cmd_start_fn(_do_cmd);
+            }
+        }
+        // move onto next command
+        index = cmd.index+1;
+    }
+
+    // if we got this far we must have successfully advanced the nav command
+    return true;
+}
+
 /// load_cmd_from_storage - load command from storage
 ///     true is return if successful
 bool AP_Mission::read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const
diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h
index f052878e8f..d862abe19b 100644
--- a/libraries/AP_Mission/AP_Mission.h
+++ b/libraries/AP_Mission/AP_Mission.h
@@ -156,9 +156,8 @@ public:
     /// get_active_do_cmd - returns active "do" command
     const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
 
-    // jump_to_cmd - jumps to command specified by index
-    //      To-Do: implement this function!
-    bool jump_to_cmd(uint8_t index) { return false; }
+    // set_current_cmd - jumps to command specified by index
+    bool set_current_cmd(uint8_t index);
 
     /// load_cmd_from_storage - load command from storage
     ///     true is return if successful