pinxian wu / mavlink_bridge

mavlink library

Dependents:   mavlink F429ZI_LCD_demo

Fork of mavlink_bridge by Benjamin Hepp

mavlink/v10/mavlink_conversions.h@0:28183cc7963f, 2015-11-24 (annotated)

Committer:
bhepp
Date:
Tue Nov 24 16:41:11 2015 +0000
Revision:
0:28183cc7963f
Child:
1:9e036a7ba2a5
Mavlink bridge for Mbed

Who changed what in which revision?

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