Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: fix get_wp_destination_loc 

was incorrectly adding alt-above-terrain as if it was alt-above-ekf-origin
also fixup comments
This commit is contained in:
Randy Mackay 2021-01-27 21:35:57 +09:00
parent 32c27b32aa
commit 85b24cf641
2 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -237,14 +237,19 @@ bool AC_WPNav::set_wp_destination_next_loc(const Location& destination)
return set_wp_destination_next(dest_neu, terr_alt);
}
// get destination as a location. Altitude frame will be absolute (AMSL) or above terrain
// returns false if unable to return a destination (for example if origin has not yet been set)
bool AC_WPNav::get_wp_destination_loc(Location& destination) const
{
Vector3f dest = get_wp_destination();
if (!AP::ahrs().get_origin(destination)) {
return false;
}
destination.offset(dest.x*0.01f, dest.y*0.01f);
destination.alt += dest.z;
destination.offset(_destination.x*0.01f, _destination.y*0.01f);
if (_terrain_alt) {
destination.set_alt_cm(_destination.z, Location::AltFrame::ABOVE_TERRAIN);
} else {
destination.alt += _destination.z;
}
return true;
}

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -106,9 +106,8 @@ public:
bool set_wp_destination_loc(const Location& destination);
bool set_wp_destination_next_loc(const Location& destination);
// returns wp location using location class.
// returns false if unable to convert from target vector to global
// coordinates
// get destination as a location. Altitude frame will be absolute (AMSL) or above terrain
// returns false if unable to return a destination (for example if origin has not yet been set)
bool get_wp_destination_loc(Location& destination) const;
// returns object avoidance adjusted destination which is always the same as get_wp_destination