/// advance_current_nav_cmd - moves current nav command forward
/// do command will also be loaded
/// accounts for do-jump commands
// returns true if command is advanced, false if failed (i.e. mission completed)
bool AP_Mission::advance_current_nav_cmd()
{
Mission_Command cmd;
uint16_t cmd_index;
// exit immediately if we're not running
if (_flags.state != MISSION_RUNNING) {
return false;
}
// exit immediately if current nav command has not completed
if (_flags.nav_cmd_loaded) {
return false;
}
// stop the current running do command
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
// get starting point for search
cmd_index = _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
// start from beginning of the mission command list
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
// start from one position past the current nav command
cmd_index++;
}
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(cmd_index, cmd, true)) {
return false;
}
// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// save previous nav command index
_prev_nav_cmd_index = _nav_cmd.index;
// set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
_cmd_start_fn(_nav_cmd);
}else{
// set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
_cmd_start_fn(_do_cmd);
}
}
// move onto next command
cmd_index = cmd.index+1;
}
// if we got this far we must have successfully advanced the nav command
return true;
}
/// resume - continues the mission execution from where we last left off
/// previous running commands will be re-initialised
void AP_Mission::resume()
{
// if mission had completed then start it from the first command
if (_flags.state == MISSION_COMPLETE) {
start();
return;
}
// if mission had stopped then restart it
if (_flags.state == MISSION_STOPPED) {
_flags.state = MISSION_RUNNING;
// if no valid nav command index restart from beginning
if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
start();
return;
}
}
// restart active navigation command. We run these on resume()
// regardless of whether the mission was stopped, as we may be
// re-entering AUTO mode and the nav_cmd callback needs to be run
// to setup the current target waypoint
// Note: if there is no active command then the mission must have been stopped just after the previous nav command completed
// update will take care of finding and starting the nav command
if (_flags.nav_cmd_loaded) {
_cmd_start_fn(_nav_cmd);
}
// restart active do command
if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
_cmd_start_fn(_do_cmd);
}
}
/// resume - continues the mission execution from where we last left off
/// previous running commands will be re-initialised
void AP_Mission::resume()
{
// if mission had completed then start it from the first command
if (_flags.state == MISSION_COMPLETE) {
start();
return;
}
// if mission had stopped then restart it
if (_flags.state == MISSION_STOPPED) {
_flags.state = MISSION_RUNNING;
// if no valid nav command index restart from beginning
if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
start();
return;
}
// restart active navigation command
// Note: if there is no active command then the mission must have been stopped just after the previous nav command completed
// update will take care of finding and starting the nav command
if (_flags.nav_cmd_loaded) {
_cmd_start_fn(_nav_cmd);
}
// restart active do command
if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
_cmd_start_fn(_do_cmd);
}
}
}
/// advance_current_do_cmd - moves current do command forward
/// accounts for do-jump commands
void AP_Mission::advance_current_do_cmd()
{
Mission_Command cmd;
uint16_t cmd_index;
// exit immediately if we're not running or we've completed all possible "do" commands
if (_flags.state != MISSION_RUNNING || _flags.do_cmd_all_done) {
return;
}
// get starting point for search
cmd_index = _do_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}else{
// start from one position past the current do command
cmd_index++;
}
// check if we've reached end of mission
if (cmd_index >= (unsigned)_cmd_total) {
// set flag to stop unnecessarily searching for do commands
_flags.do_cmd_all_done = true;
return;
}
// find next do command
if (get_next_do_cmd(cmd_index, cmd)) {
// set current do command and start it
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
_cmd_start_fn(_do_cmd);
}else{
// set flag to stop unnecessarily searching for do commands
_flags.do_cmd_all_done = true;
}
}
// set_current_cmd - jumps to command specified by index
bool AP_Mission::set_current_cmd(uint16_t index)
{
Mission_Command cmd;
// sanity check index and that we have a mission
if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
return false;
}
// stop the current running do command
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
// stop current nav cmd
_flags.nav_cmd_loaded = false;
// if index is zero then the user wants to completely restart the mission
if (index == 0 || _flags.state == MISSION_COMPLETE) {
_prev_nav_cmd_index = AP_MISSION_CMD_INDEX_NONE;
// reset the jump tracking to zero
init_jump_tracking();
if (index == 0) {
index = 1;
}
}
// if the mission is stopped or completed move the nav_cmd index to the specified point and set the state to stopped
// so that if the user resumes the mission it will begin at the specified index
if (_flags.state != MISSION_RUNNING) {
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(index, cmd, true)) {
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
return false;
}
// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// set current navigation command
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
}else{
// set current do command
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
}
}
// move onto next command
index = cmd.index+1;
}
// if we got this far then the mission can safely be "resumed" from the specified index so we set the state to "stopped"
_flags.state = MISSION_STOPPED;
return true;
}
// the state must be MISSION_RUNNING
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
if (!get_next_cmd(index, cmd, true)) {
// if we run out of nav commands mark mission as complete
complete();
// return true because we did what was requested
// which was apparently to jump to a command at the end of the mission
return true;
}
// check if navigation or "do" command
if (is_nav_cmd(cmd)) {
// save previous nav command index
_prev_nav_cmd_index = _nav_cmd.index;
// set current navigation command and start it
_nav_cmd = cmd;
_flags.nav_cmd_loaded = true;
_cmd_start_fn(_nav_cmd);
}else{
// set current do command and start it (if not already set)
if (!_flags.do_cmd_loaded) {
_do_cmd = cmd;
_flags.do_cmd_loaded = true;
_cmd_start_fn(_do_cmd);
}
}
// move onto next command
index = cmd.index+1;
}
// if we got this far we must have successfully advanced the nav command
return true;
}
