diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index 1cdd67e651..80fbf2d22f 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -417,6 +417,11 @@ void QuaternionT::from_euler(T roll, T pitch, T yaw) q3 = cr2*sp2*cy2 + sr2*cp2*sy2; q4 = cr2*cp2*sy2 - sr2*sp2*cy2; } +template +void QuaternionT::from_euler(const Vector3 &v) +{ + from_euler(v[0], v[1], v[2]); +} // create a quaternion from Euler angles applied in yaw, roll, pitch order // instead of the normal yaw, pitch, roll order diff --git a/libraries/AP_Math/quaternion.h b/libraries/AP_Math/quaternion.h index b6ad586610..3a0c479fa6 100644 --- a/libraries/AP_Math/quaternion.h +++ b/libraries/AP_Math/quaternion.h @@ -81,6 +81,7 @@ public: // create a quaternion from Euler angles void from_euler(T roll, T pitch, T yaw); + void from_euler(const Vector3 &v); // create a quaternion from Euler angles applied in yaw, roll, pitch order // instead of the normal yaw, pitch, roll order