diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 7038e46649..bf530bfa6f 100644 --- a/ArduCopter/commands_process.pde +++ b/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); + } + } + +} +