From 84b3497a82b985ffa85ac0e15eaf933e8f9307ed Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 24 Feb 2014 14:50:24 +0900 Subject: [PATCH] AP_Mission: fixes to protect against endless loops --- libraries/AP_Mission/AP_Mission.cpp | 387 ++++++++++++++-------------- libraries/AP_Mission/AP_Mission.h | 47 ++-- 2 files changed, 221 insertions(+), 213 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 78b16562f8..14e1e2b0b0 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -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,106 +52,33 @@ void AP_Mission::stop() /// previous running commands will be re-initialised void AP_Mission::resume() { - hal.console->printf_P(PSTR("\nMission RESUME\n")); - _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 mission had completed then start it from the first command + if (_flags.state == MISSION_COMPLETE) { + start(); return; } - // exit immediately if current nav command has not completed - if (_flags.nav_cmd_loaded) { - // debug - hal.console->printf_P(PSTR("\nACNC: curr nav not complete\n")); - return; - } + // if mission had stopped then restart it + if (_flags.state == MISSION_STOPPED) { + _flags.state = MISSION_RUNNING; - // 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(); + // if no valid nav command index restart from beginning + if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) { + start(); 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; + // 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); - }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); - } } - } -} -/// 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 + // restart active do command + if (_flags.do_cmd_loaded && _do_cmd.index != AP_MISSION_CMD_INDEX_NONE) { + _cmd_start_fn(_do_cmd); + } } } @@ -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; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 57712dca79..2307cd920b 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -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 {