Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Common: Location: allow retrieval of Vector3p from vector-from-origin

This commit is contained in:
Peter Barker 2024-06-06 14:43:45 +10:00 committed by Peter Barker
parent 0fc6fc4e9c
commit 1d945cc5ac
2 changed files with 21 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Common/Location.cpp
View File

@ -224,7 +224,8 @@ bool Location::get_alt_m(AltFrame desired_frame, float &ret_alt) const
#if AP_AHRS_ENABLED #if AP_AHRS_ENABLED
// converts location to a vector from origin; if this method returns // converts location to a vector from origin; if this method returns
// false then vec_ne is unmodified // false then vec_ne is unmodified
bool Location::get_vector_xy_from_origin_NE(Vector2f &vec_ne) const template<typename T>
bool Location::get_vector_xy_from_origin_NE(T &vec_ne) const
{ {
Location ekf_origin; Location ekf_origin;
if (!AP::ahrs().get_origin(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; return true;
} }
// define for float and position vectors
template bool Location::get_vector_xy_from_origin_NE<Vector2f>(Vector2f &vec_ne) const;
#if HAL_WITH_POSTYPE_DOUBLE
template bool Location::get_vector_xy_from_origin_NE<Vector2p>(Vector2p &vec_ne) const;
#endif
// converts location to a vector from origin; if this method returns // converts location to a vector from origin; if this method returns
// false then vec_neu is unmodified // false then vec_neu is unmodified
bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const template<typename T>
bool Location::get_vector_from_origin_NEU(T &vec_neu) const
{ {
// convert altitude // convert altitude
int32_t alt_above_origin_cm = 0; int32_t alt_above_origin_cm = 0;
@ -254,6 +262,13 @@ bool Location::get_vector_from_origin_NEU(Vector3f &vec_neu) const
return true; return true;
} }
// define for float and position vectors
template bool Location::get_vector_from_origin_NEU<Vector3f>(Vector3f &vec_neu) const;
#if HAL_WITH_POSTYPE_DOUBLE
template bool Location::get_vector_from_origin_NEU<Vector3p>(Vector3p &vec_neu) const;
#endif
#endif // AP_AHRS_ENABLED #endif // AP_AHRS_ENABLED
// return horizontal distance in meters between two locations // return horizontal distance in meters between two locations

6
libraries/AP_Common/Location.h
View File

@ -60,10 +60,12 @@ public:
// happen if the EKF origin has not been set yet x, y and z are in // 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 // centimetres. If this method returns false then vec_ne is
// unmodified. // unmodified.
bool get_vector_xy_from_origin_NE(Vector2f &vec_ne) const WARN_IF_UNUSED; template<typename T>
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 // converts location to a vector from origin; if this method returns
// false then vec_neu is unmodified // false then vec_neu is unmodified
bool get_vector_from_origin_NEU(Vector3f &vec_neu) const WARN_IF_UNUSED; template<typename T>
bool get_vector_from_origin_NEU(T &vec_neu) const WARN_IF_UNUSED;
// return horizontal distance in meters between two locations // return horizontal distance in meters between two locations
ftype get_distance(const Location &loc2) const; ftype get_distance(const Location &loc2) const;