From 60237bd01b999d7d6902b11f7fb9f4e5e1b27032 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Mon, 28 Nov 2011 10:31:12 -0800
Subject: [PATCH] slight refactoring to avoid the increment issue when changing
 commands

---
 ArduCopter/commands_process.pde | 52 +++++++++++++++++++--------------
 1 file changed, 30 insertions(+), 22 deletions(-)

diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde
index 3d3f3a5dec..d799c3063c 100644
--- a/ArduCopter/commands_process.pde
+++ b/ArduCopter/commands_process.pde
@@ -4,6 +4,7 @@
 //----------------------------------------
 static void change_command(uint8_t cmd_index)
 {
+	//Serial.printf("change_command: %d\n",cmd_index );
 	// limit range
 	cmd_index = min(g.command_total-1, cmd_index);
 
@@ -17,18 +18,21 @@ static void change_command(uint8_t cmd_index)
 		//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
 
 	} else {
-		//Serial.printf("APM:New cmd Index: %d\n", cmd_index);
+		// clear out command queue
 		init_commands();
+
+		// copy command to the queue
+		command_nav_queue		= temp;
 		command_nav_index 		= cmd_index;
-		prev_nav_index 			= command_nav_index;
-		update_commands(false);
+		execute_nav_command();
 	}
 }
 
 // called by 10 Hz loop
 // --------------------
-static void update_commands(bool increment)
+static void update_commands()
 {
+	//Serial.printf("update_commands: %d\n",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
 	if (g.command_total <= 1 || g.command_index == 255)
@@ -40,27 +44,11 @@ static void update_commands(bool increment)
 		// -------------------------------------------------------------------
 		if (command_nav_index < (g.command_total -1)) {
 
-			// load next index
-			if (increment)
-				command_nav_index++;
-
+			command_nav_index++;
 			command_nav_queue = get_cmd_with_index(command_nav_index);
 
 			if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){
-				// This is what we report to MAVLINK
-				g.command_index  = command_nav_index;
-
-				// Save CMD to Log
-				if (g.log_bitmask & MASK_LOG_CMD)
-					Log_Write_Cmd(g.command_index + 1, &command_nav_queue);
-
-				// Act on the new command
-				process_nav_command();
-
-				// clear May indexes to force loading of more commands
-				// existing May commands are tossed.
-				command_cond_index	= NO_COMMAND;
-
+				execute_nav_command();
 			} else{
 				// this is a conditional command so we skip it
 				command_nav_queue.id = NO_COMMAND;
@@ -119,6 +107,26 @@ static void update_commands(bool increment)
 	}
 }
 
+static void execute_nav_command(void)
+{
+	// This is what we report to MAVLINK
+	g.command_index  = command_nav_index;
+
+	// Save CMD to Log
+	if (g.log_bitmask & MASK_LOG_CMD)
+		Log_Write_Cmd(g.command_index, &command_nav_queue);
+
+	// Act on the new command
+	process_nav_command();
+
+	// clear navigation prameters
+	reset_nav();
+
+	// clear May indexes to force loading of more commands
+	// existing May commands are tossed.
+	command_cond_index	= NO_COMMAND;
+}
+
 // called with GPS navigation update - not constantly
 static void verify_commands(void)
 {