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

AP_Mission: caculate distance to do_land_start with no location correctly

Browse Source
This commit is contained in:
Iampete1 2021-07-25 00:21:07 +01:00 committed by Randy Mackay
parent cea54c48cd
commit 32b3c8ea08
2 changed files with 6 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Mission/AP_Mission.cpp
View File

@ -1980,7 +1980,7 @@ uint16_t AP_Mission::num_commands_max(void) const
// 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() const
uint16_t AP_Mission::get_landing_sequence_start()
{
struct Location current_loc;
@ -1998,6 +1998,10 @@ uint16_t AP_Mission::get_landing_sequence_start() const
continue;
}
if (tmp.id == MAV_CMD_DO_LAND_START) {
if (!tmp.content.location.initialised() && !get_next_nav_cmd(i, tmp)) {
// command does not have a valid location and cannot get next valid
continue;
}
float tmp_distance = tmp.content.location.get_distance(current_loc);
if (min_distance < 0 || tmp_distance < min_distance) {
min_distance = tmp_distance;

2
libraries/AP_Mission/AP_Mission.h
View File

@ -532,7 +532,7 @@ 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() const;
uint16_t get_landing_sequence_start();
// find the nearest landing sequence starting point (DO_LAND_START) and
// switch to that mission item. Returns false if no DO_LAND_START