diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 5eb0b3c7b9..7974c780f3 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -4,15 +4,27 @@ //---------------------------------------- static void change_command(uint8_t cmd_index) { + // limit range + cmd_index = min(g.command_total-1, cmd_index); + + // load command struct Location temp = get_cmd_with_index(cmd_index); + // verify it's a nav command if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); + } else { //Serial.printf("APM:New cmd Index: %d\n", cmd_index); - command_must_index = NO_COMMAND; - next_command.id = NO_COMMAND; - g.command_index.set_and_save(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; + prev_nav_index = command_nav_index; update_commands(); } } @@ -26,59 +38,86 @@ static void update_commands(void) if (g.command_total <= 1 || g.command_index == 255) return; - // fill command queue with a new command if available - if(next_command.id == NO_COMMAND){ + 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)) { - // fetch next command if the next command queue is empty - // ----------------------------------------------------- - - /* - range check - don't grab a command that isn't there - 0 : home command - 1 : first WP - 2 : seconds WP - 3 : third WP - - current g.command_index = 2, - Now load index 3 - if(2 < (4-1)) - if (2 < 3) - OK we can safely load the next index 3 - */ - - if (g.command_index < (g.command_total -1)) { // load next index - next_command = get_cmd_with_index(g.command_index + 1); + command_nav_index++; + command_nav_queue = get_cmd_with_index(command_nav_index); + + if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){ + // This is what we report to MAVLINK + g.command_index = command_nav_index; + + // Save CMD to Log + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.command_index + 1, &command_nav_queue); + + // Act on the new command + process_nav_command(); + + // clear May indexes to force loading of more commands + // existing May commands are tossed. + command_cond_index = NO_COMMAND; + + } else{ + // this is a conditional command so we skip it + command_nav_queue.id = NO_COMMAND; + } } } - // Are we out of must commands and the queue is empty? - if(next_command.id == NO_COMMAND && command_must_index == NO_COMMAND){ - // if no commands were available from EEPROM - // And we have no nav commands - // -------------------------------------------- - if (command_must_ID == NO_COMMAND){ - gcs_send_text_P(SEVERITY_LOW,PSTR("mission complete")); - handle_no_commands(); - g.command_index = 255; + if(command_cond_queue.id == NO_COMMAND){ + // Our queue is empty + // fill command queue with a new command if available, or do nothing + // ------------------------------------------------------------------- + + // no nav commands completed yet + if (prev_nav_index == NO_COMMAND) + return; + + 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){ + // start from scratch + // look at command after the most recent completed nav + command_cond_index = prev_nav_index + 1; + + }else{ + // we've completed 1 cond, look at next command for another + command_cond_index++; } - } - // check to see if we need to act on our command queue - if (process_next_command()){ - //Serial.printf("did PNC: %d\n", next_command.id); + if(command_cond_index < (g.command_total -2)){ + // 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); - // We acted on the queue - let's debug that - // ---------------------------------------- - print_wp(&next_command, g.command_index); + if (command_cond_queue.id > MAV_CMD_CONDITION_LAST){ + // this is a do now command + process_now_command(); - // invalidate command queue so a new one is loaded - // ----------------------------------------------- - clear_command_queue(); + // clear command queue + command_cond_queue.id = NO_COMMAND; - // make sure we load the next command index - // ---------------------------------------- - increment_WP_index(); + }else if (command_cond_queue.id > MAV_CMD_NAV_LAST ){ + // this is a conditional command + process_cond_command(); + + }else{ + // this is a nav command, don't process + // clear the command conditional queue and index + prev_nav_index = NO_COMMAND; + command_cond_index = NO_COMMAND; + command_cond_queue.id = NO_COMMAND; + } + + } } } @@ -86,109 +125,22 @@ static void update_commands(void) static void verify_commands(void) { if(verify_must()){ - //Serial.printf("verified must cmd %d\n" , command_must_index); - command_must_index = NO_COMMAND; + //Serial.printf("verified must cmd %d\n" , command_nav_index); + command_nav_queue.id = NO_COMMAND; + + // store our most recent executed nav command + prev_nav_index = command_nav_index; + + // Wipe existing conditionals + command_cond_index = NO_COMMAND; + command_cond_queue.id = NO_COMMAND; + }else{ - //Serial.printf("verified must false %d\n" , command_must_index); + //Serial.printf("verified must false %d\n" , command_nav_index); } if(verify_may()){ - //Serial.printf("verified may cmd %d\n" , command_may_index); - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; + //Serial.printf("verified may cmd %d\n" , command_cond_index); + command_cond_queue.id = NO_COMMAND; } } - -static bool -process_next_command() -{ - // these are Navigation/Must commands - // --------------------------------- - if (command_must_index == NO_COMMAND){ // no current command loaded - if (next_command.id < MAV_CMD_NAV_LAST ){ - - // we remember the index of our mission here - command_must_index = g.command_index + 1; - - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &next_command); - - // Act on the new command - process_must(); - return true; - } - } - - // these are Condition/May commands - // ---------------------- - if (command_may_index == NO_COMMAND){ - if (next_command.id > MAV_CMD_NAV_LAST && next_command.id < MAV_CMD_CONDITION_LAST ){ - - // we remember the index of our mission here - command_may_index = g.command_index + 1; - - //SendDebug("MSG new may "); - //SendDebugln(next_command.id,DEC); - //Serial.print("new command_may_index "); - //Serial.println(command_may_index,DEC); - - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &next_command); - - process_may(); - return true; - } - - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - //SendDebug("MSG new now "); - //SendDebugln(next_command.id,DEC); - - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &next_command); - process_now(); - return true; - } - } - // we did not need any new commands - return false; -} - -/**************************************************/ -// These functions implement the commands. -/**************************************************/ -static void process_must() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("New cmd: ")); - //Serial.printf("pmst %d\n", (int)next_command.id); - - // clear May indexes to force loading of more commands - // existing May commands are tossed. - command_may_index = NO_COMMAND; - command_may_ID = NO_COMMAND; - - // remember our command ID - command_must_ID = next_command.id; - - // implements the Flight Logic - handle_process_must(); - -} - -static void process_may() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("")); - //Serial.print("pmay"); - - command_may_ID = next_command.id; - handle_process_may(); -} - -static void process_now() -{ - //Serial.print("pnow"); - handle_process_now(); -}