Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM: fixed another problem with DO_JUMP

Browse Source 
off by one error!
This commit is contained in:
Andrew Tridgell 2012-08-28 12:01:12 +10:00
parent 4945d66c80
commit 16dd911547
2 changed files with 20 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/commands_logic.pde
View File

@ -519,10 +519,11 @@ static void do_jump()
next_nav_command.id = NO_COMMAND; next_nav_command.id = NO_COMMAND;
non_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); gcs_send_text_fmt(PSTR("setting command index: %i"), next_nonnav_command.p1);
g.command_index.set_and_save(next_nonnav_command.p1 - 1); g.command_index.set_and_save(next_nonnav_command.p1);
nav_command_index = next_nonnav_command.p1 - 1; nav_command_index = next_nonnav_command.p1;
next_WP = prev_WP; // Need to back "next_WP" up as it was set to the next waypoint following the jump // 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); temp = get_cmd_with_index(g.command_index);
@ -534,7 +535,7 @@ static void do_jump()
if (g.log_bitmask & MASK_LOG_CMD) { if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nav_command); Log_Write_Cmd(g.command_index, &next_nav_command);
} }
process_nav_cmd(); handle_process_nav_cmd();
} }
static void do_change_speed() static void do_change_speed()

28
ArduPlane/commands_process.pde
View File

@ -76,7 +76,7 @@ static void process_next_command()
if (g.log_bitmask & MASK_LOG_CMD) { if (g.log_bitmask & MASK_LOG_CMD) {
Log_Write_Cmd(g.command_index, &next_nav_command); 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("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 index #%i"),non_nav_command_index);
//gcs_send_text_fmt(PSTR("Non-Nav command ID #%i"),non_nav_command_ID); //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); 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); 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); 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); 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) { 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);
@ -112,7 +117,6 @@ static void process_next_command()
process_non_nav_command(); process_non_nav_command();
} }
} }
} }
@ -122,13 +126,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
non_nav_command_index = NO_COMMAND; // Redundant - remove?
non_nav_command_ID = NO_COMMAND; // Redundant - remove?
handle_process_nav_cmd(); handle_process_nav_cmd();
} }
static void process_non_nav_command() static void process_non_nav_command()
@ -141,6 +139,8 @@ static void process_non_nav_command()
handle_process_do_command(); handle_process_do_command();
// flag command ID so a new one is loaded // 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;
}
} }
} }