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

Bug fixes for command logic re-write

This commit is contained in:
Doug Weibel 2011-10-27 13:43:19 -06:00
parent 1cd3c21774
commit c7d91a199c
4 changed files with 34 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/commands.pde
View File

@ -32,7 +32,10 @@ static void update_auto()
next_WP.lng = home.lng + 1000; // so we don't have bad calcs next_WP.lng = home.lng + 1000; // so we don't have bad calcs
} }
} else { } else {
g.command_index--; if(g.command_index != 0) {
g.command_index = nav_command_index;
nav_command_index--;
}
nav_command_ID = NO_COMMAND; nav_command_ID = NO_COMMAND;
non_nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
next_nav_command.id = CMD_BLANK; next_nav_command.id = CMD_BLANK;

13
ArduPlane/commands_logic.pde
View File

@ -10,6 +10,7 @@ handle_process_nav_cmd()
// ------------------------- // -------------------------
reset_I(); reset_I();
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id);
switch(next_nav_command.id){ switch(next_nav_command.id){
case MAV_CMD_NAV_TAKEOFF: case MAV_CMD_NAV_TAKEOFF:
@ -48,6 +49,7 @@ handle_process_nav_cmd()
static void static void
handle_process_condition_command() handle_process_condition_command()
{ {
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){ switch(next_nonnav_command.id){
case MAV_CMD_CONDITION_DELAY: case MAV_CMD_CONDITION_DELAY:
@ -83,6 +85,7 @@ handle_process_condition_command()
static void handle_process_do_command() static void handle_process_do_command()
{ {
gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id);
switch(next_nonnav_command.id){ switch(next_nonnav_command.id){
case MAV_CMD_DO_JUMP: case MAV_CMD_DO_JUMP:
@ -117,9 +120,14 @@ static void handle_process_do_command()
static void handle_no_commands() static void handle_no_commands()
{ {
gcs_send_text_fmt(PSTR("Returning to Home"));
next_nav_command = home; next_nav_command = home;
next_nav_command.alt = read_alt_to_hold(); next_nav_command.alt = read_alt_to_hold();
next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM; next_nav_command.id = MAV_CMD_NAV_LOITER_UNLIM;
nav_command_ID = MAV_CMD_NAV_LOITER_UNLIM;
non_nav_command_ID = WAIT_COMMAND;
handle_process_nav_cmd();
} }
/********************************************************************************/ /********************************************************************************/
@ -182,6 +190,11 @@ static bool verify_condition_command() // Returns true if command complete
case MAV_CMD_CONDITION_CHANGE_ALT: case MAV_CMD_CONDITION_CHANGE_ALT:
return verify_change_alt(); return verify_change_alt();
break; break;
case WAIT_COMMAND:
return 0;
break;
default: default:
gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd"));

21
ArduPlane/commands_process.pde
View File

@ -8,8 +8,11 @@ static void change_command(uint8_t cmd_index)
if (temp.id > MAV_CMD_NAV_LAST ){ if (temp.id > MAV_CMD_NAV_LAST ){
gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd"));
} else { } else {
gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index);
nav_command_ID = NO_COMMAND; nav_command_ID = NO_COMMAND;
next_nav_command.id = NO_COMMAND; next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1;
g.command_index.set_and_save(cmd_index - 1); g.command_index.set_and_save(cmd_index - 1);
process_next_command(); process_next_command();
} }
@ -47,16 +50,18 @@ static void process_next_command()
// and loads conditional or immediate commands if applicable // and loads conditional or immediate commands if applicable
struct Location temp; struct Location temp;
byte old_index;
// these are Navigation/Must commands // these are Navigation/Must commands
// --------------------------------- // ---------------------------------
if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded
old_index = nav_command_index;
temp.id = MAV_CMD_NAV_LAST; temp.id = MAV_CMD_NAV_LAST;
while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) {
nav_command_index++; nav_command_index++;
temp = get_cmd_with_index(nav_command_index); temp = get_cmd_with_index(nav_command_index);
} }
gcs_send_text_fmt(PSTR("Nav command index updated to #%i"),nav_command_index);
if(nav_command_index > g.command_total){ if(nav_command_index > g.command_total){
// we are out of commands! // we are out of commands!
gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!")); gcs_send_text_P(SEVERITY_LOW,PSTR("out of commands!"));
@ -77,21 +82,27 @@ static void process_next_command()
// these are Condition/May and Do/Now commands // these are Condition/May and Do/Now commands
// ------------------------------------------- // -------------------------------------------
if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command if (non_nav_command_index == NO_COMMAND) { // If the index is NO_COMMAND then we have just loaded a nav command
non_nav_command_index = nav_command_index + 1; non_nav_command_index = old_index + 1;
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
} else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command } else if (non_nav_command_ID == NO_COMMAND) { // If the ID is NO_COMMAND then we have just completed a non-nav command
non_nav_command_index++; non_nav_command_index++;
} }
if(nav_command_index < g.command_total && non_nav_command_ID == NO_COMMAND) { //gcs_send_text_fmt(PSTR("Nav command index #%i"),nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID);
if(nav_command_index <= (int)g.command_total && non_nav_command_ID == NO_COMMAND) {
temp = get_cmd_with_index(non_nav_command_index); temp = get_cmd_with_index(non_nav_command_index);
if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do if(temp.id <= MAV_CMD_NAV_LAST) { // The next command is a nav command. No non-nav commands to do
g.command_index.set_and_save(nav_command_index); g.command_index.set_and_save(nav_command_index);
non_nav_command_index = nav_command_index; non_nav_command_index = nav_command_index;
non_nav_command_ID = WAIT_COMMAND; non_nav_command_ID = WAIT_COMMAND;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
} else { // The next command is a non-nav command. Prepare to execute it. } else { // The next command is a non-nav command. Prepare to execute it.
g.command_index.set_and_save(non_nav_command_index); g.command_index.set_and_save(non_nav_command_index);
next_nonnav_command = temp; next_nonnav_command = temp;
non_nav_command_ID = next_nonnav_command.id; non_nav_command_ID = next_nonnav_command.id;
gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i"),non_nav_command_ID);
if (g.log_bitmask & MASK_LOG_CMD) { if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nonnav_command); Log_Write_Cmd(g.command_index, &next_nonnav_command);
} }
@ -107,7 +118,7 @@ static void process_next_command()
/**************************************************/ /**************************************************/
static void process_nav_cmd() static void process_nav_cmd()
{ {
gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded")); //gcs_send_text_P(SEVERITY_LOW,PSTR("New nav command loaded"));
// clear non-nav command ID and index // clear non-nav command ID and index
non_nav_command_index = NO_COMMAND; // Redundant - remove? non_nav_command_index = NO_COMMAND; // Redundant - remove?
@ -119,7 +130,7 @@ static void process_nav_cmd()
static void process_non_nav_command() static void process_non_nav_command()
{ {
gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded")); //gcs_send_text_P(SEVERITY_LOW,PSTR("new non-nav command loaded"));
if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) { if(non_nav_command_ID < MAV_CMD_CONDITION_LAST) {
handle_process_condition_command(); handle_process_condition_command();

2
ArduPlane/defines.h
View File

@ -77,7 +77,7 @@
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library // Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
#define CMD_BLANK 0 // there is no command stored in the mem location requested #define CMD_BLANK 0 // there is no command stored in the mem location requested
#define NO_COMMAND 0 #define NO_COMMAND 0
#define WAIT_COMMAND 999 #define WAIT_COMMAND 255
// Command/Waypoint/Location Options Bitmask // Command/Waypoint/Location Options Bitmask
//-------------------- //--------------------