From 1d945cc5ac8b6555e0c86500653b74fb41106111 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 6 Jun 2024 14:43:45 +1000 Subject: [PATCH] AP_Common: Location: allow retrieval of Vector3p from vector-from-origin --- libraries/AP_Common/Location.cpp | 19 +++++++++++++++++-- libraries/AP_Common/Location.h | 6 ++++-- 2 files changed, 21 insertions(+), 4 deletions(-) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 241f5224f6..27803566d0 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -224,7 +224,8 @@ bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const #if AP_AHRS_ENABLED // converts location to a vector from origin; if this method returns // false then vec_ne is unmodified -bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const +template +bool Location::get_vector_xy_from_origin_NE(T &vec_ne) const { Location ekf_origin; if (!AP::ahrs().get_origin(ekf_origin)) { @@ -235,9 +236,16 @@ bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const return true; } +// define for float and position vectors +template bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const; +#if HAL_WITH_POSTYPE_DOUBLE +template bool Location::get_vector_xy_from_origin_NE(Vector2p &vec_ne) const; +#endif + // converts location to a vector from origin; if this method returns // false then vec_neu is unmodified -bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const +template +bool Location::get_vector_from_origin_NEU(T &vec_neu) const { // convert altitude int32_t alt_above_origin_cm = 0; @@ -254,6 +262,13 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const return true; } + +// define for float and position vectors +template bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const; +#if HAL_WITH_POSTYPE_DOUBLE +template bool Location::get_vector_from_origin_NEU(Vector3p &vec_neu) const; +#endif + #endif // AP_AHRS_ENABLED // return horizontal distance in meters between two locations diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 4b9ad8ba2f..f61756d7d9 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -60,10 +60,12 @@ public: // happen if the EKF origin has not been set yet x, y and z are in // centimetres. If this method returns false then vec_ne is // unmodified. - bool get_vector_xy_from_origin_NE(Vector2f &vec_ne) const WARN_IF_UNUSED; + template + bool get_vector_xy_from_origin_NE(T &vec_ne) const WARN_IF_UNUSED; // converts location to a vector from origin; if this method returns // false then vec_neu is unmodified - bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED; + template + bool get_vector_from_origin_NEU(T &vec_neu) const WARN_IF_UNUSED; // return horizontal distance in meters between two locations ftype get_distance(const Location &loc2) const;