Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 11:08:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: fixes to protect against endless loops

Browse Source
This commit is contained in:
Randy Mackay 2014-02-24 14:50:24 +09:00
parent 6086bc14be
commit 84b3497a82
2 changed files with 221 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

389
libraries/AP_Mission/AP_Mission.cpp
View File

@ -20,73 +20,31 @@ const AP_Param::GroupInfo AP_Mission::var_info[] PROGMEM = {
extern const AP_HAL::HAL& hal;
/// public 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)
}
///
/// public mission methods
///
/// 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()
{
hal.console->printf_P(PSTR("\nMission START\n"));
_flags.state = MISSION_RUNNING;
_flags.nav_cmd_loaded = false;
_flags.do_cmd_loaded = 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
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
void AP_Mission::stop()
{
hal.console->printf_P(PSTR("\nMission STOP\n"));
_flags.state = MISSION_STOPPED;
}
@ -94,107 +52,34 @@ void AP_Mission::stop()
/// previous running commands will be re-initialised
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;
}
/// advance_current_nav_cmd - moves current nav command forward
/// do command will also be loaded
/// accounts for do-jump commands
// 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"));
// if no valid nav command index restart from beginning
if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
start();
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) {
// 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);
}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;
}
// 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;
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
@ -219,13 +104,47 @@ bool AP_Mission::clear()
return true;
}
/// valid - validate the mission has no errors
/// currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
bool AP_Mission::valid()
/// 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()
{
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
/// returns true if successfully added, false on failure
/// 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;
}
/// 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
/// true is return if successful
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
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;
}
// 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);
// force home wp to absolute height
//if (i == 0) {
// temp.options &= ~(AP_MISSION_MASK_OPTIONS_RELATIVE_ALT);
//}
// set location id to cmd.id
// To-Do: remove id (and p1?) from Location structure
cmd.content.location.id = cmd.id;
@ -319,14 +267,108 @@ void AP_Mission::complete()
// callback to main program's mission complete function
_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
bool AP_Mission::is_nav_cmd(const Mission_Command& cmd) const
/// 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()
{
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
@ -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) {
// we have somehow reached this jump command twice and there is no chance it will complete
// To-Do: log an error?
hal.console->printf_P(PSTR("\nEndless Loop!\n")); // debug
return false;
}else{
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;
}
/// 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
/// 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
@ -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
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) {
// To-Do: log an error?
return;

47
libraries/AP_Mission/AP_Mission.h
View File

@ -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) :
_cmd_start_fn(cmd_start_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
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
@ -107,10 +108,6 @@ public:
/// num_commands - returns total number of commands in the mission
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
/// To-Do: should we validate the mission first and return true/false?
void start();
@ -126,35 +123,37 @@ public:
/// returns true if mission was running so it could not be cleared
bool clear();
/// valid - validate the mission has no errors
/// currently only checks that the number of do-commands does not exceed the AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS
bool valid();
/// 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();
///
/// 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
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
/// returns true if found, false if not found (i.e. reached end of mission command list)
/// accounts for do_jump commands
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
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
/// true is return if successful
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
/// do command will also be loaded
/// accounts for do-jump commands
// will call complete method if it reaches end of mission command list
void advance_current_nav_cmd();
// returns true if command is advanced, false if failed (i.e. mission completed)
bool advance_current_nav_cmd();
/// advance_current_do_cmd - moves current do command forward
/// accounts for do-jump commands
/// returns true if successfully advanced (can it ever be unsuccessful?)
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
/// returns true if found, false if not found (i.e. mission complete)
/// accounts for do_jump commands
@ -231,6 +227,7 @@ private:
// 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 _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
struct jump_tracking_struct {