From c7d91a199c49bfb3fa75d65c25faaeaefe86385c Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Thu, 27 Oct 2011 13:43:19 -0600 Subject: [PATCH] Bug fixes for command logic re-write --- ArduPlane/commands.pde | 5 ++++- ArduPlane/commands_logic.pde | 13 +++++++++++++ ArduPlane/commands_process.pde | 21 ++++++++++++++++----- ArduPlane/defines.h | 2 +- 4 files changed, 34 insertions(+), 7 deletions(-) diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 7e032991c1..a9f7934e74 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -32,7 +32,10 @@ static void update_auto() next_WP.lng = home.lng + 1000; // so we don't have bad calcs } } else { - g.command_index--; + if(g.command_index != 0) { + g.command_index = nav_command_index; + nav_command_index--; + } nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND; next_nav_command.id = CMD_BLANK; diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 996fa11f44..11145912d8 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -10,6 +10,7 @@ handle_process_nav_cmd() // ------------------------- reset_I(); + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nav_command.id); switch(next_nav_command.id){ case MAV_CMD_NAV_TAKEOFF: @@ -48,6 +49,7 @@ handle_process_nav_cmd() static void handle_process_condition_command() { + gcs_send_text_fmt(PSTR("Executing command ID #%i"),next_nonnav_command.id); switch(next_nonnav_command.id){ case MAV_CMD_CONDITION_DELAY: @@ -83,6 +85,7 @@ handle_process_condition_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){ case MAV_CMD_DO_JUMP: @@ -117,9 +120,14 @@ static void handle_process_do_command() static void handle_no_commands() { + gcs_send_text_fmt(PSTR("Returning to Home")); next_nav_command = home; next_nav_command.alt = read_alt_to_hold(); 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: return verify_change_alt(); break; + + case WAIT_COMMAND: + return 0; + break; + default: gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Invalid or no current Condition cmd")); diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index b1b010650f..5ef18b36b1 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -8,8 +8,11 @@ static void change_command(uint8_t cmd_index) if (temp.id > MAV_CMD_NAV_LAST ){ gcs_send_text_P(SEVERITY_LOW,PSTR("Bad Request - cannot change to non-Nav cmd")); } else { + gcs_send_text_fmt(PSTR("Received Request - jump to command #%i"),cmd_index); 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); process_next_command(); } @@ -47,16 +50,18 @@ static void process_next_command() // and loads conditional or immediate commands if applicable struct Location temp; + byte old_index; // these are Navigation/Must commands // --------------------------------- if (nav_command_ID == NO_COMMAND){ // no current navigation command loaded - + old_index = nav_command_index; temp.id = MAV_CMD_NAV_LAST; while(temp.id >= MAV_CMD_NAV_LAST && nav_command_index <= g.command_total) { 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){ // we are 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 // ------------------------------------------- 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 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); 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); non_nav_command_index = nav_command_index; 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. g.command_index.set_and_save(non_nav_command_index); next_nonnav_command = temp; 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) { Log_Write_Cmd(g.command_index, &next_nonnav_command); } @@ -107,7 +118,7 @@ static void process_next_command() /**************************************************/ 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 non_nav_command_index = NO_COMMAND; // Redundant - remove? @@ -119,7 +130,7 @@ static void process_nav_cmd() 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) { handle_process_condition_command(); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 5117f44156..19a3dc5ba5 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -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 #define CMD_BLANK 0 // there is no command stored in the mem location requested #define NO_COMMAND 0 -#define WAIT_COMMAND 999 +#define WAIT_COMMAND 255 // Command/Waypoint/Location Options Bitmask //--------------------