From da7aa36f2d1bb4f6a1f85ac7e1abfa2c474c37fd Mon Sep 17 00:00:00 2001 From: squilter Date: Wed, 26 Jul 2017 13:12:11 -0400 Subject: [PATCH] AC_WPNav: add set_wp_destination_NED to accept target in meters NED --- libraries/AC_WPNav/AC_WPNav.cpp | 7 +++++++ libraries/AC_WPNav/AC_WPNav.h | 3 +++ 2 files changed, 10 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 416e7b3e9a..15c4b90b01 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -470,6 +470,13 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) return set_wp_origin_and_destination(origin, destination, terrain_alt); } +/// set waypoint destination using NED position vector from ekf origin in meters +bool AC_WPNav::set_wp_destination_NED(const Vector3f& destination_NED) +{ + // convert NED to NEU and do not use terrain following + return set_wp_destination(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false); +} + /// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) /// returns false on failure (likely caused by missing terrain data) diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 2af5fc005b..e8abe42c7f 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -157,6 +157,9 @@ public: /// 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); + /// set waypoint destination using NED position vector from ekf origin in meters + bool set_wp_destination_NED(const Vector3f& destination_NED); + /// set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin in cm) /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) /// returns false on failure (likely caused by missing terrain data)