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

AC_WPNav: simplify get_wp_destination_loc

This commit is contained in:
Josh Henderson 2021-05-05 13:08:42 -04:00 committed by Randy Mackay
parent ac57e91e8f
commit 156da6431f
1 changed files with 3 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_WPNav/AC_WPNav.cpp
View File

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