diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 3220609de4..e0d6d49cfc 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -4,220 +4,220 @@ //---------------------------------------- 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); + //Serial.printf("change_command: %d\n",cmd_index ); + // limit range + cmd_index = min(g.command_total - 1, cmd_index); - // load command - struct Location temp = get_cmd_with_index(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); + //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")); + // 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{ - // clear out command queue - init_commands(); + }else{ + // clear out command queue + init_commands(); - // copy command to the queue - command_nav_queue = temp; - command_nav_index = cmd_index; - execute_nav_command(); - } + // copy command to the queue + command_nav_queue = temp; + command_nav_index = cmd_index; + execute_nav_command(); + } } // called by 10 Hz loop // -------------------- static void update_commands() { - //Serial.printf("update_commands: %d\n",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 - // XXX debug - //uint8_t tmp = g.command_index.get(); - //Serial.printf("command_index %u \n", tmp); + //Serial.printf("update_commands: %d\n",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 + // XXX debug + //uint8_t tmp = g.command_index.get(); + //Serial.printf("command_index %u \n", tmp); - if(g.command_total <= 1 || g.command_index >= 255) - return; + 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_queue.id == NO_COMMAND) { + // Our queue is empty + // fill command queue with a new command if available, or exit mission + // ------------------------------------------------------------------- - // find next nav command - int16_t tmp_index; + // find next nav command + int16_t tmp_index; - if(command_nav_index < g.command_total){ + 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); + // 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(); - } + 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(); + } - // 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); + // 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); + // 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_cd(&next_WP, &tmp_loc) - original_target_bearing; - temp = wrap_180(temp); - fast_corner = labs(temp) < 6000; - } - } - }else{ - // we are out of commands - exit_mission(); - return; - } - } + if(tmp_loc.lat == 0) { + fast_corner = false; + }else{ + int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing; + temp = wrap_180(temp); + fast_corner = labs(temp) < 6000; + } + } + }else{ + // we are out of commands + exit_mission(); + return; + } + } - if(command_cond_queue.id == NO_COMMAND){ - // Our queue is empty - // fill command queue with a new command if available, or do nothing - // ------------------------------------------------------------------- + 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; + // no nav commands completed yet + if(prev_nav_index == NO_COMMAND) + return; - if(command_cond_index >= command_nav_index){ - // don't process the fututre - return; + if(command_cond_index >= command_nav_index) { + // don't process the fututre + 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 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++; - } + }else{ + // we've completed 1 cond, look at next command for another + command_cond_index++; + } - 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); + 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); - if(command_cond_queue.id > MAV_CMD_CONDITION_LAST){ - // this is a do now command - process_now_command(); + 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; + // clear command queue + command_cond_queue.id = NO_COMMAND; - }else if(command_cond_queue.id > MAV_CMD_NAV_LAST){ - // this is a conditional command - process_cond_command(); + }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; - } + }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; + } - } - } + } + } } static void execute_nav_command(void) { - // This is what we report to MAVLINK - g.command_index = command_nav_index; + // 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, &command_nav_queue); + // Save CMD to Log + if(g.log_bitmask & MASK_LOG_CMD) + Log_Write_Cmd(g.command_index, &command_nav_queue); - // clear navigation prameters - reset_nav_params(); + // clear navigation prameters + reset_nav_params(); - // Act on the new command - process_nav_command(); + // 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; + // clear May indexes to force loading of more commands + // existing May commands are tossed. + command_cond_index = 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_nav_index); - command_nav_queue.id = NO_COMMAND; + if(verify_must()) { + //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; + // 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; + // Wipe existing conditionals + command_cond_index = NO_COMMAND; + command_cond_queue.id = NO_COMMAND; - }else{ - //Serial.printf("verified must false %d\n" , command_nav_index); - } + }else{ + //Serial.printf("verified must false %d\n" , command_nav_index); + } - if(verify_may()){ - //Serial.printf("verified may cmd %d\n" , command_cond_index); - command_cond_queue.id = NO_COMMAND; - } + if(verify_may()) { + //Serial.printf("verified may cmd %d\n" , command_cond_index); + 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; + 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; + // 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); - } - } + // 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); + } + } }