From ddccc92e7dd92dbf28805ee0778ee355c066b323 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 1 Dec 2023 08:59:18 +1100 Subject: [PATCH] AP_Math: added rfu_to_frd() method for Vector3 --- libraries/AP_Math/vector3.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 08f6337cef..d4d3a1baa6 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -281,6 +281,12 @@ public: return Vector3{x,y,z}; } + // convert from right-front-up to front-right-down + // or ENU to NED + Vector3 rfu_to_frd() const { + return Vector3{y,x,-z}; + } + // given a position p1 and a velocity v1 produce a vector // perpendicular to v1 maximising distance from p1. If p1 is the // zero vector the return from the function will always be the