lib/main/MAVLink/mavlink_conversions.h

   1 #ifndef  _MAVLINK_CONVERSIONS_H_
   2 #define  _MAVLINK_CONVERSIONS_H_
   3
   4 /* enable math defines on Windows */
   5 #ifdef _MSC_VER
   6 #ifndef _USE_MATH_DEFINES
   7 #define _USE_MATH_DEFINES
   8 #endif
   9 #endif
  10 #include <math.h>
  11
  12 #ifndef M_PI_2
  13     #define M_PI_2 ((float)asin(1))
  14 #endif
  15
  16 /**
  17  * @file mavlink_conversions.h
  18  *
  19  * These conversion functions follow the NASA rotation standards definition file
  20  * available online.
  21  *
  22  * Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
  23  * (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
  24  * rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
  25  * protocol as widely as possible.
  26  *
  27  * @author James Goppert
  28  * @author Thomas Gubler <thomasgubler@gmail.com>
  29  */
  30
  31
  32 /**
  33  * Converts a quaternion to a rotation matrix
  34  *
  35  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  36  * @param dcm a 3x3 rotation matrix
  37  */
  38 MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
  39 {
  40     double a = quaternion[0];
  41     double b = quaternion[1];
  42     double c = quaternion[2];
  43     double d = quaternion[3];
  44     double aSq = a * a;
  45     double bSq = b * b;
  46     double cSq = c * c;
  47     double dSq = d * d;
  48     dcm[0][0] = aSq + bSq - cSq - dSq;
  49     dcm[0][1] = 2 * (b * c - a * d);
  50     dcm[0][2] = 2 * (a * c + b * d);
  51     dcm[1][0] = 2 * (b * c + a * d);
  52     dcm[1][1] = aSq - bSq + cSq - dSq;
  53     dcm[1][2] = 2 * (c * d - a * b);
  54     dcm[2][0] = 2 * (b * d - a * c);
  55     dcm[2][1] = 2 * (a * b + c * d);
  56     dcm[2][2] = aSq - bSq - cSq + dSq;
  57 }
  58
  59
  60 /**
  61  * Converts a rotation matrix to euler angles
  62  *
  63  * @param dcm a 3x3 rotation matrix
  64  * @param roll the roll angle in radians
  65  * @param pitch the pitch angle in radians
  66  * @param yaw the yaw angle in radians
  67  */
  68 MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
  69 {
  70     float phi, theta, psi;
  71     theta = asinf(-dcm[2][0]);
  72
  73     if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
  74         phi = 0.0f;
  75         psi = (atan2f(dcm[1][2] - dcm[0][1],
  76                 dcm[0][2] + dcm[1][1]) + phi);
  77
  78     } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
  79         phi = 0.0f;
  80         psi = atan2f(dcm[1][2] - dcm[0][1],
  81                   dcm[0][2] + dcm[1][1] - phi);
  82
  83     } else {
  84         phi = atan2f(dcm[2][1], dcm[2][2]);
  85         psi = atan2f(dcm[1][0], dcm[0][0]);
  86     }
  87
  88     *roll = phi;
  89     *pitch = theta;
  90     *yaw = psi;
  91 }
  92
  93
  94 /**
  95  * Converts a quaternion to euler angles
  96  *
  97  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
  98  * @param roll the roll angle in radians
  99  * @param pitch the pitch angle in radians
 100  * @param yaw the yaw angle in radians
 101  */
 102 MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
 103 {
 104     float dcm[3][3];
 105     mavlink_quaternion_to_dcm(quaternion, dcm);
 106     mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
 107 }
 108
 109
 110 /**
 111  * Converts euler angles to a quaternion
 112  *
 113  * @param roll the roll angle in radians
 114  * @param pitch the pitch angle in radians
 115  * @param yaw the yaw angle in radians
 116  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
 117  */
 118 MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
 119 {
 120     float cosPhi_2 = cos_approx(roll / 2);
 121     float sinPhi_2 = sin_approx(roll / 2);
 122     float cosTheta_2 = cos_approx(pitch / 2);
 123     float sinTheta_2 = sin_approx(pitch / 2);
 124     float cosPsi_2 = cos_approx(yaw / 2);
 125     float sinPsi_2 = sin_approx(yaw / 2);
 126     quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
 127             sinPhi_2 * sinTheta_2 * sinPsi_2);
 128     quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
 129             cosPhi_2 * sinTheta_2 * sinPsi_2);
 130     quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
 131             sinPhi_2 * cosTheta_2 * sinPsi_2);
 132     quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
 133             sinPhi_2 * sinTheta_2 * cosPsi_2);
 134 }
 135
 136
 137 /**
 138  * Converts a rotation matrix to a quaternion
 139  * Reference:
 140  *  - Shoemake, Quaternions,
 141  *  http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
 142  *
 143  * @param dcm a 3x3 rotation matrix
 144  * @param quaternion a [w, x, y, z] ordered quaternion (null-rotation being 1 0 0 0)
 145  */
 146 MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
 147 {
 148     float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
 149     if (tr > 0.0f) {
 150         float s = sqrtf(tr + 1.0f);
 151         quaternion[0] = s * 0.5f;
 152         s = 0.5f / s;
 153         quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
 154         quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
 155         quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
 156     } else {
 157         /* Find maximum diagonal element in dcm
 158          * store index in dcm_i */
 159         int dcm_i = 0;
 160         int i;
 161         for (i = 1; i < 3; i++) {
 162             if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
 163                 dcm_i = i;
 164             }
 165         }
 166
 167         int dcm_j = (dcm_i + 1) % 3;
 168         int dcm_k = (dcm_i + 2) % 3;
 169
 170         float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
 171                     dcm[dcm_k][dcm_k]) + 1.0f);
 172         quaternion[dcm_i + 1] = s * 0.5f;
 173         s = 0.5f / s;
 174         quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
 175         quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
 176         quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
 177     }
 178 }
 179
 180
 181 /**
 182  * Converts euler angles to a rotation matrix
 183  *
 184  * @param roll the roll angle in radians
 185  * @param pitch the pitch angle in radians
 186  * @param yaw the yaw angle in radians
 187  * @param dcm a 3x3 rotation matrix
 188  */
 189 MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
 190 {
 191     float cosPhi = cos_approx(roll);
 192     float sinPhi = sin_approx(roll);
 193     float cosThe = cos_approx(pitch);
 194     float sinThe = sin_approx(pitch);
 195     float cosPsi = cos_approx(yaw);
 196     float sinPsi = sin_approx(yaw);
 197
 198     dcm[0][0] = cosThe * cosPsi;
 199     dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
 200     dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
 201
 202     dcm[1][0] = cosThe * sinPsi;
 203     dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
 204     dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
 205
 206     dcm[2][0] = -sinThe;
 207     dcm[2][1] = sinPhi * cosThe;
 208     dcm[2][2] = cosPhi * cosThe;
 209 }
 210
 211 #endif