Emaxx Navigation Group / mavlink_emaxx

Mavlink Messages for Emaxx Nav Board

Dependents:   Madpulse_Speed_Control_temp Madpulse_Speed_Control_Students

mavlink_conversions.h@1:7e9af9a921f7, 2017-01-20 (annotated)

Committer:
jdawkins
Date:
Fri Jan 20 13:27:11 2017 +0000
Revision:
1:7e9af9a921f7
Parent:
0:bb2cacd02294 
Fixed Asin error in mavlink_conversions.h

Who changed what in which revision?

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