diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index a229968c59..809802011a 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -517,6 +517,24 @@ bool AP_Mission::set_current_cmd(uint16_t index, bool rewind) return true; } +// restart current navigation command. Used to handle external changes to mission +// returns true on success, false if mission is not running or current nav command is invalid +bool AP_Mission::restart_current_nav_cmd() +{ + // return immediately if mission is not running + if (_flags.state != MISSION_RUNNING) { + return false; + } + + // return immediately if nav command index is invalid + const uint16_t nav_cmd_index = get_current_nav_index(); + if ((nav_cmd_index == 0) || (nav_cmd_index >= num_commands())) { + return false; + } + + return set_current_cmd(_nav_cmd.index); +} + // returns false on any issue at all. bool AP_Mission::set_item(uint16_t index, mavlink_mission_item_int_t& src_packet) { diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index fcafdcbe3c..ae05784fce 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -494,6 +494,10 @@ public: // set_current_cmd - jumps to command specified by index bool set_current_cmd(uint16_t index, bool rewind = false); + // restart current navigation command. Used to handle external changes to mission + // returns true on success, false if current nav command has been deleted + bool restart_current_nav_cmd(); + /// load_cmd_from_storage - load command from storage /// true is return if successful bool read_cmd_from_storage(uint16_t index, Mission_Command& cmd) const;