include_v2.0/mavlink_conversions.h
Go to the documentation of this file.
1 #pragma once
2 
3 #ifndef MAVLINK_NO_CONVERSION_HELPERS
4 
5 /* enable math defines on Windows */
6 #ifdef _MSC_VER
7 #ifndef _USE_MATH_DEFINES
8 #define _USE_MATH_DEFINES
9 #endif
10 #endif
11 #include <math.h>
12 
13 #ifndef M_PI_2
14  #define M_PI_2 ((float)asin(1))
15 #endif
16 
39 MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
40 {
41  double a = quaternion[0];
42  double b = quaternion[1];
43  double c = quaternion[2];
44  double d = quaternion[3];
45  double aSq = a * a;
46  double bSq = b * b;
47  double cSq = c * c;
48  double dSq = d * d;
49  dcm[0][0] = aSq + bSq - cSq - dSq;
50  dcm[0][1] = 2 * (b * c - a * d);
51  dcm[0][2] = 2 * (a * c + b * d);
52  dcm[1][0] = 2 * (b * c + a * d);
53  dcm[1][1] = aSq - bSq + cSq - dSq;
54  dcm[1][2] = 2 * (c * d - a * b);
55  dcm[2][0] = 2 * (b * d - a * c);
56  dcm[2][1] = 2 * (a * b + c * d);
57  dcm[2][2] = aSq - bSq - cSq + dSq;
58 }
59 
60 
69 MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
70 {
71  float phi, theta, psi;
72  theta = asinf(-dcm[2][0]);
73 
74  if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
75  phi = 0.0f;
76  psi = (atan2f(dcm[1][2] - dcm[0][1],
77  dcm[0][2] + dcm[1][1]) + phi);
78 
79  } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
80  phi = 0.0f;
81  psi = atan2f(dcm[1][2] - dcm[0][1],
82  dcm[0][2] + dcm[1][1] - phi);
83 
84  } else {
85  phi = atan2f(dcm[2][1], dcm[2][2]);
86  psi = atan2f(dcm[1][0], dcm[0][0]);
87  }
88 
89  *roll = phi;
90  *pitch = theta;
91  *yaw = psi;
92 }
93 
94 
103 MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
104 {
105  float dcm[3][3];
106  mavlink_quaternion_to_dcm(quaternion, dcm);
107  mavlink_dcm_to_euler((const float(*)[3])dcm, roll, pitch, yaw);
108 }
109 
110 
119 MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
120 {
121  float cosPhi_2 = cosf(roll / 2);
122  float sinPhi_2 = sinf(roll / 2);
123  float cosTheta_2 = cosf(pitch / 2);
124  float sinTheta_2 = sinf(pitch / 2);
125  float cosPsi_2 = cosf(yaw / 2);
126  float sinPsi_2 = sinf(yaw / 2);
127  quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
128  sinPhi_2 * sinTheta_2 * sinPsi_2);
129  quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
130  cosPhi_2 * sinTheta_2 * sinPsi_2);
131  quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
132  sinPhi_2 * cosTheta_2 * sinPsi_2);
133  quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
134  sinPhi_2 * sinTheta_2 * cosPsi_2);
135 }
136 
137 
147 MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
148 {
149  float tr = dcm[0][0] + dcm[1][1] + dcm[2][2];
150  if (tr > 0.0f) {
151  float s = sqrtf(tr + 1.0f);
152  quaternion[0] = s * 0.5f;
153  s = 0.5f / s;
154  quaternion[1] = (dcm[2][1] - dcm[1][2]) * s;
155  quaternion[2] = (dcm[0][2] - dcm[2][0]) * s;
156  quaternion[3] = (dcm[1][0] - dcm[0][1]) * s;
157  } else {
158  /* Find maximum diagonal element in dcm
159  * store index in dcm_i */
160  int dcm_i = 0;
161  int i;
162  for (i = 1; i < 3; i++) {
163  if (dcm[i][i] > dcm[dcm_i][dcm_i]) {
164  dcm_i = i;
165  }
166  }
167 
168  int dcm_j = (dcm_i + 1) % 3;
169  int dcm_k = (dcm_i + 2) % 3;
170 
171  float s = sqrtf((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
172  dcm[dcm_k][dcm_k]) + 1.0f);
173  quaternion[dcm_i + 1] = s * 0.5f;
174  s = 0.5f / s;
175  quaternion[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s;
176  quaternion[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s;
177  quaternion[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s;
178  }
179 }
180 
181 
190 MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
191 {
192  float cosPhi = cosf(roll);
193  float sinPhi = sinf(roll);
194  float cosThe = cosf(pitch);
195  float sinThe = sinf(pitch);
196  float cosPsi = cosf(yaw);
197  float sinPsi = sinf(yaw);
198 
199  dcm[0][0] = cosThe * cosPsi;
200  dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
201  dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
202 
203  dcm[1][0] = cosThe * sinPsi;
204  dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
205  dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
206 
207  dcm[2][0] = -sinThe;
208  dcm[2][1] = sinPhi * cosThe;
209  dcm[2][2] = cosPhi * cosThe;
210 }
211 
212 #endif // MAVLINK_NO_CONVERSION_HELPERS


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07