[46] | 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 |
---|