aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mission: create start_command and verify_command wrappers

Browse Source
This commit is contained in:
Peter Barker 2018-10-23 13:56:46 +11:00 committed by Peter Barker
parent 3505314e64
commit 8ebec6a237
2 changed files with 20 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Mission/AP_Mission.cpp
View File

@ -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;

3
libraries/AP_Mission/AP_Mission.h
View File

@ -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