From 764d86216eb415289486f948641e6c63be230cbf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 22 Sep 2012 16:18:32 +1000
Subject: [PATCH] APM: fixed mission reset by setting waypoint to zero

this does a full mission reset (equivalent to rebooting)
---
 ArduPlane/commands.pde         |  1 +
 ArduPlane/commands_process.pde | 10 +++++++++-
 ArduPlane/control_modes.pde    |  2 +-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde
index 6b45a71824..825dbd4b74 100644
--- a/ArduPlane/commands.pde
+++ b/ArduPlane/commands.pde
@@ -9,6 +9,7 @@ static void init_commands()
     nav_command_ID  = NO_COMMAND;
     non_nav_command_ID      = NO_COMMAND;
     next_nav_command.id     = CMD_BLANK;
+    nav_command_index = 0;
 }
 
 static void update_auto()
diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde
index 05aecf0da0..67ca9a915a 100644
--- a/ArduPlane/commands_process.pde
+++ b/ArduPlane/commands_process.pde
@@ -4,7 +4,15 @@
 //----------------------------------------
 void change_command(uint8_t cmd_index)
 {
-    struct Location temp = get_cmd_with_index(cmd_index);
+    struct Location temp;
+
+    if (cmd_index == 0) {
+        init_commands();
+        gcs_send_text_fmt(PSTR("Received Request - reset mission"));
+        return;
+    }
+
+    temp = get_cmd_with_index(cmd_index);
 
     if (temp.id > MAV_CMD_NAV_LAST ) {
         gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde
index a8eabfa076..fa7d47117d 100644
--- a/ArduPlane/control_modes.pde
+++ b/ArduPlane/control_modes.pde
@@ -39,7 +39,7 @@ static void read_control_switch()
         APM_RC.InputCh(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) {
         // reset to first waypoint in mission
         prev_WP = current_loc;
-        change_command(1);
+        change_command(0);
     }
 
     switch_debouncer = false;