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

AP_Mission: take location in get_landing_sequence_start, jump_to_landing_sequence, jump_to_abort_landing_sequence, and is_best_land_sequence, add helpers for scripting

Browse Source
This commit is contained in:
Iampete1 2024-03-27 17:59:52 +00:00 committed by Andrew Tridgell
parent b16f70a83d
commit afe257a8db
2 changed files with 54 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
libraries/AP_Mission/AP_Mission.cpp
View File

@ -2317,14 +2317,8 @@ void AP_Mission::check_eeprom_version()
// find the nearest landing sequence starting point (DO_LAND_START) and // find the nearest landing sequence starting point (DO_LAND_START) and
// return its index. Returns 0 if no appropriate DO_LAND_START point can // return its index. Returns 0 if no appropriate DO_LAND_START point can
// be found. // be found.
uint16_t AP_Mission::get_landing_sequence_start() uint16_t AP_Mission::get_landing_sequence_start(const Location &current_loc)
{ {
Location current_loc;
if (!AP::ahrs().get_location(current_loc)) {
return 0;
}
const Location::AltFrame current_loc_alt_frame = current_loc.get_alt_frame(); const Location::AltFrame current_loc_alt_frame = current_loc.get_alt_frame();
uint16_t landing_start_index = 0; uint16_t landing_start_index = 0;
float min_distance = -1; float min_distance = -1;
@ -2369,9 +2363,9 @@ uint16_t AP_Mission::get_landing_sequence_start()
switch to that mission item. Returns false if no DO_LAND_START switch to that mission item. Returns false if no DO_LAND_START
available. available.
*/ */
bool AP_Mission::jump_to_landing_sequence(void) bool AP_Mission::jump_to_landing_sequence(const Location &current_loc)
{ {
uint16_t land_idx = get_landing_sequence_start(); uint16_t land_idx = get_landing_sequence_start(current_loc);
if (land_idx != 0 && set_current_cmd(land_idx)) { if (land_idx != 0 && set_current_cmd(land_idx)) {
//if the mission has ended it has to be restarted //if the mission has ended it has to be restarted
@ -2389,29 +2383,25 @@ bool AP_Mission::jump_to_landing_sequence(void)
} }
// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
bool AP_Mission::jump_to_abort_landing_sequence(void) bool AP_Mission::jump_to_abort_landing_sequence(const Location &current_loc)
{ {
Location current_loc;
uint16_t abort_index = 0; uint16_t abort_index = 0;
if (AP::ahrs().get_location(current_loc)) { float min_distance = FLT_MAX;
float min_distance = FLT_MAX;
const auto count = num_commands(); const auto count = num_commands();
for (uint16_t i = 1; i < count; i++) { for (uint16_t i = 1; i < count; i++) {
if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) { if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
continue; continue;
} }
Mission_Command tmp; Mission_Command tmp;
if (!read_cmd_from_storage(i, tmp)) { if (!read_cmd_from_storage(i, tmp)) {
continue; continue;
} }
if (tmp.id == MAV_CMD_DO_GO_AROUND) { if (tmp.id == MAV_CMD_DO_GO_AROUND) {
float tmp_distance = tmp.content.location.get_distance(current_loc); float tmp_distance = tmp.content.location.get_distance(current_loc);
if (tmp_distance < min_distance) { if (tmp_distance < min_distance) {
min_distance = tmp_distance; min_distance = tmp_distance;
abort_index = i; abort_index = i;
}
} }
} }
} }
@ -2434,7 +2424,7 @@ bool AP_Mission::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 // 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) bool AP_Mission::is_best_land_sequence(const Location &current_loc)
{ {
// check if there is even a running mission to interupt // check if there is even a running mission to interupt
if (_flags.state != MISSION_RUNNING) { if (_flags.state != MISSION_RUNNING) {
@ -2457,19 +2447,12 @@ bool AP_Mission::is_best_land_sequence(void)
// go through the mission for the nearest DO_LAND_START first as this is the most probable route // 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. // to a landing with the minimum number of WP.
uint16_t do_land_start_index = get_landing_sequence_start(); uint16_t do_land_start_index = get_landing_sequence_start(current_loc);
if (do_land_start_index == 0) { if (do_land_start_index == 0) {
// then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
return false; return false;
} }
// get our current location
Location current_loc;
if (!AP::ahrs().get_location(current_loc)) {
// we don't know where we are!!
return false;
}
// get distance to landing if travelled to nearest DO_LAND_START via RTL // get distance to landing if travelled to nearest DO_LAND_START via RTL
float dist_via_do_land; float dist_via_do_land;
if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) { if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) {
@ -2927,6 +2910,30 @@ void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd,
#endif #endif
} }
// Helpers to fill in location for scripting
#if AP_SCRIPTING_ENABLED
bool AP_Mission::jump_to_landing_sequence(void)
{
Location loc;
if (AP::ahrs().get_location(loc)) {
return jump_to_landing_sequence(loc);
}
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
return false;
}
bool AP_Mission::jump_to_abort_landing_sequence(void)
{
Location loc;
if (AP::ahrs().get_location(loc)) {
return jump_to_abort_landing_sequence(loc);
}
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
return false;
}
#endif // AP_SCRIPTING_ENABLED
// singleton instance // singleton instance
AP_Mission *AP_Mission::_singleton; AP_Mission *AP_Mission::_singleton;

12
libraries/AP_Mission/AP_Mission.h
View File

@ -665,18 +665,24 @@ public:
// find the nearest landing sequence starting point (DO_LAND_START) and // find the nearest landing sequence starting point (DO_LAND_START) and
// return its index. Returns 0 if no appropriate DO_LAND_START point can // return its index. Returns 0 if no appropriate DO_LAND_START point can
// be found. // be found.
uint16_t get_landing_sequence_start(); uint16_t get_landing_sequence_start(const Location &current_loc);
// find the nearest landing sequence starting point (DO_LAND_START) and // find the nearest landing sequence starting point (DO_LAND_START) and
// switch to that mission item. Returns false if no DO_LAND_START // switch to that mission item. Returns false if no DO_LAND_START
// available. // available.
bool jump_to_landing_sequence(void); bool jump_to_landing_sequence(const Location &current_loc);
// jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort // 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(const Location &current_loc);
// Scripting helpers for the above functions to fill in the location
#if AP_SCRIPTING_ENABLED
bool jump_to_landing_sequence(void);
bool jump_to_abort_landing_sequence(void); bool jump_to_abort_landing_sequence(void);
#endif
// check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan // 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); bool is_best_land_sequence(const Location &current_loc);
// set in_landing_sequence flag // set in_landing_sequence flag
void set_in_landing_sequence_flag(bool flag) void set_in_landing_sequence_flag(bool flag)