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,169 +38,109 @@ 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){ - - // 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 + 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)) { - 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); - // 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_nav_queue.id <= MAV_CMD_NAV_LAST ){ + // This is what we report to MAVLINK + g.command_index = command_nav_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); + // Save CMD to Log + if (g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.command_index + 1, &command_nav_queue); - // We acted on the queue - let's debug that - // ---------------------------------------- - print_wp(&next_command, g.command_index); + // Act on the new command + process_nav_command(); - // invalidate command queue so a new one is loaded - // ----------------------------------------------- - clear_command_queue(); + // clear May indexes to force loading of more commands + // existing May commands are tossed. + command_cond_index = NO_COMMAND; - // make sure we load the next command index - // ---------------------------------------- - increment_WP_index(); + } else{ + // this is a conditional command so we skip it + command_nav_queue.id = NO_COMMAND; + } + } } -} -// called with GPS navigation update - not constantly -static void verify_commands(void) -{ - if(verify_must()){ - //Serial.printf("verified must cmd %d\n" , command_must_index); - command_must_index = NO_COMMAND; - }else{ - //Serial.printf("verified must false %d\n" , command_must_index); - } + if(command_cond_queue.id == NO_COMMAND){ + // Our queue is empty + // fill command queue with a new command if available, or do nothing + // ------------------------------------------------------------------- - if(verify_may()){ - //Serial.printf("verified may cmd %d\n" , command_may_index); - command_may_index = NO_COMMAND; - command_may_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 ){ + // no nav commands completed yet + if (prev_nav_index == NO_COMMAND) + return; - // we remember the index of our mission here - command_must_index = g.command_index + 1; + if (command_cond_index >= command_nav_index){ + // don't process the fututre + //command_cond_index = NO_COMMAND; + return; - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &next_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; - // Act on the new command - process_must(); - return true; + }else{ + // we've completed 1 cond, look at next command for another + command_cond_index++; } - } - - // 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; + 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); - //SendDebug("MSG new may "); - //SendDebugln(next_command.id,DEC); - //Serial.print("new command_may_index "); - //Serial.println(command_may_index,DEC); + if (command_cond_queue.id > MAV_CMD_CONDITION_LAST){ + // this is a do now command + process_now_command(); - // Save CMD to Log - if (g.log_bitmask & MASK_LOG_CMD) - Log_Write_Cmd(g.command_index + 1, &next_command); + // clear command queue + command_cond_queue.id = NO_COMMAND; - process_may(); - return true; - } + }else if (command_cond_queue.id > MAV_CMD_NAV_LAST ){ + // this is a conditional command + process_cond_command(); - // these are Do/Now commands - // --------------------------- - if (next_command.id > MAV_CMD_CONDITION_LAST){ - //SendDebug("MSG new now "); - //SendDebugln(next_command.id,DEC); + }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; + } - 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() +// called with GPS navigation update - not constantly +static void verify_commands(void) { - //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; + if(verify_must()){ + //Serial.printf("verified must cmd %d\n" , command_nav_index); + command_nav_queue.id = NO_COMMAND; - // implements the Flight Logic - handle_process_must(); + // 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; -static void process_may() -{ - //gcs_send_text_P(SEVERITY_LOW,PSTR("")); - //Serial.print("pmay"); - - command_may_ID = next_command.id; - handle_process_may(); -} + }else{ + //Serial.printf("verified must false %d\n" , command_nav_index); + } -static void process_now() -{ - //Serial.print("pnow"); - handle_process_now(); + if(verify_may()){ + //Serial.printf("verified may cmd %d\n" , command_cond_index); + command_cond_queue.id = NO_COMMAND; + } }