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
|
#ifndef _MAVLINK_CONVERSIONS_H_
#define _MAVLINK_CONVERSIONS_H_
#include <math.h>
/**
* @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
*/
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.0 * (b * c - a * d);
dcm[0][2] = 2.0 * (a * c + b * d);
dcm[1][0] = 2.0 * (b * c + a * d);
dcm[1][1] = aSq - bSq + cSq - dSq;
dcm[1][2] = 2.0 * (c * d - a * b);
dcm[2][0] = 2.0 * (b * d - a * c);
dcm[2][1] = 2.0 * (a * b + c * d);
dcm[2][2] = aSq - bSq - cSq + dSq;
}
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 (fabs(theta - M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi);
} else if (fabs(theta + 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;
}
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(dcm, roll, pitch, yaw);
}
MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
{
double cosPhi_2 = cos((double)roll / 2.0);
double sinPhi_2 = sin((double)roll / 2.0);
double cosTheta_2 = cos((double)pitch / 2.0);
double sinTheta_2 = sin((double)pitch / 2.0);
double cosPsi_2 = cos((double)yaw / 2.0);
double sinPsi_2 = sin((double)yaw / 2.0);
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);
}
MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
{
quaternion[0] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] + dcm[1][1] + dcm[2][2])));
quaternion[1] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] - dcm[1][1] - dcm[2][2])));
quaternion[2] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] + dcm[1][1] - dcm[2][2])));
quaternion[3] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] - dcm[1][1] + dcm[2][2])));
}
MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
{
double cosPhi = cos(roll);
double sinPhi = sin(roll);
double cosThe = cos(pitch);
double sinThe = sin(pitch);
double cosPsi = cos(yaw);
double sinPsi = sin(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
|
