From ddd89e1849a4fb723fcf0079e066585a6e888f80 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@jasonshort.local>
Date: Wed, 16 Nov 2011 00:21:12 -0800
Subject: [PATCH] Mission Scripting updates

---
 ArduCopter/commands_logic.pde  |  8 ++++----
 ArduPlane/commands_process.pde | 24 +++++++++++++-----------
 2 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde
index 1f68159c72..f664bcbea1 100644
--- a/ArduCopter/commands_logic.pde
+++ b/ArduCopter/commands_logic.pde
@@ -453,7 +453,7 @@ static bool verify_loiter_time()
 
 static bool verify_loiter_turns()
 {
-	Serial.printf("loiter_sum: %d \n", loiter_sum);
+	//Serial.printf("loiter_sum: %d \n", loiter_sum);
 	// have we rotated around the center enough times?
 	// -----------------------------------------------
 	if(abs(loiter_sum) > loiter_total) {
@@ -582,7 +582,7 @@ static bool verify_wait_delay()
 
 static bool verify_change_alt()
 {
-	Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
+	//Serial.printf("change_alt, ca:%d, na:%d\n", (int)current_loc.alt, (int)next_WP.alt);
 	if (condition_start < next_WP.alt){
 		// we are going higer
 		if(current_loc.alt > next_WP.alt){
@@ -601,7 +601,7 @@ static bool verify_change_alt()
 
 static bool verify_within_distance()
 {
-	Serial.printf("cond dist :%d\n", (int)condition_value);
+	//Serial.printf("cond dist :%d\n", (int)condition_value);
 	if (wp_distance < condition_value){
 		condition_value = 0;
 		return true;
@@ -611,7 +611,7 @@ static bool verify_within_distance()
 
 static bool verify_yaw()
 {
-	Serial.printf("vyaw %d\n", (int)(nav_yaw/100));
+	//Serial.printf("vyaw %d\n", (int)(nav_yaw/100));
 
 	if((millis() - command_yaw_start_time) > command_yaw_time){
 		// time out
diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde
index e5a0bcb8ae..974474d503 100644
--- a/ArduPlane/commands_process.pde
+++ b/ArduPlane/commands_process.pde
@@ -16,8 +16,8 @@ static void change_command(uint8_t cmd_index)
 		non_nav_command_ID 	= NO_COMMAND;
 
 		nav_command_index 	= cmd_index - 1;
-		g.command_index.set_and_save(cmd_index - 1);
-		process_next_command();
+		g.command_index.set_and_save(cmd_index);
+		update_commands();
 	}
 }
 
@@ -25,23 +25,21 @@ static void change_command(uint8_t cmd_index)
 // --------------------
 static void update_commands(void)
 {
-	if(home_is_set == false){
-		return;	// don't do commands
-	}
-
 	if(control_mode == AUTO){
-		process_next_command();
+		if(home_is_set == true && g.command_total > 1){
+			process_next_command();
+		}
 	}									// Other (eg GCS_Auto) modes may be implemented here
 }
 
 static void verify_commands(void)
 {
 	if(verify_nav_command()){
-		nav_command_ID 	= NO_COMMAND;
+		nav_command_ID = NO_COMMAND;
 	}
 
 	if(verify_condition_command()){
-		non_nav_command_ID		= NO_COMMAND;
+		non_nav_command_ID = NO_COMMAND;
 	}
 }
 
@@ -63,7 +61,9 @@ static void process_next_command()
 			nav_command_index++;
 			temp = get_cmd_with_index(nav_command_index);
 		}
+
 		gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
+
 		if(nav_command_index > g.command_total){
 			// we are out of commands!
 			gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
@@ -99,12 +99,14 @@ static void process_next_command()
 			g.command_index.set_and_save(nav_command_index);
 			non_nav_command_index = nav_command_index;
 			non_nav_command_ID = WAIT_COMMAND;
-		gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+			gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+
 		} else {								// The next command is a non-nav command.  Prepare to execute it.
 			g.command_index.set_and_save(non_nav_command_index);
 			next_nonnav_command = temp;
 			non_nav_command_ID = next_nonnav_command.id;
-		gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+			gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
+
 			if (g.log_bitmask & MASK_LOG_CMD) {
 				Log_Write_Cmd(g.command_index, &next_nonnav_command);
 			}