/// update - ensures the command queues are loaded with the next command and calls main programs command_init and command_verify functions to progress the mission
/// should be called at 10hz or higher
void AP_Mission::update()
{
// exit immediately if not running or no mission commands
if (_flags.state != MISSION_RUNNING || _cmd_total == 0) {
return;
}
// check if we have an active nav command
if (!_flags.nav_cmd_loaded || _nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
// advance in mission if no active nav command
if (!advance_current_nav_cmd()) {
// failure to advance nav command means mission has completed
complete();
return;
}
}else{
// run the active nav command
if (_cmd_verify_fn(_nav_cmd)) {
// market _nav_cmd as complete (it will be started on the next iteration)
_flags.nav_cmd_loaded = false;
}
}
// check if we have an active do command
if (!_flags.do_cmd_loaded || _do_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
advance_current_do_cmd();
}else{
// run the active do command
if (_cmd_verify_fn(_do_cmd)) {
// market _nav_cmd as complete (it will be started on the next iteration)
_flags.do_cmd_loaded = false;
}
}
}
/// start - resets current commands to point to the beginning of the mission
/// To-Do: should we validate the mission first and return true/false?
void AP_Mission::start()
{
_flags.state = MISSION_RUNNING;
reset(); // reset mission to the first command, resets jump tracking
// advance to the first command
if (!advance_current_nav_cmd()) {
// on failure set mission complete
complete();
}
}
/// start - resets current commands to point to the beginning of the mission
/// To-Do: should we validate the mission first and return true/false?
void AP_Mission::start()
{
_flags.state = MISSION_RUNNING;
_flags.nav_cmd_loaded = false;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
init_jump_tracking();
// advance to the first command
if (!advance_current_nav_cmd()) {
// on failure set mission complete
complete();
}
}
