lib/main/MAVLink/mavlink_conversions.h

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