Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 09:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: Check if failsafes should interupt landing approach

Browse Source
This commit is contained in:
Gone4Dirt 2020-02-17 23:44:53 +00:00 committed by Peter Barker
parent 32ecd904bd
commit 44718cd00f
2 changed files with 174 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

163
libraries/AP_Mission/AP_Mission.cpp
View File

@ -27,7 +27,7 @@ const AP_Param::GroupInfo AP_Mission::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Mission options bitmask
// @Description: Bitmask of what options to use in missions.
// @Bitmask: 0:Clear Mission on reboot
// @Bitmask: 0:Clear Mission on reboot, 1:Use distance to land calc on battery failsafe
// @User: Advanced
AP_GROUPINFO("OPTIONS", 2, AP_Mission, _options, AP_MISSION_OPTIONS_DEFAULT),
@ -176,6 +176,7 @@ void AP_Mission::reset()
_flags.nav_cmd_loaded = false;
_flags.do_cmd_loaded = false;
_flags.do_cmd_all_done = false;
_flags.in_landing_sequence = 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;
@ -282,6 +283,11 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
bool AP_Mission::start_command(const Mission_Command& cmd)
{
// check for landing related commands and set in_landing_sequence flag
if (is_landing_type_cmd(cmd.id) || cmd.id == MAV_CMD_DO_LAND_START) {
set_in_landing_sequence_flag(true);
}
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u %s", cmd.index, cmd.type());
switch (cmd.id) {
case MAV_CMD_DO_GRIPPER:
@ -391,6 +397,12 @@ int32_t AP_Mission::get_next_ground_course_cd(int32_t default_angle)
// set_current_cmd - jumps to command specified by index
bool AP_Mission::set_current_cmd(uint16_t index)
{
// read command to check for DO_LAND_START
Mission_Command cmd;
if (!read_cmd_from_storage(index, cmd) || (cmd.id != MAV_CMD_DO_LAND_START)) {
_flags.in_landing_sequence = false;
}
// sanity check index and that we have a mission
if (index >= (unsigned)_cmd_total || _cmd_total == 1) {
return false;
@ -422,7 +434,6 @@ bool AP_Mission::set_current_cmd(uint16_t index)
// search until we find next nav command or reach end of command list
while (!_flags.nav_cmd_loaded) {
// get next command
Mission_Command cmd;
if (!get_next_cmd(index, cmd, true)) {
_nav_cmd.index = AP_MISSION_CMD_INDEX_NONE;
return false;
@ -1460,6 +1471,7 @@ void AP_Mission::complete()
{
// flag mission as complete
_flags.state = MISSION_COMPLETE;
_flags.in_landing_sequence = false;
// callback to main program's mission complete function
_mission_complete_fn();
@ -1583,7 +1595,7 @@ void AP_Mission::advance_current_do_cmd()
/// returns true if found, false if not found (i.e. mission complete)
/// accounts for do_jump commands
/// increment_jump_num_times_if_found should be set to true if advancing the active navigation command
bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found)
bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg)
{
uint16_t cmd_index = start_index;
Mission_Command temp_cmd;
@ -1633,7 +1645,7 @@ bool AP_Mission::get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool i
if (jump_times_run < temp_cmd.content.jump.num_times) {
// update the record of the number of times run
if (increment_jump_num_times_if_found) {
increment_jump_times_run(temp_cmd);
increment_jump_times_run(temp_cmd, send_gcs_msg);
}
// continue searching from jump target
cmd_index = temp_cmd.content.jump.target;
@ -1720,7 +1732,7 @@ int16_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)
void AP_Mission::increment_jump_times_run(Mission_Command& cmd, bool send_gcs_msg)
{
// exit immediately if cmd is not a do-jump command
if (cmd.id != MAV_CMD_DO_JUMP) {
@ -1732,7 +1744,9 @@ void AP_Mission::increment_jump_times_run(Mission_Command& cmd)
for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
if (_jump_tracking[i].index == cmd.index) {
_jump_tracking[i].num_times_run++;
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
if (send_gcs_msg) {
gcs().send_text(MAV_SEVERITY_INFO, "Mission: %u Jump %i/%i", _jump_tracking[i].index, _jump_tracking[i].num_times_run, cmd.content.jump.num_times);
}
return;
}else if(_jump_tracking[i].index == AP_MISSION_CMD_INDEX_NONE) {
// we've searched through all known jump commands and haven't found it so allocate new space in _jump_tracking array
@ -1818,6 +1832,7 @@ bool AP_Mission::jump_to_landing_sequence(void)
}
gcs().send_text(MAV_SEVERITY_INFO, "Landing sequence start");
_flags.in_landing_sequence = true;
return true;
}
@ -1856,6 +1871,8 @@ bool AP_Mission::jump_to_abort_landing_sequence(void)
resume();
}
_flags.in_landing_sequence = false;
gcs().send_text(MAV_SEVERITY_INFO, "Landing abort sequence start");
return true;
}
@ -1864,6 +1881,140 @@ bool AP_Mission::jump_to_abort_landing_sequence(void)
return false;
}
// check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
bool AP_Mission::is_best_land_sequence(void)
{
// check if there is even a running mission to interupt
if (_flags.state != MISSION_RUNNING) {
return false;
}
// check if aircraft has already jumped to a landing sequence
if (_flags.in_landing_sequence) {
return true;
}
// check if MIS_OPTIONS bit set to allow distance calculation to be done
if (!(_options & AP_MISSION_MASK_DIST_TO_LAND_CALC)) {
return false;
}
// The decision to allow a failsafe to interupt a potential landing approach
// is a distance travelled minimization problem. Look forward in
// mission to evaluate the shortest remaining distance to land.
// go through the mission for the nearest DO_LAND_START first as this is the most probable route
// to a landing with the minimum number of WP.
uint16_t do_land_start_index = get_landing_sequence_start();
if (do_land_start_index == 0) {
// then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
return false;
}
// get our current location
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
// we don't know where we are!!
return false;
}
// get distance to landing if travelled to nearest DO_LAND_START via RTL
float dist_via_do_land;
if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) {
// cant get a valid distance to landing
return false;
}
// get distance to landing if continue along current mission path
float dist_continue_to_land;
if (!distance_to_landing(_nav_cmd.index, dist_continue_to_land, current_loc)) {
// cant get a valid distance to landing
return false;
}
// compare distances
if (dist_via_do_land >= dist_continue_to_land) {
// then the mission should carry on uninterrupted as that is the shorter distance
gcs().send_text(MAV_SEVERITY_NOTICE, "Rejecting RTL: closer land if mis continued");
return true;
} else {
// allow failsafes to interrupt the current mission
return false;
}
}
// Approximate the distance travelled to get to a landing. DO_JUMP commands are observed in look forward.
bool AP_Mission::distance_to_landing(uint16_t index, float &tot_distance, Location prev_loc)
{
Mission_Command temp_cmd;
tot_distance = 0.0f;
bool ret;
// back up jump tracking to reset after distance calculation
jump_tracking_struct _jump_tracking_backup[AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS];
for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
_jump_tracking_backup[i] = _jump_tracking[i];
}
// run through remainder of mission to approximate a distance to landing
for (uint8_t i=0; i<255; i++) {
// search until the end of the mission command list
for (uint16_t cmd_index = index; cmd_index < (unsigned)_cmd_total; cmd_index++) {
// get next command
if (!get_next_cmd(cmd_index, temp_cmd, true, false)) {
// we got to the end of the mission
ret = false;
goto reset_do_jump_tracking;
}
if (temp_cmd.id == MAV_CMD_NAV_WAYPOINT || temp_cmd.id == MAV_CMD_NAV_SPLINE_WAYPOINT || is_landing_type_cmd(temp_cmd.id)) {
break;
} else if (is_nav_cmd(temp_cmd) || temp_cmd.id == MAV_CMD_CONDITION_DELAY) {
// if we receive a nav command that we dont handle then give up as cant measure the distance e.g. MAV_CMD_NAV_LOITER_UNLIM
ret = false;
goto reset_do_jump_tracking;
}
}
index = temp_cmd.index+1;
if (!(temp_cmd.content.location.lat == 0 && temp_cmd.content.location.lng == 0)) {
// add distance to running total
float disttemp = prev_loc.get_distance(temp_cmd.content.location);
tot_distance = tot_distance + disttemp;
// store wp location as previous
prev_loc = temp_cmd.content.location;
}
if (is_landing_type_cmd(temp_cmd.id)) {
// reached a landing!
ret = true;
goto reset_do_jump_tracking;
}
}
// reached end of loop without getting to a landing
ret = false;
reset_do_jump_tracking:
for (uint8_t i=0; i<AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS; i++) {
_jump_tracking[i] = _jump_tracking_backup[i];
}
return ret;
}
// check if command is a landing type command.
bool AP_Mission::is_landing_type_cmd(uint16_t id) const
{
switch (id) {
case MAV_CMD_NAV_LAND:
case MAV_CMD_NAV_VTOL_LAND:
case MAV_CMD_DO_PARACHUTE:
return true;
default:
return false;
}
}
const char *AP_Mission::Mission_Command::type() const {
switch(id) {
case MAV_CMD_NAV_WAYPOINT:

19
libraries/AP_Mission/AP_Mission.h
View File

@ -38,6 +38,7 @@
#define AP_MISSION_OPTIONS_DEFAULT 0 // Do not clear the mission when rebooting
#define AP_MISSION_MASK_MISSION_CLEAR (1<<0) // If set then Clear the mission on boot
#define AP_MISSION_MASK_DIST_TO_LAND_CALC (1<<1) // Allow distance to best landing calculation to be run on failsafe
/// @class AP_Mission
/// @brief Object managing Mission
@ -303,6 +304,7 @@ public:
_flags.state = MISSION_STOPPED;
_flags.nav_cmd_loaded = false;
_flags.do_cmd_loaded = false;
_flags.in_landing_sequence = false;
}
// get singleton instance
@ -471,6 +473,12 @@ public:
// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
bool jump_to_abort_landing_sequence(void);
// check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
bool is_best_land_sequence(void);
// set in_landing_sequence flag
void set_in_landing_sequence_flag(bool flag) { _flags.in_landing_sequence = flag; }
// get a reference to the AP_Mission semaphore, allowing an external caller to lock the
// storage while working with multiple waypoints
HAL_Semaphore &get_semaphore(void) {
@ -495,6 +503,7 @@ private:
uint8_t nav_cmd_loaded : 1; // true if a "navigation" command has been loaded into _nav_cmd
uint8_t do_cmd_loaded : 1; // true if a "do"/"conditional" command has been loaded into _do_cmd
uint8_t do_cmd_all_done : 1; // true if all "do"/"conditional" commands have been completed (stops unnecessary searching through eeprom for do commands)
bool in_landing_sequence : 1; // true if the mission has jumped to a landing
} _flags;
///
@ -523,7 +532,7 @@ private:
/// returns true if found, false if not found (i.e. mission complete)
/// accounts for do_jump commands
/// increment_jump_num_times_if_found should be set to true if advancing the active navigation command
bool get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found);
bool get_next_cmd(uint16_t start_index, Mission_Command& cmd, bool increment_jump_num_times_if_found, bool send_gcs_msg = true);
/// get_next_do_cmd - gets next "do" or "conditional" command after start_index
/// returns true if found, false if not found
@ -542,12 +551,18 @@ private:
int16_t 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 increment_jump_times_run(Mission_Command& cmd);
void increment_jump_times_run(Mission_Command& cmd, bool send_gcs_msg = true);
/// check_eeprom_version - checks version of missions stored in eeprom matches this library
/// command list will be cleared if they do not match
void check_eeprom_version();
// check if command is a landing type command. Asside the obvious, MAV_CMD_DO_PARACHUTE is considered a type of landing
bool is_landing_type_cmd(uint16_t id) const;
// approximate the distance travelled to get to a landing. DO_JUMP commands are observed in look forward.
bool distance_to_landing(uint16_t index, float &tot_distance,Location current_loc);
/// 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);