From 2c962afe98290050e8678d993db51c4559d3e04d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 11 May 2018 21:54:14 +1000 Subject: [PATCH] AC_WPNav: add get_wp_destination to get a Location_Class for the wp --- libraries/AC_WPNav/AC_WPNav.cpp | 10 ++++++++++ libraries/AC_WPNav/AC_WPNav.h | 5 +++++ 2 files changed, 15 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 629bef2ba7..b812a2095e 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -201,6 +201,16 @@ bool AC_WPNav::set_wp_destination(const Location_Class& destination) return set_wp_destination(dest_neu, terr_alt); } +bool AC_WPNav::get_wp_destination(Location_Class& destination) { + 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; + return true; +} + /// set_wp_destination waypoint using position vector (distance from home in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 9dd2e32c51..70baa04762 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -103,6 +103,11 @@ public: /// returns false if conversion from location to vector from ekf origin cannot be calculated bool set_wp_destination(const Location_Class& destination); + // returns wp location using location class. + // returns false if unable to convert from target vector to global + // coordinates + bool get_wp_destination(Location_Class& destination); + /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false);