Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify ArduCopter/commands_process.pde

This commit is contained in:
uncrustify 2012-08-16 17:50:03 -07:00 committed by Pat Hickey
parent b6dd8aa592
commit 1e2c01d8f6
1 changed files with 155 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

310
ArduCopter/commands_process.pde
View File

@ -4,220 +4,220 @@
//---------------------------------------- //----------------------------------------
static void change_command(uint8_t cmd_index) static void change_command(uint8_t cmd_index)
{ {
//Serial.printf("change_command: %d\n",cmd_index ); //Serial.printf("change_command: %d\n",cmd_index );
// limit range // limit range
cmd_index = min(g.command_total - 1, cmd_index); cmd_index = min(g.command_total - 1, cmd_index);
// load command // load command
struct Location temp = get_cmd_with_index(cmd_index); 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 // 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")); //gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
}else{ }else{
// clear out command queue // clear out command queue
init_commands(); init_commands();
// copy command to the queue // copy command to the queue
command_nav_queue = temp; command_nav_queue = temp;
command_nav_index = cmd_index; command_nav_index = cmd_index;
execute_nav_command(); execute_nav_command();
} }
} }
// called by 10 Hz loop // called by 10 Hz loop
// -------------------- // --------------------
static void update_commands() static void update_commands()
{ {
//Serial.printf("update_commands: %d\n",increment ); //Serial.printf("update_commands: %d\n",increment );
// A: if we do not have any commands there is nothing to do // A: if we do not have any commands there is nothing to do
// B: We have completed the mission, don't redo the mission // B: We have completed the mission, don't redo the mission
// XXX debug // XXX debug
//uint8_t tmp = g.command_index.get(); //uint8_t tmp = g.command_index.get();
//Serial.printf("command_index %u \n", tmp); //Serial.printf("command_index %u \n", tmp);
if(g.command_total <= 1 || g.command_index >= 255) if(g.command_total <= 1 || g.command_index >= 255)
return; return;
if(command_nav_queue.id == NO_COMMAND){ if(command_nav_queue.id == NO_COMMAND) {
// Our queue is empty // Our queue is empty
// fill command queue with a new command if available, or exit mission // fill command queue with a new command if available, or exit mission
// ------------------------------------------------------------------- // -------------------------------------------------------------------
// find next nav command // find next nav command
int16_t tmp_index; 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? // what is the next index for a nav command?
tmp_index = find_next_nav_index(command_nav_index + 1); tmp_index = find_next_nav_index(command_nav_index + 1);
if(tmp_index == -1){ if(tmp_index == -1) {
exit_mission(); exit_mission();
return; return;
}else{ }else{
command_nav_index = tmp_index; command_nav_index = tmp_index;
command_nav_queue = get_cmd_with_index(command_nav_index); command_nav_queue = get_cmd_with_index(command_nav_index);
execute_nav_command(); execute_nav_command();
} }
// try to load the next nav for better speed control // try to load the next nav for better speed control
// find_next_nav_index takes the next guess to start the search // find_next_nav_index takes the next guess to start the search
tmp_index = find_next_nav_index(command_nav_index + 1); tmp_index = find_next_nav_index(command_nav_index + 1);
// Fast corner management // Fast corner management
// ---------------------- // ----------------------
if(tmp_index == -1){ if(tmp_index == -1) {
// there are no more commands left // there are no more commands left
}else{ }else{
// we have at least one more cmd left // we have at least one more cmd left
Location tmp_loc = get_cmd_with_index(tmp_index); Location tmp_loc = get_cmd_with_index(tmp_index);
if(tmp_loc.lat == 0){ if(tmp_loc.lat == 0) {
fast_corner = false; fast_corner = false;
}else{ }else{
int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing; int32_t temp = get_bearing_cd(&next_WP, &tmp_loc) - original_target_bearing;
temp = wrap_180(temp); temp = wrap_180(temp);
fast_corner = labs(temp) < 6000; fast_corner = labs(temp) < 6000;
} }
} }
}else{ }else{
// we are out of commands // we are out of commands
exit_mission(); exit_mission();
return; return;
} }
} }
if(command_cond_queue.id == NO_COMMAND){ if(command_cond_queue.id == NO_COMMAND) {
// Our queue is empty // Our queue is empty
// fill command queue with a new command if available, or do nothing // fill command queue with a new command if available, or do nothing
// ------------------------------------------------------------------- // -------------------------------------------------------------------
// no nav commands completed yet // no nav commands completed yet
if(prev_nav_index == NO_COMMAND) if(prev_nav_index == NO_COMMAND)
return; return;
if(command_cond_index >= command_nav_index){ if(command_cond_index >= command_nav_index) {
// don't process the fututre // don't process the fututre
return; return;
}else if(command_cond_index == NO_COMMAND){ }else if(command_cond_index == NO_COMMAND) {
// start from scratch // start from scratch
// look at command after the most recent completed nav // look at command after the most recent completed nav
command_cond_index = prev_nav_index + 1; command_cond_index = prev_nav_index + 1;
}else{ }else{
// we've completed 1 cond, look at next command for another // we've completed 1 cond, look at next command for another
command_cond_index++; command_cond_index++;
} }
if(command_cond_index < (g.command_total -2)){ if(command_cond_index < (g.command_total -2)) {
// we're OK to load a new command (last command must be a nav command) // 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); 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 // this is a do now command
process_now_command(); process_now_command();
// clear command queue // clear command queue
command_cond_queue.id = NO_COMMAND; 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 // this is a conditional command
process_cond_command(); process_cond_command();
}else{ }else{
// this is a nav command, don't process // this is a nav command, don't process
// clear the command conditional queue and index // clear the command conditional queue and index
prev_nav_index = NO_COMMAND; prev_nav_index = NO_COMMAND;
command_cond_index = NO_COMMAND; command_cond_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND; command_cond_queue.id = NO_COMMAND;
} }
} }
} }
} }
static void execute_nav_command(void) static void execute_nav_command(void)
{ {
// This is what we report to MAVLINK // This is what we report to MAVLINK
g.command_index = command_nav_index; g.command_index = command_nav_index;
// Save CMD to Log // 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); Log_Write_Cmd(g.command_index, &command_nav_queue);
// clear navigation prameters // clear navigation prameters
reset_nav_params(); reset_nav_params();
// Act on the new command // Act on the new command
process_nav_command(); process_nav_command();
// clear May indexes to force loading of more commands // clear May indexes to force loading of more commands
// existing May commands are tossed. // existing May commands are tossed.
command_cond_index = NO_COMMAND; command_cond_index = NO_COMMAND;
} }
// called with GPS navigation update - not constantly // called with GPS navigation update - not constantly
static void verify_commands(void) static void verify_commands(void)
{ {
if(verify_must()){ if(verify_must()) {
//Serial.printf("verified must cmd %d\n" , command_nav_index); //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 // store our most recent executed nav command
prev_nav_index = command_nav_index; prev_nav_index = command_nav_index;
// Wipe existing conditionals // Wipe existing conditionals
command_cond_index = NO_COMMAND; command_cond_index = NO_COMMAND;
command_cond_queue.id = NO_COMMAND; command_cond_queue.id = NO_COMMAND;
}else{ }else{
//Serial.printf("verified must false %d\n" , command_nav_index); //Serial.printf("verified must false %d\n" , command_nav_index);
} }
if(verify_may()){ if(verify_may()) {
//Serial.printf("verified may cmd %d\n" , command_cond_index); //Serial.printf("verified may cmd %d\n" , command_cond_index);
command_cond_queue.id = NO_COMMAND; command_cond_queue.id = NO_COMMAND;
} }
} }
// Finds the next navgation command in EEPROM // Finds the next navgation command in EEPROM
static int16_t find_next_nav_index(int16_t search_index) static int16_t find_next_nav_index(int16_t search_index)
{ {
Location tmp; Location tmp;
while(search_index < g.command_total - 1){ while(search_index < g.command_total - 1) {
tmp = get_cmd_with_index(search_index); tmp = get_cmd_with_index(search_index);
if(tmp.id <= MAV_CMD_NAV_LAST){ if(tmp.id <= MAV_CMD_NAV_LAST) {
return search_index; return search_index;
}else{ }else{
search_index++; search_index++;
} }
} }
return -1; return -1;
} }
static void exit_mission() static void exit_mission()
{ {
// we are out of commands // we are out of commands
g.command_index = 255; g.command_index = 255;
// if we are on the ground, enter stabilize, else Land // if we are on the ground, enter stabilize, else Land
if(land_complete == true){ if(land_complete == true) {
// we will disarm the motors after landing. // we will disarm the motors after landing.
}else{ }else{
// If the approach altitude is valid (above 1m), do approach, else land // If the approach altitude is valid (above 1m), do approach, else land
if(g.rtl_approach_alt == 0){ if(g.rtl_approach_alt == 0) {
set_mode(LAND); set_mode(LAND);
}else{ }else{
set_mode(LOITER); set_mode(LOITER);
set_new_altitude(g.rtl_approach_alt); set_new_altitude(g.rtl_approach_alt);
} }
} }
} }