From 85e9fa8c3b1923f40bd23c5b6b99d18d6d7bc0bd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 28 Aug 2012 12:01:12 +1000 Subject: [PATCH] APM: fixed another problem with DO_JUMP off by one error! --- ArduPlane/commands_logic.pde | 11 ++++++----- ArduPlane/commands_process.pde | 28 ++++++++++++++-------------- 2 files changed, 20 insertions(+), 19 deletions(-) diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2237a8458f..f21a2ef225 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -519,10 +519,11 @@ static void do_jump() next_nav_command.id = NO_COMMAND; non_nav_command_ID = NO_COMMAND; - gcs_send_text_fmt(PSTR("setting command index: %i"), next_nonnav_command.p1 - 1); - g.command_index.set_and_save(next_nonnav_command.p1 - 1); - nav_command_index = next_nonnav_command.p1 - 1; - next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump + gcs_send_text_fmt(PSTR("setting command index: %i"), next_nonnav_command.p1); + g.command_index.set_and_save(next_nonnav_command.p1); + nav_command_index = next_nonnav_command.p1; + // Need to back "next_WP" up as it was set to the next waypoint following the jump + next_WP = prev_WP; temp = get_cmd_with_index(g.command_index); @@ -534,7 +535,7 @@ static void do_jump() if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nav_command); } - process_nav_cmd(); + handle_process_nav_cmd(); } static void do_change_speed() diff --git a/ArduPlane/commands_process.pde b/ArduPlane/commands_process.pde index aafd9cb878..71a3b83cc2 100644 --- a/ArduPlane/commands_process.pde +++ b/ArduPlane/commands_process.pde @@ -76,7 +76,7 @@ static void process_next_command() if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nav_command); } - process_nav_cmd(); + handle_process_nav_cmd(); } } @@ -92,19 +92,24 @@ static void process_next_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) { + 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 + 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); + gcs_send_text_fmt(PSTR("Non-Nav command ID updated to #%i idx=%u"), + (unsigned)non_nav_command_ID, + (unsigned)non_nav_command_index); - } 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); 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); + gcs_send_text_fmt(PSTR("(2) Non-Nav command ID updated to #%i idx=%u"), + (unsigned)non_nav_command_ID, (unsigned)non_nav_command_index); if (g.log_bitmask & MASK_LOG_CMD) { Log_Write_Cmd(g.command_index, &next_nonnav_command); @@ -112,7 +117,6 @@ static void process_next_command() process_non_nav_command(); } - } } @@ -122,13 +126,7 @@ static void process_next_command() static void process_nav_cmd() { //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? - non_nav_command_ID = NO_COMMAND; // Redundant - remove? - handle_process_nav_cmd(); - } static void process_non_nav_command() @@ -141,6 +139,8 @@ static void process_non_nav_command() handle_process_do_command(); // flag command ID so a new one is loaded // ----------------------------------------- - non_nav_command_ID = NO_COMMAND; + if (non_nav_command_ID != WAIT_COMMAND) { + non_nav_command_ID = NO_COMMAND; + } } }