[88] | 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 = asin(-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 = cosf(roll / 2);
|
---|
| 121 | float sinPhi_2 = sinf(roll / 2);
|
---|
| 122 | float cosTheta_2 = cosf(pitch / 2);
|
---|
| 123 | float sinTheta_2 = sinf(pitch / 2);
|
---|
| 124 | float cosPsi_2 = cosf(yaw / 2);
|
---|
| 125 | float sinPsi_2 = sinf(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 = cosf(roll);
|
---|
| 192 | float sinPhi = sinf(roll);
|
---|
| 193 | float cosThe = cosf(pitch);
|
---|
| 194 | float sinThe = sinf(pitch);
|
---|
| 195 | float cosPsi = cosf(yaw);
|
---|
| 196 | float sinPsi = sinf(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
|
---|