From 151cae6074be35a338389804ef228c182b7bf0b7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 18 Sep 2011 16:27:43 -0700 Subject: [PATCH] Jump command was reported to have issues, updated to match APM branch. --- ArduCopter/commands.pde | 3 +-- ArduCopter/commands_logic.pde | 12 +++++++----- ArduCopter/commands_process.pde | 5 +++-- 3 files changed, 11 insertions(+), 9 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index ff7e6d6bb7..ea8c5efff0 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -101,8 +101,7 @@ static void set_command_with_index(struct Location temp, int i) static void increment_WP_index() { if (g.waypoint_index < g.waypoint_total) { - g.waypoint_index.set_and_save(g.waypoint_index + 1); - //SendDebug("MSG WP index is incremented to "); + g.waypoint_index++; } SendDebugln(g.waypoint_index,DEC); diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 5b3d88fe2e..1a12fc4023 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -616,13 +616,15 @@ static void do_jump() struct Location temp; if(next_command.lat > 0) { - command_must_index = NO_COMMAND; - command_may_index = NO_COMMAND; - temp = get_command_with_index(g.waypoint_index); - temp.lat = next_command.lat - 1; // Decrement repeat counter + command_must_index = 0; + command_may_index = 0; + temp = get_command_with_index(g.waypoint_index); + temp.lat = next_command.lat - 1; // Decrement repeat counter set_command_with_index(temp, g.waypoint_index); - g.waypoint_index.set_and_save(next_command.p1 - 1); + g.waypoint_index = next_command.p1 - 1; + } else if (next_command.lat == -1) { + g.waypoint_index = next_command.p1 - 1; } } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index a6ac1f245c..76ef1c13e7 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -26,6 +26,7 @@ static void update_commands(void) // fetch next command if the next command queue is empty // ----------------------------------------------------- if (g.waypoint_index < g.waypoint_total) { + // only if we have a cmd stored in EEPROM next_command = get_command_with_index(g.waypoint_index + 1); //Serial.printf("queue CMD %d\n", next_command.id); @@ -162,7 +163,7 @@ static void process_may() { //gcs.send_text_P(SEVERITY_LOW,PSTR("")); //gcs.send_message(MSG_COMMAND_LIST, g.waypoint_index); - Serial.print("pmay"); + //Serial.print("pmay"); command_may_ID = next_command.id; handle_process_may(); @@ -170,6 +171,6 @@ static void process_may() static void process_now() { - Serial.print("pnow"); + //Serial.print("pnow"); handle_process_now(); }