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

ACM Commands parser to look for future nav commands to inspect upcoming turn angle

This commit is contained in:
Jason Short 2012-08-09 16:47:29 -07:00
parent d796814dfc
commit b5156185cc
1 changed files with 86 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
ArduCopter/commands_process.pde
View File

@ -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 ){
execute_nav_command();
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{
// this is a conditional command so we skip it
command_nav_queue.id = NO_COMMAND;
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);
// 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;
}
}
@ -86,7 +102,6 @@ static void update_commands()
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){
@ -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);
}
}
}