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