Mission Scripting updates

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

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,12 +25,10 @@ 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){
+		if(home_is_set == true && g.command_total > 1){
 			process_next_command();
+		}
 	}									// Other (eg GCS_Auto) modes may be implemented here
 }
 
@@ -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!"));
@@ -100,11 +100,13 @@ static void process_next_command()
 			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);
+
 		} 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);
+
 			if (g.log_bitmask & MASK_LOG_CMD) {
 				Log_Write_Cmd(g.command_index, &next_nonnav_command);
 			}