mavlink_conversions.h
Go to the documentation of this file.
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 
38 MAVLINK_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 
68 MAVLINK_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 
102 MAVLINK_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 
118 MAVLINK_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 
146 MAVLINK_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 
189 MAVLINK_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
d
XmlRpcServer s
float asin(float x)
Definition: turbomath.cpp:449


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Apr 15 2021 05:07:46