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


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