source: flair-src/trunk/tools/Controller/Mavlink/src/include/mavlink_conversions.h@ 172

Last change on this file since 172 was 88, checked in by Sanahuja Guillaume, 8 years ago

m
File size: 6.2 KB
Line 
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 */
38MAVLINK_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 */
68MAVLINK_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 */
102MAVLINK_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 */
118MAVLINK_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 */
146MAVLINK_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 */
189MAVLINK_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
Note: See TracBrowser for help on using the repository browser.