Skip to content

Commit

Permalink
AP_Mission: take location in get_landing_sequence_start, `jump_to_l…
Browse files Browse the repository at this point in the history 
…anding_sequence`, `jump_to_abort_landing_sequence`, and `is_best_land_sequence`, add helpers for scripting
  • Loading branch information
@IamPete1 @tridge
IamPete1 authored and tridge committed Apr 2, 2024
1 parent b16f70a commit afe257a
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 41 deletions.
83 changes: 45 additions & 38 deletions libraries/AP_Mission/AP_Mission.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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
Expand All @@ -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;
}
}
}
Expand All @@ -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) {
Expand All @@ -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)) {
Expand Down Expand Up @@ -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;

Expand Down
12 changes: 9 additions & 3 deletions libraries/AP_Mission/AP_Mission.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -665,18 +665,24 @@ class AP_Mission
// 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)
Expand Down

0 comments on commit afe257a

Please sign in to comment.