Working implementation of Jump Command

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);
 				}
 			}
 

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;
 }

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);
 	}
 }
 

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 ){