dRonin  adbada4
dRonin firmware
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
coordinate_conversions.c
Go to the documentation of this file.
1 
20 /*
21  * This program is free software; you can redistribute it and/or modify
22  * it under the terms of the GNU General Public License as published by
23  * the Free Software Foundation; either version 3 of the License, or
24  * (at your option) any later version.
25  *
26  * This program is distributed in the hope that it will be useful, but
27  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
28  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
29  * for more details.
30  *
31  * You should have received a copy of the GNU General Public License along
32  * with this program; if not, see <http://www.gnu.org/licenses/>
33  */
34 
35 #include <math.h>
36 #include <stdint.h>
37 #include "coordinate_conversions.h"
38 #include "physical_constants.h"
39 
40 // ****** find ECEF to NED rotation matrix ********
41 void RneFromLLA(float LLA[3], float Rne[3][3])
42 {
43  float sinLat, sinLon, cosLat, cosLon;
44 
45  sinLat = (float)sinf(DEG2RAD * LLA[0]);
46  sinLon = (float)sinf(DEG2RAD * LLA[1]);
47  cosLat = (float)cosf(DEG2RAD * LLA[0]);
48  cosLon = (float)cosf(DEG2RAD * LLA[1]);
49 
50  Rne[0][0] = -sinLat * cosLon;
51  Rne[0][1] = -sinLat * sinLon;
52  Rne[0][2] = cosLat;
53  Rne[1][0] = -sinLon;
54  Rne[1][1] = cosLon;
55  Rne[1][2] = 0;
56  Rne[2][0] = -cosLat * cosLon;
57  Rne[2][1] = -cosLat * sinLon;
58  Rne[2][2] = -sinLat;
59 }
60 
61 // ****** find roll, pitch, yaw from quaternion ********
62 void Quaternion2RPY(const float q[4], float rpy[3])
63 {
64  float R13, R11, R12, R23, R33;
65  float q0s = q[0] * q[0];
66  float q1s = q[1] * q[1];
67  float q2s = q[2] * q[2];
68  float q3s = q[3] * q[3];
69 
70  R13 = 2.0f * (q[1] * q[3] - q[0] * q[2]);
71  R11 = q0s + q1s - q2s - q3s;
72  R12 = 2.0f * (q[1] * q[2] + q[0] * q[3]);
73  R23 = 2.0f * (q[2] * q[3] + q[0] * q[1]);
74  R33 = q0s - q1s - q2s + q3s;
75 
76  /* If we're nearly nose-up or nose-down, consider yaw to be 0, encode
77  * information in roll to deal with loss of freedom
78  */
79  if (R13 >= 0.985f) {
80  if (R13 >= 1) {
81  R13 = 1;
82  }
83 
84  rpy[2] = 0;
85  rpy[1] = RAD2DEG * asinf(-R13);
86  rpy[0] = 2 * RAD2DEG * atan2f(q[3], q[0]);
87 
88  return;
89  } else if (R13 <= -0.985f) {
90  if (R13 <= -1) {
91  R13 = -1;
92  }
93 
94  rpy[2] = 0;
95  rpy[1] = RAD2DEG * asinf(-R13);
96  rpy[0] = -2 * RAD2DEG * atan2f(q[3], q[0]);
97 
98 
99  return;
100  }
101 
102  rpy[1] = RAD2DEG * asinf(-R13); // pitch always between -pi/2 to pi/2
103  rpy[2] = RAD2DEG * atan2f(R12, R11);
104  rpy[0] = RAD2DEG * atan2f(R23, R33);
105 }
106 
107 // ****** find quaternion from roll, pitch, yaw ********
108 void RPY2Quaternion(const float rpy[3], float q[4])
109 {
110  float phi, theta, psi;
111  float cphi, sphi, ctheta, stheta, cpsi, spsi;
112 
113  phi = DEG2RAD * rpy[0] / 2;
114  theta = DEG2RAD * rpy[1] / 2;
115  psi = DEG2RAD * rpy[2] / 2;
116  cphi = cosf(phi);
117  sphi = sinf(phi);
118  ctheta = cosf(theta);
119  stheta = sinf(theta);
120  cpsi = cosf(psi);
121  spsi = sinf(psi);
122 
123  q[0] = cphi * ctheta * cpsi + sphi * stheta * spsi;
124  q[1] = sphi * ctheta * cpsi - cphi * stheta * spsi;
125  q[2] = cphi * stheta * cpsi + sphi * ctheta * spsi;
126  q[3] = cphi * ctheta * spsi - sphi * stheta * cpsi;
127 
128  if (q[0] < 0) { // q0 always positive for uniqueness
129  q[0] = -q[0];
130  q[1] = -q[1];
131  q[2] = -q[2];
132  q[3] = -q[3];
133  }
134 }
135 
136 //** Find Rbe, that rotates a vector from earth fixed to body frame, from quaternion **
137 void Quaternion2R(float q[4], float Rbe[3][3])
138 {
139  float q0s = q[0] * q[0], q1s = q[1] * q[1], q2s = q[2] * q[2], q3s = q[3] * q[3];
140 
141  Rbe[0][0] = q0s + q1s - q2s - q3s;
142  Rbe[0][1] = 2 * (q[1] * q[2] + q[0] * q[3]);
143  Rbe[0][2] = 2 * (q[1] * q[3] - q[0] * q[2]);
144  Rbe[1][0] = 2 * (q[1] * q[2] - q[0] * q[3]);
145  Rbe[1][1] = q0s - q1s + q2s - q3s;
146  Rbe[1][2] = 2 * (q[2] * q[3] + q[0] * q[1]);
147  Rbe[2][0] = 2 * (q[1] * q[3] + q[0] * q[2]);
148  Rbe[2][1] = 2 * (q[2] * q[3] - q[0] * q[1]);
149  Rbe[2][2] = q0s - q1s - q2s + q3s;
150 }
151 
152 //** Find Rbe, that rotates a vector from earth fixed to body frame, from euler angles, using Tait-Bryan
153 //** convention, i.e. Rbe=Rot_x(roll)*Rot_y(pitch)*Rot_z(yaw) **
154 void Euler2R(float rpy[3], float Rbe[3][3])
155 {
156  float sF = sinf(rpy[0]), cF = cosf(rpy[0]);
157  float sT = sinf(rpy[1]), cT = cosf(rpy[1]);
158  float sP = sinf(rpy[2]), cP = cosf(rpy[2]);
159 
160  Rbe[0][0] = cT*cP;
161  Rbe[0][1] = cT*sP;
162  Rbe[0][2] = -sT;
163  Rbe[1][0] = sF*sT*cP - cF*sP;
164  Rbe[1][1] = sF*sT*sP + cF*cP;
165  Rbe[1][2] = cT*sF;
166  Rbe[2][0] = cF*sT*cP + sF*sP;
167  Rbe[2][1] = cF*sT*sP - sF*cP;
168  Rbe[2][2] = cT*cF;
169 }
170 
171 // ****** convert Rotation Matrix to Quaternion ********
172 // ****** if R converts from e to b, q is rotation from e to b ****
173 void R2Quaternion(float R[3][3], float q[4])
174 {
175  float m[4], mag;
176  uint8_t index,i;
177 
178  m[0] = 1 + R[0][0] + R[1][1] + R[2][2];
179  m[1] = 1 + R[0][0] - R[1][1] - R[2][2];
180  m[2] = 1 - R[0][0] + R[1][1] - R[2][2];
181  m[3] = 1 - R[0][0] - R[1][1] + R[2][2];
182 
183  // find maximum divisor
184  index = 0;
185  mag = m[0];
186  for (i=1;i<4;i++){
187  if (m[i] > mag){
188  mag = m[i];
189  index = i;
190  }
191  }
192  mag = 2*sqrtf(mag);
193 
194  if (index == 0) {
195  q[0] = mag/4;
196  q[1] = (R[1][2]-R[2][1])/mag;
197  q[2] = (R[2][0]-R[0][2])/mag;
198  q[3] = (R[0][1]-R[1][0])/mag;
199  }
200  else if (index == 1) {
201  q[1] = mag/4;
202  q[0] = (R[1][2]-R[2][1])/mag;
203  q[2] = (R[0][1]+R[1][0])/mag;
204  q[3] = (R[0][2]+R[2][0])/mag;
205  }
206  else if (index == 2) {
207  q[2] = mag/4;
208  q[0] = (R[2][0]-R[0][2])/mag;
209  q[1] = (R[0][1]+R[1][0])/mag;
210  q[3] = (R[1][2]+R[2][1])/mag;
211  }
212  else {
213  q[3] = mag/4;
214  q[0] = (R[0][1]-R[1][0])/mag;
215  q[1] = (R[0][2]+R[2][0])/mag;
216  q[2] = (R[1][2]+R[2][1])/mag;
217  }
218 
219  // q0 positive, i.e. angle between pi and -pi
220  if (q[0] < 0){
221  q[0] = -q[0];
222  q[1] = -q[1];
223  q[2] = -q[2];
224  q[3] = -q[3];
225  }
226 }
227 
228 // ****** Rotation Matrix from Two Vector Directions ********
229 // ****** given two vector directions (v1 and v2) known in two frames (b and e) find Rbe ***
230 // ****** solution is approximate if can't be exact ***
231 uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
232 {
233  float Rib[3][3], Rie[3][3];
234  float mag;
235  uint8_t i,j,k;
236 
237  // identity rotation in case of error
238  for (i=0;i<3;i++){
239  for (j=0;j<3;j++)
240  Rbe[i][j]=0;
241  Rbe[i][i]=1;
242  }
243 
244  // The first rows of rot matrices chosen in direction of v1
245  mag = VectorMagnitude(v1b);
246  if (fabsf(mag) < 1e-30f)
247  return (-1);
248  for (i=0;i<3;i++)
249  Rib[0][i]=v1b[i]/mag;
250 
251  mag = VectorMagnitude(v1e);
252  if (fabsf(mag) < 1e-30f)
253  return (-1);
254  for (i=0;i<3;i++)
255  Rie[0][i]=v1e[i]/mag;
256 
257  // The second rows of rot matrices chosen in direction of v1xv2
258  CrossProduct(v1b,v2b,&Rib[1][0]);
259  mag = VectorMagnitude(&Rib[1][0]);
260  if (fabsf(mag) < 1e-30f)
261  return (-1);
262  for (i=0;i<3;i++)
263  Rib[1][i]=Rib[1][i]/mag;
264 
265  CrossProduct(v1e,v2e,&Rie[1][0]);
266  mag = VectorMagnitude(&Rie[1][0]);
267  if (fabsf(mag) < 1e-30f)
268  return (-1);
269  for (i=0;i<3;i++)
270  Rie[1][i]=Rie[1][i]/mag;
271 
272  // The third rows of rot matrices are XxY (Row1xRow2)
273  CrossProduct(&Rib[0][0],&Rib[1][0],&Rib[2][0]);
274  CrossProduct(&Rie[0][0],&Rie[1][0],&Rie[2][0]);
275 
276  // Rbe = Rbi*Rie = Rib'*Rie
277  for (i=0;i<3;i++)
278  for(j=0;j<3;j++){
279  Rbe[i][j]=0;
280  for(k=0;k<3;k++)
281  Rbe[i][j] += Rib[k][i]*Rie[k][j];
282  }
283 
284  return 1;
285 }
286 
287 void Rv2Rot(float Rv[3], float R[3][3])
288 {
289  // Compute rotation matrix from a rotation vector
290  // To save .text space, uses Quaternion2R()
291  float q[4];
292 
293  float angle = VectorMagnitude(Rv);
294  if (angle <= 0.00048828125f) {
295  // angle < sqrt(2*machine_epsilon(float)), so flush cos(x) to 1.0f
296  q[0] = 1.0f;
297 
298  // and flush sin(x/2)/x to 0.5
299  q[1] = 0.5f*Rv[0];
300  q[2] = 0.5f*Rv[1];
301  q[3] = 0.5f*Rv[2];
302  // This prevents division by zero, while retaining full accuracy
303  }
304  else {
305  q[0] = cosf(angle*0.5f);
306  float scale = sinf(angle*0.5f) / angle;
307  q[1] = scale*Rv[0];
308  q[2] = scale*Rv[1];
309  q[3] = scale*Rv[2];
310  }
311 
312  Quaternion2R(q, R);
313 }
314 
315 // ****** Vector Cross Product ********
316 void CrossProduct(const float v1[3], const float v2[3], float result[3])
317 {
318  result[0] = v1[1]*v2[2] - v2[1]*v1[2];
319  result[1] = v2[0]*v1[2] - v1[0]*v2[2];
320  result[2] = v1[0]*v2[1] - v2[0]*v1[1];
321 }
322 
323 // ****** Vector Magnitude ********
324 float VectorMagnitude(const float v[3])
325 {
326  return(sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]));
327 }
328 
333 void quat_inverse(float q[4])
334 {
335  q[1] = -q[1];
336  q[2] = -q[2];
337  q[3] = -q[3];
338 }
339 
345 void quat_copy(const float q[4], float qnew[4])
346 {
347  qnew[0] = q[0];
348  qnew[1] = q[1];
349  qnew[2] = q[2];
350  qnew[3] = q[3];
351 }
352 
359 void quat_mult(const float q1[4], const float q2[4], float qout[4])
360 {
361  qout[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3];
362  qout[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2];
363  qout[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1];
364  qout[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0];
365 }
366 
374 void rot_mult(float R[3][3], const float vec[3], float vec_out[3], bool transpose)
375 {
376  if (!transpose){
377  vec_out[0] = R[0][0] * vec[0] + R[0][1] * vec[1] + R[0][2] * vec[2];
378  vec_out[1] = R[1][0] * vec[0] + R[1][1] * vec[1] + R[1][2] * vec[2];
379  vec_out[2] = R[2][0] * vec[0] + R[2][1] * vec[1] + R[2][2] * vec[2];
380  }
381  else {
382  vec_out[0] = R[0][0] * vec[0] + R[1][0] * vec[1] + R[2][0] * vec[2];
383  vec_out[1] = R[0][1] * vec[0] + R[1][1] * vec[1] + R[2][1] * vec[2];
384  vec_out[2] = R[0][2] * vec[0] + R[1][2] * vec[1] + R[2][2] * vec[2];
385  }
386 }
387 
void rot_mult(float R[3][3], const float vec[3], float vec_out[3], bool transpose)
Rotate a vector by a rotation matrix.
void Rv2Rot(float Rv[3], float R[3][3])
struct _msp_pid_item mag
Definition: msp_messages.h:104
void Quaternion2R(float q[4], float Rbe[3][3])
void RPY2Quaternion(const float rpy[3], float q[4])
void quat_copy(const float q[4], float qnew[4])
Duplicate a quaternion.
float R[NUMV]
Definition: insgps14state.c:67
static float scale(float val, float inMin, float inMax, float outMin, float outMax)
Definition: txpid.c:444
volatile int j
Definition: loadabletest.c:12
float VectorMagnitude(const float v[3])
void quat_mult(const float q1[4], const float q2[4], float qout[4])
Multiply two quaternions into a third.
void RneFromLLA(float LLA[3], float Rne[3][3])
void R2Quaternion(float R[3][3], float q[4])
uint8_t i
Definition: msp_messages.h:97
void Euler2R(float rpy[3], float Rbe[3][3])
Header for Coordinate conversions library in coordinate_conversions.c.
void CrossProduct(const float v1[3], const float v2[3], float result[3])
tuple f
Definition: px_mkfw.py:81
void quat_inverse(float q[4])
Compute the inverse of a quaternion.
uint8_t RotFrom2Vectors(const float v1b[3], const float v1e[3], const float v2b[3], const float v2e[3], float Rbe[3][3])
void Quaternion2RPY(const float q[4], float rpy[3])