mavlink_conversions.h
Go to the documentation of this file.
00001 #ifndef  _MAVLINK_CONVERSIONS_H_
00002 #define  _MAVLINK_CONVERSIONS_H_
00003 
00004 /* enable math defines on Windows */
00005 #ifdef _MSC_VER
00006 #ifndef _USE_MATH_DEFINES
00007 #define _USE_MATH_DEFINES
00008 #endif
00009 #endif
00010 #include <math.h>
00011 
00012 #ifndef M_PI_2
00013     #define M_PI_2 ((float)asin(1))
00014 #endif
00015 
00038 MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
00039 {
00040     double a = quaternion[0];
00041     double b = quaternion[1];
00042     double c = quaternion[2];
00043     double d = quaternion[3];
00044     double aSq = a * a;
00045     double bSq = b * b;
00046     double cSq = c * c;
00047     double dSq = d * d;
00048     dcm[0][0] = aSq + bSq - cSq - dSq;
00049     dcm[0][1] = 2 * (b * c - a * d);
00050     dcm[0][2] = 2 * (a * c + b * d);
00051     dcm[1][0] = 2 * (b * c + a * d);
00052     dcm[1][1] = aSq - bSq + cSq - dSq;
00053     dcm[1][2] = 2 * (c * d - a * b);
00054     dcm[2][0] = 2 * (b * d - a * c);
00055     dcm[2][1] = 2 * (a * b + c * d);
00056     dcm[2][2] = aSq - bSq - cSq + dSq;
00057 }
00058 
00059 
00068 MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
00069 {
00070     float phi, theta, psi;
00071     theta = asin(-dcm[2][0]);
00072 
00073     if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
00074         phi = 0.0f;
00075         psi = (atan2f(dcm[1][2] - dcm[0][1],
00076                 dcm[0][2] + dcm[1][1]) + phi);
00077 
00078     } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
00079         phi = 0.0f;
00080         psi = atan2f(dcm[1][2] - dcm[0][1],
00081                   dcm[0][2] + dcm[1][1] - phi);
00082 
00083     } else {
00084         phi = atan2f(dcm[2][1], dcm[2][2]);
00085         psi = atan2f(dcm[1][0], dcm[0][0]);
00086     }
00087 
00088     *roll = phi;
00089     *pitch = theta;
00090     *yaw = psi;
00091 }
00092 
00093 
00102 MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
00103 {
00104     float dcm[3][3];
00105     mavlink_quaternion_to_dcm(quaternion, dcm);
00106     mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
00107 }
00108 
00109 
00118 MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
00119 {
00120     float cosPhi_2 = cosf(roll / 2);
00121     float sinPhi_2 = sinf(roll / 2);
00122     float cosTheta_2 = cosf(pitch / 2);
00123     float sinTheta_2 = sinf(pitch / 2);
00124     float cosPsi_2 = cosf(yaw / 2);
00125     float sinPsi_2 = sinf(yaw / 2);
00126     quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
00127             sinPhi_2 * sinTheta_2 * sinPsi_2);
00128     quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
00129             cosPhi_2 * sinTheta_2 * sinPsi_2);
00130     quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
00131             sinPhi_2 * cosTheta_2 * sinPsi_2);
00132     quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
00133             sinPhi_2 * sinTheta_2 * cosPsi_2);
00134 }
00135 
00136 
00146 MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
00147 {
00148     float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
00149     if (tr > 0.0f) {
00150         float s = sqrtf(tr + 1.0f);
00151         quaternion[0] = s * 0.5f;
00152         s = 0.5f / s;
00153         quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
00154         quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
00155         quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
00156     } else {
00157         /* Find maximum diagonal element in dcm
00158          * store index in dcm_i */
00159         int dcm_i = 0;
00160         int i;
00161         for (i = 1; i < 3; i++) {
00162             if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
00163                 dcm_i = i;
00164             }
00165         }
00166 
00167         int dcm_j = (dcm_i + 1) % 3;
00168         int dcm_k = (dcm_i + 2) % 3;
00169 
00170         float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
00171                     dcm[dcm_k][dcm_k]) + 1.0f);
00172         quaternion[dcm_i + 1] = s * 0.5f;
00173         s = 0.5f / s;
00174         quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
00175         quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
00176         quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
00177     }
00178 }
00179 
00180 
00189 MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
00190 {
00191     float cosPhi = cosf(roll);
00192     float sinPhi = sinf(roll);
00193     float cosThe = cosf(pitch);
00194     float sinThe = sinf(pitch);
00195     float cosPsi = cosf(yaw);
00196     float sinPsi = sinf(yaw);
00197 
00198     dcm[0][0] = cosThe * cosPsi;
00199     dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
00200     dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
00201 
00202     dcm[1][0] = cosThe * sinPsi;
00203     dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
00204     dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
00205 
00206     dcm[2][0] = -sinThe;
00207     dcm[2][1] = sinPhi * cosThe;
00208     dcm[2][2] = cosPhi * cosThe;
00209 }
00210 
00211 #endif


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43