ACM Commands parser to look for future nav commands to inspect upcoming turn angle

ArduCopter/commands_process.pde

@@ -6,7 +6,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);
+	cmd_index = min(g.command_total - 1, cmd_index);
 
 	// load command
 	struct Location temp = get_cmd_with_index(cmd_index);
@@ -14,10 +14,10 @@ static void change_command(uint8_t 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 ){
+	if(temp.id > MAV_CMD_NAV_LAST){
 		//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
 
-	} else {
+	}else{
 		// clear out command queue
 		init_commands();
 
@@ -39,39 +39,55 @@ static void update_commands()
 	//uint8_t tmp = g.command_index.get();
 	//Serial.printf("command_index %u \n", tmp);
 
-	if (g.command_total <= 1 || g.command_index >= 127)
+	if(g.command_total <= 1 || g.command_index >= 255)
 		return;
 
 	if(command_nav_queue.id == NO_COMMAND){
 		// Our queue is empty
 		// fill command queue with a new command if available, or exit mission
 		// -------------------------------------------------------------------
-		if (command_nav_index < (g.command_total -1)) {
 
-			command_nav_index++;
-			command_nav_queue = get_cmd_with_index(command_nav_index);
+		// find next nav command
+		int16_t tmp_index;
 
-			if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){
+		if(command_nav_index < g.command_total){
+
+			// what is the next index for a nav command?
+			tmp_index = find_next_nav_index(command_nav_index + 1);
+
+			if(tmp_index == -1){
+				exit_mission();
+				return;
+			}else{
+				command_nav_index = tmp_index;
+				command_nav_queue = get_cmd_with_index(command_nav_index);
 				execute_nav_command();
-			} else{
-				// this is a conditional command so we skip it
-				command_nav_queue.id = NO_COMMAND;
+			}
+
+			// try to load the next nav for better speed control
+			// find_next_nav_index takes the next guess to start the search
+			tmp_index = find_next_nav_index(command_nav_index + 1);
+
+			// Fast corner management
+			// ----------------------
+			if(tmp_index == -1){
+				// there are no more commands left
+			}else{
+				// we have at least one more cmd left
+				Location tmp_loc = get_cmd_with_index(tmp_index);
+
+				if(tmp_loc.lat == 0){
+					fast_corner = false;
+				}else{
+					int32_t temp = get_bearing(&next_WP, &tmp_loc) - original_target_bearing;
+					temp = wrap_180(temp);
+					fast_corner = abs(temp) < 6000;
+				}
 			}
 		}else{
 			// we are out of commands
-			g.command_index  = command_nav_index = 255;
-			// if we are on the ground, enter stabilize, else Land
-			if (land_complete == true){
-				// we will disarm the motors after landing.
-			} else {
-				// If the approach altitude is valid (above 1m), do approach, else land
-				if(g.rtl_approach_alt == 0){
-					set_mode(LAND);
-				}else{
-					set_mode(LOITER);
-					set_new_altitude(g.rtl_approach_alt);
-				}
-			}
+			exit_mission();
+			return;
 		}
 	}
 
@@ -81,15 +97,14 @@ static void update_commands()
 		// -------------------------------------------------------------------
 
 		// no nav commands completed yet
-		if (prev_nav_index == NO_COMMAND)
+		if(prev_nav_index == NO_COMMAND)
 			return;
 
-		if (command_cond_index >= command_nav_index){
+		if(command_cond_index >= command_nav_index){
 			// don't process the fututre
-			//command_cond_index = NO_COMMAND;
 			return;
 
-		}else if (command_cond_index == NO_COMMAND){
+		}else if(command_cond_index == NO_COMMAND){
 			// start from scratch
 			// look at command after the most recent completed nav
 			command_cond_index = prev_nav_index + 1;
@@ -103,14 +118,14 @@ static void update_commands()
 			// we're OK to load a new command (last command must be a nav command)
 			command_cond_queue = get_cmd_with_index(command_cond_index);
 
-			if (command_cond_queue.id > MAV_CMD_CONDITION_LAST){
+			if(command_cond_queue.id > MAV_CMD_CONDITION_LAST){
 				// this is a do now command
 				process_now_command();
 
 				// clear command queue
 				command_cond_queue.id = NO_COMMAND;
 
-			}else if (command_cond_queue.id > MAV_CMD_NAV_LAST ){
+			}else if(command_cond_queue.id > MAV_CMD_NAV_LAST){
 				// this is a conditional command
 				process_cond_command();
 
@@ -129,10 +144,10 @@ static void update_commands()
 static void execute_nav_command(void)
 {
 	// This is what we report to MAVLINK
-	g.command_index  = command_nav_index;
+	g.command_index	= command_nav_index;
 
 	// Save CMD to Log
-	if (g.log_bitmask & MASK_LOG_CMD)
+	if(g.log_bitmask & MASK_LOG_CMD)
 		Log_Write_Cmd(g.command_index, &command_nav_queue);
 
 	// clear navigation prameters
@@ -151,10 +166,10 @@ static void verify_commands(void)
 {
 	if(verify_must()){
 		//Serial.printf("verified must cmd %d\n" , command_nav_index);
-		command_nav_queue.id = NO_COMMAND;
+		command_nav_queue.id 	= NO_COMMAND;
 
 		// store our most recent executed nav command
-		prev_nav_index = command_nav_index;
+		prev_nav_index 			= command_nav_index;
 
 		// Wipe existing conditionals
 		command_cond_index 		= NO_COMMAND;
@@ -169,3 +184,40 @@ static void verify_commands(void)
 		command_cond_queue.id = NO_COMMAND;
 	}
 }
+
+// Finds the next navgation command in EEPROM
+static int16_t find_next_nav_index(int16_t search_index)
+{
+	Location tmp;
+	while(search_index < g.command_total - 1){
+		tmp = get_cmd_with_index(search_index);
+		if(tmp.id <= MAV_CMD_NAV_LAST){
+			return search_index;
+		}else{
+			search_index++;
+		}
+	}
+	return -1;
+}
+
+
+static void exit_mission()
+{
+	// we are out of commands
+	g.command_index = 255;
+
+	// if we are on the ground, enter stabilize, else Land
+	if(land_complete == true){
+		// we will disarm the motors after landing.
+	}else{
+		// If the approach altitude is valid (above 1m), do approach, else land
+		if(g.rtl_approach_alt == 0){
+			set_mode(LAND);
+		}else{
+			set_mode(LOITER);
+			set_new_altitude(g.rtl_approach_alt);
+		}
+	}
+
+}
+