Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -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
// return its index. Returns 0 if no appropriate DO_LAND_START point can
// 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();
uint16_t landing_start_index = 0;
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
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 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
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;
if (AP::ahrs().get_location(current_loc)) {
float min_distance = FLT_MAX;
float min_distance = FLT_MAX;
const auto count = num_commands();
for (uint16_t i = 1; i < count; i++) {
if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
continue;
}
Mission_Command tmp;
if (!read_cmd_from_storage(i, tmp)) {
continue;
}
if (tmp.id == MAV_CMD_DO_GO_AROUND) {
float tmp_distance = tmp.content.location.get_distance(current_loc);
if (tmp_distance < min_distance) {
min_distance = tmp_distance;
abort_index = i;
}
const auto count = num_commands();
for (uint16_t i = 1; i < count; i++) {
if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
continue;
}
Mission_Command tmp;
if (!read_cmd_from_storage(i, tmp)) {
continue;
}
if (tmp.id == MAV_CMD_DO_GO_AROUND) {
float tmp_distance = tmp.content.location.get_distance(current_loc);
if (tmp_distance < min_distance) {
min_distance = tmp_distance;
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
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
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
// 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) {
// 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_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
float dist_via_do_land;
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
}
// 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
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
// return its index. Returns 0 if no appropriate DO_LAND_START point can
// 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
// switch to that mission item. Returns false if no DO_LAND_START
// 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
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);
#endif
// 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
void set_in_landing_sequence_flag(bool flag)