source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/include/mavlink_conversions.h @ 77

Last change on this file since 77 was 75, checked in by Thomas Fuhrmann, 5 years ago

Change the version of mavlink generated messages and rename it to include

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.