zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Mission Scripting re-write

master
Jason Short 13 years ago
parent
163345dadd
commit
0245e73a8b
1 changed files with 91 additions and 139 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 230
      ArduCopter/commands_process.pde

230
ArduCopter/commands_process.pde
Unescape View File

@ -4,15 +4,27 @@ @@ -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) @@ -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 <pnc> 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 <pnc> 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: <process_must>"));
//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("<process_may>"));
//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;
}
}

Write Preview
Loading…
Cancel
Save