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

AP_Mission: only continue after land if next waypoint is a takeoff

Browse Source
This commit is contained in:
Iampete1 2021-08-30 21:22:58 +01:00 committed by Randy Mackay
parent 09b26e4d67
commit d9e30452ed
2 changed files with 36 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
libraries/AP_Mission/AP_Mission.cpp
View File

@ -149,15 +149,10 @@ void AP_Mission::resume()
// update will take care of finding and starting the nav command
}
/// check mission starts with a takeoff command
bool AP_Mission::starts_with_takeoff_cmd()
/// check if the next nav command is a takeoff, skipping delays
bool AP_Mission::is_takeoff_next(uint16_t cmd_index)
{
Mission_Command cmd = {};
uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}
// check a maximum of 16 items, remembering that missions can have
// loops in them
for (uint8_t i=0; i<16; i++, cmd_index++) {
@ -180,6 +175,33 @@ bool AP_Mission::starts_with_takeoff_cmd()
return false;
}
/// check mission starts with a takeoff command
bool AP_Mission::starts_with_takeoff_cmd()
{
uint16_t cmd_index = _restart ? AP_MISSION_CMD_INDEX_NONE : _nav_cmd.index;
if (cmd_index == AP_MISSION_CMD_INDEX_NONE) {
cmd_index = AP_MISSION_FIRST_REAL_COMMAND;
}
return is_takeoff_next(cmd_index);
}
/*
return true if MIS_OPTIONS is set to allow continue of mission
logic after a land and the next waypoint is a takeoff. If this
is false then after a landing is complete the vehicle should
disarm and mission logic should stop
*/
bool AP_Mission::continue_after_land_check_for_takeoff()
{
if (!continue_after_land()) {
return false;
}
if (_nav_cmd.index == AP_MISSION_CMD_INDEX_NONE) {
return false;
}
return is_takeoff_next(_nav_cmd.index+1);
}
/// start_or_resume - if MIS_AUTORESTART=0 this will call resume(), otherwise it will call start()
void AP_Mission::start_or_resume()
{

9
libraries/AP_Mission/AP_Mission.h
View File

@ -577,9 +577,11 @@ public:
/*
return true if MIS_OPTIONS is set to allow continue of mission
logic after a land. If this is false then after a landing is
complete the vehicle should disarm and mission logic should stop
logic after a land and the next waypoint is a takeoff. If this
is false then after a landing is complete the vehicle should
disarm and mission logic should stop
*/
bool continue_after_land_check_for_takeoff(void);
bool continue_after_land(void) const {
return (_options.get() & AP_MISSION_MASK_CONTINUE_AFTER_LAND) != 0;
}
@ -676,6 +678,9 @@ private:
/// sanity checks that the masked fields are not NaN's or infinite
static MAV_MISSION_RESULT sanity_check_params(const mavlink_mission_item_int_t& packet);
/// check if the next nav command is a takeoff, skipping delays
bool is_takeoff_next(uint16_t start_index);
// pointer to main program functions
mission_cmd_fn_t _cmd_start_fn; // pointer to function which will be called when a new command is started
mission_cmd_fn_t _cmd_verify_fn; // pointer to function which will be called repeatedly to ensure a command is progressing