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

Mission: allow set_current_cmd when mission is not running

Browse Source 
Can now be run when mission is complete or stopped.  When mission is
resumed it will begin at the set command.  This command can be either a
nav or do command
This commit is contained in:
Randy Mackay 2014-03-07 17:01:05 +09:00
parent f036aa1780
commit 512b378c4a
1 changed files with 44 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_Mission/AP_Mission.cpp
View File

@ -219,20 +219,8 @@ bool AP_Mission::set_current_cmd(uint16_t index)
{
Mission_Command cmd;
// exit immediately if we're not running
// To-Do: allow setting command while mission is stopped and use the index provided when mission is started
if (_flags.state != MISSION_RUNNING) {
return false;
}
if (index == 0) {
// changing mission item to zero is a mission reset
start();
return true;
}
// sanity check index
if (index >= _cmd_total || index == 0) {
// sanity check index and that we have a mission
if (index >= _cmd_total || _cmd_total == 1) {
return false;
}
@ -244,6 +232,47 @@ bool AP_Mission::set_current_cmd(uint16_t index)
// 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();
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
@ -767,6 +796,7 @@ bool AP_Mission::advance_current_nav_cmd()
// 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