diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2e5524e6b1..66597206c3 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -469,19 +469,30 @@ static void do_loiter_at_location() static void do_jump() { struct Location temp; + gcs_send_text_fmt(PSTR("In jump. Jumps left: %i"),next_nonnav_command.lat); if(next_nonnav_command.lat > 0) { - nav_command_ID = NO_COMMAND; + nav_command_ID = NO_COMMAND; + next_nav_command.id = NO_COMMAND; non_nav_command_ID = NO_COMMAND; + temp = get_cmd_with_index(g.command_index); temp.lat = next_nonnav_command.lat - 1; // Decrement repeat counter set_cmd_with_index(temp, g.command_index); + 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 + process_next_command(); } else if (next_nonnav_command.lat == -1) { // A repeat count of -1 = repeat forever 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 + process_next_command(); } }