From 16eb5641c2881cc9653c3dae3eb9cd4d48c68fd2 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@jasonshort.local>
Date: Wed, 16 Nov 2011 21:28:56 -0800
Subject: [PATCH] Working implementation of Jump Command

---
 ArduCopter/ArduCopter.pde       |  2 +-
 ArduCopter/commands.pde         | 20 ++++----------------
 ArduCopter/commands_logic.pde   | 18 +++++++++++-------
 ArduCopter/commands_process.pde | 22 ++++++++++------------
 4 files changed, 26 insertions(+), 36 deletions(-)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index c55c2826e5..4d02571c90 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -691,7 +691,7 @@ static void medium_loop()
 			// --------------------
 			if(control_mode == AUTO){
 				if(home_is_set == true && g.command_total > 1){
-					update_commands();
+					update_commands(true);
 				}
 			}
 
diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde
index 379bd99d77..0c3298fbab 100644
--- a/ArduCopter/commands.pde
+++ b/ArduCopter/commands.pde
@@ -2,22 +2,10 @@
 
 static void init_commands()
 {
-	// Zero is home, the curren command
-    g.command_index = 0;
-
-    // This are registers for the current may and must commands
-    // setting to zero will allow them to be written to by new commands
-	command_nav_index	= NO_COMMAND;
-	command_cond_index	= NO_COMMAND;
-	prev_nav_index 		= NO_COMMAND;
-
-	// clear the command queue
-	clear_command_queue();
-}
-
-// forces the loading of a new command
-// queue is emptied after a new command is processed
-static void clear_command_queue(){
+    g.command_index 		= NO_COMMAND;
+	command_nav_index		= NO_COMMAND;
+	command_cond_index		= NO_COMMAND;
+	prev_nav_index 			= NO_COMMAND;
 	command_cond_queue.id 	= NO_COMMAND;
 	command_nav_queue.id 	= NO_COMMAND;
 }
diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index f664bcbea1..e83f23720d 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -660,25 +660,29 @@ static void do_loiter_at_location()
 
 static void do_jump()
 {
+	//Serial.printf("do Jump: %d\n", jump);
+
 	if(jump == -10){
+		//Serial.printf("Fresh Jump\n");
+		// we use a locally stored index for jump
 		jump = command_cond_queue.lat;
 	}
+	//Serial.printf("Jumps left: %d\n",jump);
 
 	if(jump > 0) {
+		//Serial.printf("Do Jump to %d\n",command_cond_queue.p1);
 		jump--;
-		command_nav_index 	= 0;
-		command_cond_index 	= 0;
-
-		// set pointer to desired index
-		g.command_index 	= command_cond_queue.p1 - 1;
+		change_command(command_cond_queue.p1);
 
 	} else if (jump == 0){
+		//Serial.printf("Did last jump\n");
 		// we're done, move along
-		jump = -10;
+		jump = -11;
 
 	} else if (jump == -1) {
+		//Serial.printf("jumpForever\n");
 		// repeat forever
-	    g.command_index 	= command_cond_queue.p1 - 1;
+		change_command(command_cond_queue.p1);
 	}
 }
 
diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde
index 7974c780f3..3d3f3a5dec 100644
--- a/ArduCopter/commands_process.pde
+++ b/ArduCopter/commands_process.pde
@@ -10,28 +10,24 @@ static void change_command(uint8_t cmd_index)
 	// load command
 	struct Location temp = get_cmd_with_index(cmd_index);
 
+	//Serial.printf("loading cmd: %d with id:%d\n", cmd_index, temp.id);
+
 	// verify it's a nav command
 	if (temp.id > MAV_CMD_NAV_LAST ){
-		gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
+		//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
 
 	} else {
 		//Serial.printf("APM:New cmd Index: %d\n", cmd_index);
-		command_cond_index 		= NO_COMMAND;
-		command_cond_queue.id 	= NO_COMMAND;
-
-		command_nav_index 		= NO_COMMAND;
-		command_nav_queue.id 	= NO_COMMAND;
-
-		// we save one step back, because we add one in update
-		command_nav_index 		= cmd_index-1;
+		init_commands();
+		command_nav_index 		= cmd_index;
 		prev_nav_index 			= command_nav_index;
-		update_commands();
+		update_commands(false);
 	}
 }
 
 // called by 10 Hz loop
 // --------------------
-static void update_commands(void)
+static void update_commands(bool increment)
 {
 	// A: if we do not have any commands there is nothing to do
 	// B: We have completed the mission, don't redo the mission
@@ -45,7 +41,9 @@ static void update_commands(void)
 		if (command_nav_index < (g.command_total -1)) {
 
 			// load next index
-			command_nav_index++;
+			if (increment)
+				command_nav_index++;
+
 			command_nav_queue = get_cmd_with_index(command_nav_index);
 
 			if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){