Commit 10a26dbd by Sebastian Riebeling

### added Quaternion to YPR function

parent 4effa1fe
 ... ... @@ -26,6 +26,11 @@ #include #include #include #include #include // Ein Makro für die Berechnung von Prozentsätzen (Wieviel Prozent p sind P von G?) // Wichtiges Feature: Behandlung des Falles G = 0. Rückgabewert in diesem Fall 0. #define PERCENT_OF(P, G) (G == 0 ? 0 : 100*(double) P/(double) G) ... ... @@ -342,6 +347,11 @@ ITA_BASE_API void convertVU2YPR( const double vx, const double vy, const double //! Orientierung in View-Up-Vektor in Yaw-Pitch-Roll Winkeln umrechnen (Alle Winkel in Bogenmaß) ITA_BASE_API void convertVU2YPR( const float vx, const float vy, const float vz, const float ux, const float uy, const float uz, float& yaw, float& pitch, float& roll ); //! Orientation in Quaternion: calculate Yaw-Pitch-Roll angles (all angles in radian) ITA_BASE_API void calculateYPRAnglesFromQuaternion(VistaQuaternion q, double& dRoll, double& dPitch, double& dYaw); //! Datenklasse für Fehlerwerte template< typename T > class ITA_BASE_API ErrorValues ... ...
 ... ... @@ -6,6 +6,10 @@ #include #include #include #include #include using namespace ITAConstants; int getExp2( const unsigned int x ) ... ... @@ -426,6 +430,32 @@ void convertVU2YPR( const double vx, const double vy, const double vz, const dou */ } void calculateYPRAnglesFromQuaternion(VistaQuaternion q, double& dRoll, double& dPitch, double& dYaw) { //Quaternion: [qx, qy, qz, qw] as described/defined in VistaQuaternion // [0, 1, 2, 3] //Taken from http://www.tu-berlin.de/fileadmin/fg169/miscellaneous/Quaternions.pdf //First convert Quaternion to a DCM, then transform DCM to euler double a21 = 2 * (q[0] * q[1] + q[2] * q[3]);; double a22 = pow(q[3], 2) - pow(q[0], 2) + pow(q[1], 2) - pow(q[2], 2); double a13 = 2 * (q[0] * q[2] + q[1] * q[3]); double a33 = pow(q[3], 2) - pow(q[0], 2) - pow(q[1], 2) + pow(q[2], 2); double a23 = 2 * (q[1] * q[2] - q[0] * q[3]); //Calculation is based on Roll around z; Yaw around negative x; Pitch around y dRoll = atan2(-a21, a22); dYaw = -atan2(-a13, a33); dPitch = asin(-a23); } void pow2space( std::vector& dest, const int a, const int b ) { int xa = getExp2( a ); ... ...
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment