diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 7a31550946..284123357c 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -235,7 +235,7 @@ void AP_Mission::update() } }else{ // run the active nav command - if (_cmd_verify_fn(_nav_cmd)) { + if (verify_command(_nav_cmd)) { // market _nav_cmd as complete (it will be started on the next iteration) _flags.nav_cmd_loaded = false; // immediately advance to the next mission command @@ -251,14 +251,24 @@ void AP_Mission::update() if (!_flags.do_cmd_loaded) { 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) + // check the active do command + if (verify_command(_do_cmd)) { + // mark _do_cmd as complete _flags.do_cmd_loaded = false; } } } +bool AP_Mission::verify_command(const Mission_Command& cmd) +{ + return _cmd_verify_fn(cmd); +} + +bool AP_Mission::start_command(const Mission_Command& cmd) +{ + return _cmd_start_fn(cmd); +} + /// /// public command methods /// @@ -1411,7 +1421,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index) } // set current navigation command and start it _nav_cmd = cmd; - if (_cmd_start_fn(_nav_cmd)) { + if (start_command(_nav_cmd)) { _flags.nav_cmd_loaded = true; } }else{ @@ -1419,7 +1429,7 @@ bool AP_Mission::advance_current_nav_cmd(uint16_t starting_index) if (!_flags.do_cmd_loaded) { _do_cmd = cmd; _flags.do_cmd_loaded = true; - _cmd_start_fn(_do_cmd); + start_command(_do_cmd); } else { // protect against endless loops of do-commands if (max_loops-- == 0) { @@ -1473,7 +1483,7 @@ void AP_Mission::advance_current_do_cmd() // set current do command and start it _do_cmd = cmd; _flags.do_cmd_loaded = true; - _cmd_start_fn(_do_cmd); + start_command(_do_cmd); }else{ // set flag to stop unnecessarily searching for do commands _flags.do_cmd_all_done = true; diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index 267349def5..542d9ce5ff 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -499,6 +499,9 @@ private: /// complete - mission is marked complete and clean-up performed including calling the mission_complete_fn void complete(); + bool verify_command(const Mission_Command& cmd); + bool start_command(const Mission_Command& cmd); + /// advance_current_nav_cmd - moves current nav command forward // starting_index is used to set the index from which searching will begin, leave as 0 to search from the current navigation target /// do command will also be loaded