mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 07:28:29 -04:00
AP_Mission: fixes to protect against endless loops
This commit is contained in:
parent
6086bc14be
commit
84b3497a82
@ -20,73 +20,31 @@ const AP_Param::GroupInfo AP_Mission::var_info[] PROGMEM = {
|
|||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
/// public methods
|
///
|
||||||
|
/// public mission methods
|
||||||
/// public methods
|
///
|
||||||
/// 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
|
|
||||||
// this may also set the active do command or end the mission
|
|
||||||
advance_current_nav_cmd();
|
|
||||||
}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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
// check if nav, do, cond commands are loaded
|
|
||||||
// if not load next nav-cmd
|
|
||||||
// this should also prompt loading of new do or conditional commands
|
|
||||||
// call command_init for each command loaded
|
|
||||||
|
|
||||||
// if we're running nav_command, verify it
|
|
||||||
// if command completed also push it to the prev_cmd
|
|
||||||
// if command completed, mark nav command as unloaded (so it'll be loaded next time through)
|
|
||||||
|
|
||||||
// if we're running do_command or conditional command, verify it
|
|
||||||
// if it completes, unload the do-command queue (so a new one will be loaded next time through)
|
|
||||||
}
|
|
||||||
|
|
||||||
/// start - resets current commands to point to the beginning of the mission
|
/// start - resets current commands to point to the beginning of the mission
|
||||||
/// To-Do: should we validate the mission first and return true/false?
|
/// To-Do: should we validate the mission first and return true/false?
|
||||||
void AP_Mission::start()
|
void AP_Mission::start()
|
||||||
{
|
{
|
||||||
hal.console->printf_P(PSTR("\nMission START\n"));
|
|
||||||
_flags.state = MISSION_RUNNING;
|
_flags.state = MISSION_RUNNING;
|
||||||
_flags.nav_cmd_loaded = false;
|
_flags.nav_cmd_loaded = false;
|
||||||
_flags.do_cmd_loaded = false;
|
_flags.do_cmd_loaded = false;
|
||||||
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
|
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
|
||||||
_do_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();
|
init_jump_tracking();
|
||||||
// advance to the first command
|
// advance to the first command
|
||||||
advance_current_nav_cmd();
|
if (!advance_current_nav_cmd()) {
|
||||||
|
// on failure set mission complete
|
||||||
|
complete();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// stop - stops mission execution. subsequent calls to update() will have no effect until the mission is started or resumed
|
/// stop - stops mission execution. subsequent calls to update() will have no effect until the mission is started or resumed
|
||||||
void AP_Mission::stop()
|
void AP_Mission::stop()
|
||||||
{
|
{
|
||||||
hal.console->printf_P(PSTR("\nMission STOP\n"));
|
|
||||||
_flags.state = MISSION_STOPPED;
|
_flags.state = MISSION_STOPPED;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -94,107 +52,34 @@ void AP_Mission::stop()
|
|||||||
/// previous running commands will be re-initialised
|
/// previous running commands will be re-initialised
|
||||||
void AP_Mission::resume()
|
void AP_Mission::resume()
|
||||||
{
|
{
|
||||||
hal.console->printf_P(PSTR("\nMission RESUME\n"));
|
// 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;
|
_flags.state = MISSION_RUNNING;
|
||||||
}
|
|
||||||
|
|
||||||
/// advance_current_nav_cmd - moves current nav command forward
|
// if no valid nav command index restart from beginning
|
||||||
/// do command will also be loaded
|
if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
|
||||||
/// accounts for do-jump commands
|
start();
|
||||||
// will call complete method if it reaches end of mission command list
|
|
||||||
void AP_Mission::advance_current_nav_cmd()
|
|
||||||
{
|
|
||||||
Mission_Command cmd;
|
|
||||||
uint8_t cmd_index;
|
|
||||||
|
|
||||||
// exit immediately if we're not running
|
|
||||||
if (_flags.state != MISSION_RUNNING) {
|
|
||||||
// debug
|
|
||||||
hal.console->printf_P(PSTR("\nACNC: not running\n"));
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// exit immediately if current nav command has not completed
|
// 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) {
|
if (_flags.nav_cmd_loaded) {
|
||||||
// debug
|
|
||||||
hal.console->printf_P(PSTR("\nACNC: curr nav not complete\n"));
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// stop the current running do command
|
|
||||||
_do_cmd.index = AP_MISSION_CMD_INDEX_NONE;
|
|
||||||
_flags.do_cmd_loaded = false;
|
|
||||||
|
|
||||||
// get starting point for search
|
|
||||||
cmd_index = _nav_cmd.index;
|
|
||||||
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
|
|
||||||
cmd_index = 0;
|
|
||||||
}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)) {
|
|
||||||
complete();
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check if navigation or "do" command
|
|
||||||
if (is_nav_cmd(cmd)) {
|
|
||||||
// set current navigation command and start it
|
|
||||||
_nav_cmd = cmd;
|
|
||||||
_flags.nav_cmd_loaded = true;
|
|
||||||
_cmd_start_fn(_nav_cmd);
|
_cmd_start_fn(_nav_cmd);
|
||||||
}else{
|
}
|
||||||
// set current do command and start it (if not already set)
|
|
||||||
if (!_flags.do_cmd_loaded) {
|
// restart active do command
|
||||||
_do_cmd = cmd;
|
if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) {
|
||||||
_flags.do_cmd_loaded = true;
|
|
||||||
_cmd_start_fn(_do_cmd);
|
_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;
|
|
||||||
uint8_t cmd_index;
|
|
||||||
|
|
||||||
// exit immediately if we're not running
|
|
||||||
if (_flags.state != MISSION_RUNNING) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// get starting point for search
|
|
||||||
cmd_index = _do_cmd.index;
|
|
||||||
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
|
|
||||||
cmd_index = 0;
|
|
||||||
}else{
|
|
||||||
// start from one position past the current do command
|
|
||||||
cmd_index++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// check if we've reached end of mission
|
|
||||||
if (cmd_index >= _cmd_total) {
|
|
||||||
// To-Do: set a flag to stop us from attempting to look for do-commands over and over?
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// find next do command
|
|
||||||
if (get_next_do_cmd(cmd_index, cmd)) {
|
|
||||||
// set current do command and start it
|
|
||||||
_do_cmd = cmd;
|
|
||||||
_cmd_start_fn(_do_cmd);
|
|
||||||
}else{
|
|
||||||
// To-Do: set a flag to stop us from attempting to look for do-commands over and over?
|
|
||||||
// if added this flag should be reset once we advance the nav command
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// clear - clears out mission
|
/// clear - clears out mission
|
||||||
@ -219,13 +104,47 @@ bool AP_Mission::clear()
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// valid - validate the mission has no errors
|
/// 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
|
||||||
/// currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
|
/// should be called at 10hz or higher
|
||||||
bool AP_Mission::valid()
|
void AP_Mission::update()
|
||||||
{
|
{
|
||||||
return true;
|
// 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;
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
///
|
||||||
|
/// public command methods
|
||||||
|
///
|
||||||
|
|
||||||
/// add_cmd - adds a command to the end of the command list and writes to storage
|
/// add_cmd - adds a command to the end of the command list and writes to storage
|
||||||
/// returns true if successfully added, false on failure
|
/// returns true if successfully added, false on failure
|
||||||
/// cmd.index is updated with it's new position in the mission
|
/// cmd.index is updated with it's new position in the mission
|
||||||
@ -244,6 +163,42 @@ bool AP_Mission::add_cmd(Mission_Command& cmd)
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
|
||||||
|
bool AP_Mission::is_nav_cmd(const Mission_Command& cmd)
|
||||||
|
{
|
||||||
|
return (cmd.id <= MAV_CMD_NAV_LAST);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
|
||||||
|
/// returns true if found, false if not found (i.e. reached end of mission command list)
|
||||||
|
/// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
|
||||||
|
bool AP_Mission::get_next_nav_cmd(uint8_t start_index, Mission_Command& cmd)
|
||||||
|
{
|
||||||
|
uint8_t cmd_index = start_index;
|
||||||
|
Mission_Command temp_cmd;
|
||||||
|
|
||||||
|
// search until the end of the mission command list
|
||||||
|
while(cmd_index < _cmd_total) {
|
||||||
|
// get next command
|
||||||
|
if (!get_next_cmd(cmd_index, temp_cmd, false)) {
|
||||||
|
// no more commands so return failure
|
||||||
|
return false;
|
||||||
|
}else{
|
||||||
|
// if found a "navigation" command then return it
|
||||||
|
if (is_nav_cmd(temp_cmd)) {
|
||||||
|
cmd = temp_cmd;
|
||||||
|
return true;
|
||||||
|
}else{
|
||||||
|
// move on in list
|
||||||
|
cmd_index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// if we got this far we did not find a navigation command
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
/// load_cmd_from_storage - load command from storage
|
/// load_cmd_from_storage - load command from storage
|
||||||
/// true is return if successful
|
/// true is return if successful
|
||||||
bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const
|
bool AP_Mission::read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const
|
||||||
@ -283,19 +238,12 @@ bool AP_Mission::write_cmd_to_storage(uint8_t index, Mission_Command& cmd)
|
|||||||
{
|
{
|
||||||
// range check cmd's index
|
// range check cmd's index
|
||||||
if (index >= AP_MISSION_MAX_COMMANDS) {
|
if (index >= AP_MISSION_MAX_COMMANDS) {
|
||||||
// debug
|
|
||||||
hal.console->printf_P(PSTR("fail %d > %d\n"),(int)index,(int)AP_MISSION_MAX_COMMANDS);
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate where in storage the command should be placed
|
// calculate where in storage the command should be placed
|
||||||
uint16_t pos_in_storage = AP_MISSION_EEPROM_START_BYTE + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
|
uint16_t pos_in_storage = AP_MISSION_EEPROM_START_BYTE + (index * AP_MISSION_EEPROM_COMMAND_SIZE);
|
||||||
|
|
||||||
// force home wp to absolute height
|
|
||||||
//if (i == 0) {
|
|
||||||
// temp.options &= ~(AP_MISSION_MASK_OPTIONS_RELATIVE_ALT);
|
|
||||||
//}
|
|
||||||
|
|
||||||
// set location id to cmd.id
|
// set location id to cmd.id
|
||||||
// To-Do: remove id (and p1?) from Location structure
|
// To-Do: remove id (and p1?) from Location structure
|
||||||
cmd.content.location.id = cmd.id;
|
cmd.content.location.id = cmd.id;
|
||||||
@ -319,14 +267,108 @@ void AP_Mission::complete()
|
|||||||
|
|
||||||
// callback to main program's mission complete function
|
// callback to main program's mission complete function
|
||||||
_mission_complete_fn();
|
_mission_complete_fn();
|
||||||
// debug
|
|
||||||
hal.console->printf_P(PSTR("\nMission COMPLETE\n"));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
|
/// advance_current_nav_cmd - moves current nav command forward
|
||||||
bool AP_Mission::is_nav_cmd(const Mission_Command& cmd) const
|
/// 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()
|
||||||
{
|
{
|
||||||
return (cmd.id <= MAV_CMD_NAV_LAST);
|
Mission_Command cmd;
|
||||||
|
uint8_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;
|
||||||
|
|
||||||
|
// get starting point for search
|
||||||
|
cmd_index = _nav_cmd.index;
|
||||||
|
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
|
||||||
|
cmd_index = 0;
|
||||||
|
}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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// advance_current_do_cmd - moves current do command forward
|
||||||
|
/// accounts for do-jump commands
|
||||||
|
void AP_Mission::advance_current_do_cmd()
|
||||||
|
{
|
||||||
|
Mission_Command cmd;
|
||||||
|
uint8_t cmd_index;
|
||||||
|
|
||||||
|
// exit immediately if we're not running
|
||||||
|
if (_flags.state != MISSION_RUNNING) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get starting point for search
|
||||||
|
cmd_index = _do_cmd.index;
|
||||||
|
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
|
||||||
|
cmd_index = 0;
|
||||||
|
}else{
|
||||||
|
// start from one position past the current do command
|
||||||
|
cmd_index++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// check if we've reached end of mission
|
||||||
|
if (cmd_index >= _cmd_total) {
|
||||||
|
// To-Do: set a flag to stop us from attempting to look for do-commands over and over?
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// find next do command
|
||||||
|
if (get_next_do_cmd(cmd_index, cmd)) {
|
||||||
|
// set current do command and start it
|
||||||
|
_do_cmd = cmd;
|
||||||
|
_cmd_start_fn(_do_cmd);
|
||||||
|
}else{
|
||||||
|
// To-Do: set a flag to stop us from attempting to look for do-commands over and over?
|
||||||
|
// if added this flag should be reset once we advance the nav command
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
/// get_next_cmd - gets next command found at or after start_index
|
/// get_next_cmd - gets next command found at or after start_index
|
||||||
@ -351,7 +393,6 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
|
|||||||
if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
|
if (!increment_jump_num_times_if_found && jump_index == cmd_index) {
|
||||||
// we have somehow reached this jump command twice and there is no chance it will complete
|
// we have somehow reached this jump command twice and there is no chance it will complete
|
||||||
// To-Do: log an error?
|
// To-Do: log an error?
|
||||||
hal.console->printf_P(PSTR("\nEndless Loop!\n")); // debug
|
|
||||||
return false;
|
return false;
|
||||||
}else{
|
}else{
|
||||||
if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
|
if (jump_index == AP_MISSION_CMD_INDEX_NONE) {
|
||||||
@ -383,36 +424,6 @@ bool AP_Mission::get_next_cmd(uint8_t start_index, Mission_Command& cmd, bool in
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
|
|
||||||
/// returns true if found, false if not found (i.e. reached end of mission command list)
|
|
||||||
/// accounts for do_jump commands but never increments the jump's num_times_run (advance_current_nav_cmd is responsible for this)
|
|
||||||
bool AP_Mission::get_next_nav_cmd(uint8_t start_index, Mission_Command& cmd)
|
|
||||||
{
|
|
||||||
uint8_t cmd_index = start_index;
|
|
||||||
Mission_Command temp_cmd;
|
|
||||||
|
|
||||||
// search until the end of the mission command list
|
|
||||||
while(cmd_index < _cmd_total) {
|
|
||||||
// get next command
|
|
||||||
if (!get_next_cmd(cmd_index, temp_cmd, false)) {
|
|
||||||
// no more commands so return failure
|
|
||||||
return false;
|
|
||||||
}else{
|
|
||||||
// if found a "navigation" command then return it
|
|
||||||
if (is_nav_cmd(temp_cmd)) {
|
|
||||||
cmd = temp_cmd;
|
|
||||||
return true;
|
|
||||||
}else{
|
|
||||||
// move on in list
|
|
||||||
cmd_index++;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// if we got this far we did not find a navigation command
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
|
|
||||||
/// get_next_do_cmd - gets next "do" or "conditional" command after start_index
|
/// get_next_do_cmd - gets next "do" or "conditional" command after start_index
|
||||||
/// returns true if found, false if not found
|
/// returns true if found, false if not found
|
||||||
/// stops and returns false if it hits another navigation command before it finds the first do or conditional command
|
/// stops and returns false if it hits another navigation command before it finds the first do or conditional command
|
||||||
@ -482,7 +493,7 @@ uint8_t AP_Mission::get_jump_times_run(const Mission_Command& cmd)
|
|||||||
/// increment_jump_times_run - increments the recorded number of times the jump command has been run
|
/// increment_jump_times_run - increments the recorded number of times the jump command has been run
|
||||||
void AP_Mission::increment_jump_times_run(Mission_Command& cmd)
|
void AP_Mission::increment_jump_times_run(Mission_Command& cmd)
|
||||||
{
|
{
|
||||||
// exit immediatley if cmd is not a do-jump command
|
// exit immediately if cmd is not a do-jump command
|
||||||
if (cmd.id != MAV_CMD_DO_JUMP) {
|
if (cmd.id != MAV_CMD_DO_JUMP) {
|
||||||
// To-Do: log an error?
|
// To-Do: log an error?
|
||||||
return;
|
return;
|
||||||
|
@ -82,7 +82,8 @@ public:
|
|||||||
AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
|
AP_Mission(mission_cmd_fn_t cmd_start_fn, mission_cmd_fn_t cmd_verify_fn, mission_complete_fn_t mission_complete_fn) :
|
||||||
_cmd_start_fn(cmd_start_fn),
|
_cmd_start_fn(cmd_start_fn),
|
||||||
_cmd_verify_fn(cmd_verify_fn),
|
_cmd_verify_fn(cmd_verify_fn),
|
||||||
_mission_complete_fn(mission_complete_fn)
|
_mission_complete_fn(mission_complete_fn),
|
||||||
|
_prev_nav_cmd_index(AP_MISSION_CMD_INDEX_NONE)
|
||||||
{
|
{
|
||||||
// load parameter defaults
|
// load parameter defaults
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
@ -98,7 +99,7 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
///
|
///
|
||||||
/// mission methods
|
/// public mission methods
|
||||||
///
|
///
|
||||||
|
|
||||||
/// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
|
/// status - returns the status of the mission (i.e. Mission_Started, Mission_Complete, Mission_Stopped
|
||||||
@ -107,10 +108,6 @@ public:
|
|||||||
/// num_commands - returns total number of commands in the mission
|
/// num_commands - returns total number of commands in the mission
|
||||||
uint8_t num_commands() const { return _cmd_total; }
|
uint8_t num_commands() const { return _cmd_total; }
|
||||||
|
|
||||||
/// 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 update();
|
|
||||||
|
|
||||||
/// start - resets current commands to point to the beginning of the mission
|
/// start - resets current commands to point to the beginning of the mission
|
||||||
/// To-Do: should we validate the mission first and return true/false?
|
/// To-Do: should we validate the mission first and return true/false?
|
||||||
void start();
|
void start();
|
||||||
@ -126,35 +123,37 @@ public:
|
|||||||
/// returns true if mission was running so it could not be cleared
|
/// returns true if mission was running so it could not be cleared
|
||||||
bool clear();
|
bool clear();
|
||||||
|
|
||||||
/// valid - validate the mission has no errors
|
/// 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
|
||||||
/// currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
|
/// should be called at 10hz or higher
|
||||||
bool valid();
|
void update();
|
||||||
|
|
||||||
///
|
///
|
||||||
/// command methods
|
/// public command methods
|
||||||
///
|
///
|
||||||
|
|
||||||
|
/// add_cmd - adds a command to the end of the command list and writes to storage
|
||||||
|
/// returns true if successfully added, false on failure
|
||||||
|
/// cmd.index is updated with it's new position in the mission
|
||||||
|
bool add_cmd(Mission_Command& cmd);
|
||||||
|
|
||||||
|
/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
|
||||||
|
static bool is_nav_cmd(const Mission_Command& cmd);
|
||||||
|
|
||||||
/// get_active_nav_cmd - returns the current "navigation" command
|
/// get_active_nav_cmd - returns the current "navigation" command
|
||||||
const Mission_Command& get_current_nav_cmd() const { return _nav_cmd; }
|
const Mission_Command& get_current_nav_cmd() const { return _nav_cmd; }
|
||||||
|
|
||||||
|
/// get_prev_nav_cmd_index - returns the previous "navigation" commands index (i.e. position in the mission command list)
|
||||||
|
/// we do not return the entire command to save on RAM
|
||||||
|
uint8_t get_prev_nav_cmd_index() { return _prev_nav_cmd_index; }
|
||||||
|
|
||||||
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
|
/// get_next_nav_cmd - gets next "navigation" command found at or after start_index
|
||||||
/// returns true if found, false if not found (i.e. reached end of mission command list)
|
/// returns true if found, false if not found (i.e. reached end of mission command list)
|
||||||
/// accounts for do_jump commands
|
/// accounts for do_jump commands
|
||||||
bool get_next_nav_cmd(uint8_t start_index, Mission_Command& cmd);
|
bool get_next_nav_cmd(uint8_t start_index, Mission_Command& cmd);
|
||||||
|
|
||||||
/// set_current_nav_cmd - sets the current "navigation" command to the command number
|
|
||||||
/// returns true if successful, false on failure (i.e. if the index does not refer to a navigation command)
|
|
||||||
/// current do and conditional commands will also be modified
|
|
||||||
bool set_current_nav_cmd(uint8_t index);
|
|
||||||
|
|
||||||
/// get_active_do_cmd - returns active "do" command
|
/// get_active_do_cmd - returns active "do" command
|
||||||
const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
|
const Mission_Command& get_current_do_cmd() const { return _do_cmd; }
|
||||||
|
|
||||||
/// add_cmd - adds a command to the end of the command list and writes to storage
|
|
||||||
/// returns true if successfully added, false on failure
|
|
||||||
/// cmd.index is updated with it's new position in the mission
|
|
||||||
bool add_cmd(Mission_Command& cmd);
|
|
||||||
|
|
||||||
/// load_cmd_from_storage - load command from storage
|
/// load_cmd_from_storage - load command from storage
|
||||||
/// true is return if successful
|
/// true is return if successful
|
||||||
bool read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const;
|
bool read_cmd_from_storage(uint8_t index, Mission_Command& cmd) const;
|
||||||
@ -185,17 +184,14 @@ private:
|
|||||||
/// advance_current_nav_cmd - moves current nav command forward
|
/// advance_current_nav_cmd - moves current nav command forward
|
||||||
/// do command will also be loaded
|
/// do command will also be loaded
|
||||||
/// accounts for do-jump commands
|
/// accounts for do-jump commands
|
||||||
// will call complete method if it reaches end of mission command list
|
// returns true if command is advanced, false if failed (i.e. mission completed)
|
||||||
void advance_current_nav_cmd();
|
bool advance_current_nav_cmd();
|
||||||
|
|
||||||
/// advance_current_do_cmd - moves current do command forward
|
/// advance_current_do_cmd - moves current do command forward
|
||||||
/// accounts for do-jump commands
|
/// accounts for do-jump commands
|
||||||
/// returns true if successfully advanced (can it ever be unsuccessful?)
|
/// returns true if successfully advanced (can it ever be unsuccessful?)
|
||||||
void advance_current_do_cmd();
|
void advance_current_do_cmd();
|
||||||
|
|
||||||
/// is_nav_cmd - returns true if the command's id is a "navigation" command, false if "do" or "conditional" command
|
|
||||||
bool is_nav_cmd(const Mission_Command& cmd) const;
|
|
||||||
|
|
||||||
/// get_next_cmd - gets next command found at or after start_index
|
/// get_next_cmd - gets next command found at or after start_index
|
||||||
/// returns true if found, false if not found (i.e. mission complete)
|
/// returns true if found, false if not found (i.e. mission complete)
|
||||||
/// accounts for do_jump commands
|
/// accounts for do_jump commands
|
||||||
@ -231,6 +227,7 @@ private:
|
|||||||
// internal variables
|
// internal variables
|
||||||
struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index
|
struct Mission_Command _nav_cmd; // current "navigation" command. It's position in the command list is held in _nav_cmd.index
|
||||||
struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
|
struct Mission_Command _do_cmd; // current "do" command. It's position in the command list is held in _do_cmd.index
|
||||||
|
uint8_t _prev_nav_cmd_index; // index of the previous "navigation" command. Rarely used which is why we don't store the whole command
|
||||||
|
|
||||||
// jump related variables
|
// jump related variables
|
||||||
struct jump_tracking_struct {
|
struct jump_tracking_struct {
|
||||||
|
Loading…
Reference in New Issue
Block a user