Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM: fixed another problem with DO_JUMP 

off by one error!
This commit is contained in:
Andrew Tridgell 2012-08-28 12:01:12 +10:00
parent ded64fd9ea
commit 85e9fa8c3b
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;
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()

28
ArduPlane/commands_process.pde
View File

@ -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;
}
}
}