Plane: fixed setting of waypoint when not in AUTO

this fixes a bug where the next waypoint is set incorrectly if set via
MAVLink when not in AUTO
This commit is contained in:
Andrew Tridgell 2013-10-22 09:06:52 +11:00
parent 20ae1b7bc3
commit 6f3458eb25

View File

@ -23,7 +23,18 @@ void change_command(uint8_t cmd_index)
next_nav_command.id = NO_COMMAND; next_nav_command.id = NO_COMMAND;
non_nav_command_ID = NO_COMMAND; non_nav_command_ID = NO_COMMAND;
nav_command_index = cmd_index - 1; /*
if we are in AUTO then we need to set the nav_command_index
to one less than the requested index as
process_next_command() will increment the index before using
it. If not in AUTO then we just set the index as give.
Thanks to Michael Day for finding this!
*/
if (control_mode == AUTO) {
nav_command_index = cmd_index - 1;
} else {
nav_command_index = cmd_index;
}
g.command_index.set_and_save(cmd_index); g.command_index.set_and_save(cmd_index);
update_commands(); update_commands();
} }