From a91c0f4b12e873d2ae8d383a992e2e7c56b315c4 Mon Sep 17 00:00:00 2001 From: Josh Henderson Date: Wed, 5 May 2021 13:08:42 -0400 Subject: [PATCH] AC_WPNav: simplify get_wp_destination_loc --- libraries/AC_WPNav/AC_WPNav.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index afc5b9206a..500212e807 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -261,19 +261,15 @@ 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 +// 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) bool AC_WPNav::get_wp_destination_loc(Location& destination) const { if (!AP::ahrs().get_origin(destination)) { return false; } - 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; - } + + destination = Location{get_wp_destination(), _terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN}; return true; }