1 /* 2 * Copyright (c) 2015, Freescale Semiconductor, Inc. 3 * Copyright 2016-2017 NXP 4 * All rights reserved. 5 * 6 * SPDX-License-Identifier: BSD-3-Clause 7 */ 8 9 /*! \file orientation.h 10 \brief Functions to convert between various orientation representations 11 12 Functions to convert between various orientation representations. Also 13 includes functions for manipulating quaternions. 14 */ 15 16 #ifndef ORIENTATION_H 17 #define ORIENTATION_H 18 19 /// quaternion structure definition 20 typedef struct Quaternion 21 { 22 float q0; ///< scalar component 23 float q1; ///< x vector component 24 float q2; ///< y vector component 25 float q3; ///< z vector component 26 } Quaternion; 27 28 // function prototypes 29 /// Aerospace NED accelerometer 3DOF tilt function, computing rotation matrix fR 30 void f3DOFTiltNED( 31 float fR[][3], ///< computed rotation matrix (output) 32 float fGc[] ///< calibrated accelerometer input vector 33 ); 34 /// Android accelerometer 3DOF tilt function computing, rotation matrix fR 35 void f3DOFTiltAndroid( 36 float fR[][3], ///< computed rotation matrix (output) 37 float fGc[] ///< calibrated accelerometer input vector 38 ); 39 /// Windows 8 accelerometer 3DOF tilt function computing, rotation matrix fR 40 void f3DOFTiltWin8( 41 float fR[][3], ///< computed rotation matrix (output) 42 float fGc[] ///< calibrated accelerometer input vector 43 ); 44 /// Aerospace NED magnetometer 3DOF flat eCompass function, computing rotation matrix fR 45 void f3DOFMagnetometerMatrixNED( 46 float fR[][3], ///< computed rotation matrix (output) 47 float fBc[] ///< calibrated magnetometer reading (input) 48 ); 49 /// Android magnetometer 3DOF flat eCompass function, computing rotation matrix fR 50 void f3DOFMagnetometerMatrixAndroid( 51 float fR[][3], ///< computed rotation matrix (output) 52 float fBc[] ///< calibrated magnetometer reading (input) 53 ); 54 /// Windows 8 magnetometer 3DOF flat eCompass function, computing rotation matrix fR 55 void f3DOFMagnetometerMatrixWin8( 56 float fR[][3], ///< computed rotation matrix (output) 57 float fBc[] ///< calibrated magnetometer reading (input) 58 ); 59 /// NED: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta 60 void feCompassNED( 61 float fR[][3], ///< computed rotation matrix (output) 62 float *pfDelta, ///< magnetic inclination angle (output) 63 float *pfsinDelta, ///< sin of the inclination angle 64 float *pfcosDelta, ///< cos of the inclination angle 65 float fBc[], ///< calibrated magnetometer vector (input) 66 float fGc[], ///< calibrated accelerometer input vector (input) 67 float *pfmodBc, ///< modulus of the calibrated magnetic vector 68 float *pfmodGc ///< modulus of the calibrated accelerometer vector 69 ); 70 /// Android: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta 71 void feCompassAndroid( 72 float fR[][3], ///< computed rotation matrix (output) 73 float *pfDelta, ///< magnetic inclination angle (output) 74 float *pfsinDelta, ///< sin of the inclination angle 75 float *pfcosDelta, ///< cos of the inclination angle 76 float fBc[], ///< calibrated magnetometer reading (input) 77 float fGc[], ///< calibrated accelerometer input vector (input) 78 float *pfmodBc, ///< modulus of the calibrated magnetic vector 79 float *pfmodGc ///< modulus of the calibrated accelerometer vector 80 ); 81 /// Win8: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta 82 void feCompassWin8( 83 float fR[][3], ///< computed rotation matrix (output) 84 float *pfDelta, ///< magnetic inclination angle (output) 85 float *pfsinDelta, ///< sin of the inclination angle 86 float *pfcosDelta, ///< cos of the inclination angle 87 float fBc[], ///< calibrated magnetometer reading (input) 88 float fGc[], ///< calibrated accelerometer input vector (input) 89 float *pfmodBc, ///< modulus of the calibrated magnetic vector 90 float *pfmodGc ///< modulus of the calibrated accelerometer vector 91 ); 92 /// extract the NED angles in degrees from the NED rotation matrix 93 void fNEDAnglesDegFromRotationMatrix( 94 float R[][3], ///< rotation matrix input 95 float *pfPhiDeg, ///< output: the roll angle range -180.0 <= Phi < 180.0 deg 96 float *pfTheDeg, ///< output: the pitch angle -90.0 <= Theta <= 90.0 deg 97 float *pfPsiDeg, ///< output: the yaw (compass) angle 0.0 <= Psi < 360.0 deg 98 float *pfRhoDeg, ///< output: For NED, the compass heading Rho equals the yaw angle Psi 99 float *pfChiDeg ///< output: the tilt angle from vertical Chi (0 <= Chi <= 180 deg) 100 ); 101 /// extract the Android angles in degrees from the Android rotation matrix 102 void fAndroidAnglesDegFromRotationMatrix( 103 float R[][3], ///< rotation matrix input 104 float *pfPhiDeg, ///< the roll angle -90.0 <= Phi <= 90.0 deg 105 float *pfTheDeg, ///< the pitch angle -180.0 <= The < 180.0 deg 106 float *pfPsiDeg, ///< yaw angle Psi with range 0.0 <= Psi < 360.0 deg 107 float *pfRhoDeg, ///< the compass heading angle Rho equals the yaw angle Psi 108 float *pfChiDeg ///< the tilt angle from vertical Chi (0 <= Chi <= 180 deg) 109 ); 110 /// extract the Windows 8 angles in degrees from the Windows 8 rotation matrix 111 void fWin8AnglesDegFromRotationMatrix( 112 float R[][3], ///< rotation matrix input 113 float *pfPhiDeg, ///< the roll angle -90.0 <= Phi <= 90.0 deg 114 float *pfTheDeg, ///< pitch angle Theta in the range -180.0 <= The < 180.0 deg 115 float *pfPsiDeg, ///< yaw angle Psi in range 0.0 <= Psi < 360.0 deg 116 float *pfRhoDeg, ///< the compass angle Rho = 360 - Psi 117 float *pfChiDeg ///< tilt angle from vertical Chi (0 <= Chi <= 180 deg) 118 ); 119 /// compute the orientation quaternion from a 3x3 rotation matrix 120 void fQuaternionFromRotationMatrix( 121 float R[][3], ///< Rotation matrix (input) 122 Quaternion *pq ///< Quaternion (output) 123 ); 124 /// compute the rotation matrix from an orientation quaternion 125 void fRotationMatrixFromQuaternion( 126 float R[][3], ///< Rotation matrix (output) 127 const Quaternion *pq ///< Quaternion (input) 128 ); 129 /// function compute the quaternion product qB * qC 130 void qAeqBxC( 131 Quaternion *pqA, 132 const Quaternion *pqB, 133 const Quaternion *pqC 134 ); 135 /// function compute the quaternion product qA = qA * qB 136 void qAeqAxB( 137 Quaternion *pqA, 138 const Quaternion *pqB 139 ); 140 /// function compute the quaternion product conjg(qA) * qB 141 Quaternion qconjgAxB( 142 const Quaternion *pqA, 143 const Quaternion *pqB 144 ); 145 /// function normalizes a rotation quaternion and ensures q0 is non-negative 146 void fqAeqNormqA( 147 Quaternion *pqA 148 ); 149 /// set a quaternion to the unit quaternion 150 void fqAeq1( 151 Quaternion *pqA 152 ); 153 /// computes normalized rotation quaternion from a rotation vector (deg) 154 void fQuaternionFromRotationVectorDeg( 155 Quaternion *pq, ///< quaternion (output) 156 const float rvecdeg[], ///< rotation vector in degrees 157 float fscaling ///< delta Time 158 ); 159 /// computes rotation vector (deg) from rotation quaternion 160 void fRotationVectorDegFromQuaternion( 161 Quaternion *pq, ///< quaternion (input) 162 float rvecdeg[] ///< rotation vector in degrees (output) 163 ); 164 /// function low pass filters an orientation quaternion and computes virtual gyro rotation rate 165 void fLPFOrientationQuaternion( 166 Quaternion *pq, 167 Quaternion *pLPq, 168 float flpf, 169 float fdeltat, 170 float fOmega[] 171 ); 172 /// function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q 173 /// using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)} 174 void fveqconjgquq( 175 Quaternion *pfq, 176 float fu[], 177 float fv[] 178 ); 179 180 #endif // #ifndef ORIENTATION_H 181
