Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

slight refactoring to avoid the increment issue when changing commands

Browse Source
This commit is contained in:
Jason Short 2011-11-28 10:31:12 -08:00
parent eea0062ea6
commit 60237bd01b
1 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
ArduCopter/commands_process.pde
View File

@ -4,6 +4,7 @@
//---------------------------------------- //----------------------------------------
static void change_command(uint8_t cmd_index) static void change_command(uint8_t 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);
@ -17,18 +18,21 @@ static void change_command(uint8_t cmd_index)
//gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd")); //gcs_send_text_P(SEVERITY_LOW,PSTR("error: non-Nav cmd"));
} else { } else {
//Serial.printf("APM:New cmd Index: %d\n", cmd_index); // clear out command queue
init_commands(); init_commands();
// copy command to the queue
command_nav_queue = temp;
command_nav_index = cmd_index; command_nav_index = cmd_index;
prev_nav_index = command_nav_index; execute_nav_command();
update_commands(false);
} }
} }
// called by 10 Hz loop // called by 10 Hz loop
// -------------------- // --------------------
static void update_commands(bool increment) static void update_commands()
{ {
//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
if (g.command_total <= 1 || g.command_index == 255) if (g.command_total <= 1 || g.command_index == 255)
@ -40,27 +44,11 @@ static void update_commands(bool increment)
// ------------------------------------------------------------------- // -------------------------------------------------------------------
if (command_nav_index < (g.command_total -1)) { if (command_nav_index < (g.command_total -1)) {
// load next index command_nav_index++;
if (increment)
command_nav_index++;
command_nav_queue = get_cmd_with_index(command_nav_index); command_nav_queue = get_cmd_with_index(command_nav_index);
if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){ if (command_nav_queue.id <= MAV_CMD_NAV_LAST ){
// This is what we report to MAVLINK execute_nav_command();
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{ } else{
// this is a conditional command so we skip it // this is a conditional command so we skip it
command_nav_queue.id = NO_COMMAND; command_nav_queue.id = NO_COMMAND;
@ -119,6 +107,26 @@ static void update_commands(bool increment)
} }
} }
static void execute_nav_command(void)
{
// 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);
// Act on the new command
process_nav_command();
// clear navigation prameters
reset_nav();
// 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 // called with GPS navigation update - not constantly
static void verify_commands(void) static void verify_commands(void)
{ {