From 85b24cf641c4c58156546997feac835bd96b7787 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 27 Jan 2021 21:35:57 +0900 Subject: [PATCH] AC_WPNav: fix get_wp_destination_loc was incorrectly adding alt-above-terrain as if it was alt-above-ekf-origin also fixup comments --- libraries/AC_WPNav/AC_WPNav.cpp | 11 ++++++++--- libraries/AC_WPNav/AC_WPNav.h | 5 ++--- 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 819f2a7f29..fd373f6482 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -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; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 460f4c1f1a..e9faeadac2 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -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