mavlink_conversions.h

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 
00016 /**
00017  * @file mavlink_conversions.h
00018  *
00019  * These conversion functions follow the NASA rotation standards definition file
00020  * available online.
00021  *
00022  * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
00023  * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
00024  * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
00025  * protocol as widely as possible.
00026  *
00027  * @author James Goppert
00028  * @author Thomas Gubler <thomasgubler@gmail.com>
00029  */
00030 
00031 
00032 /**
00033  * Converts a quaternion to a rotation matrix
00034  *
00035  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
00036  * @param dcm a 3x3 rotation matrix
00037  */
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 
00060 /**
00061  * Converts a rotation matrix to euler angles
00062  *
00063  * @param dcm a 3x3 rotation matrix
00064  * @param roll the roll angle in radians
00065  * @param pitch the pitch angle in radians
00066  * @param yaw the yaw angle in radians
00067  */
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 
00094 /**
00095  * Converts a quaternion to euler angles
00096  *
00097  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
00098  * @param roll the roll angle in radians
00099  * @param pitch the pitch angle in radians
00100  * @param yaw the yaw angle in radians
00101  */
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 
00110 /**
00111  * Converts euler angles to a quaternion
00112  *
00113  * @param roll the roll angle in radians
00114  * @param pitch the pitch angle in radians
00115  * @param yaw the yaw angle in radians
00116  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
00117  */
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 
00137 /**
00138  * Converts a rotation matrix to a quaternion
00139  * Reference:
00140  *  - Shoemake, Quaternions,
00141  *  http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
00142  *
00143  * @param dcm a 3x3 rotation matrix
00144  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
00145  */
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 
00181 /**
00182  * Converts euler angles to a rotation matrix
00183  *
00184  * @param roll the roll angle in radians
00185  * @param pitch the pitch angle in radians
00186  * @param yaw the yaw angle in radians
00187  * @param dcm a 3x3 rotation matrix
00188  */
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
00212