ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
orientation.c
Go to the documentation of this file.
1 // Copyright (c) 2014, Freescale Semiconductor, Inc.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 // * Redistributions of source code must retain the above copyright
7 // notice, this list of conditions and the following disclaimer.
8 // * Redistributions in binary form must reproduce the above copyright
9 // notice, this list of conditions and the following disclaimer in the
10 // documentation and/or other materials provided with the distribution.
11 // * Neither the name of Freescale Semiconductor, Inc. nor the
12 // names of its contributors may be used to endorse or promote products
13 // derived from this software without specific prior written permission.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
16 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
17 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
18 // DISCLAIMED. IN NO EVENT SHALL FREESCALE SEMICONDUCTOR, INC. BE LIABLE FOR ANY
19 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
20 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
21 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
22 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
23 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
24 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
25 //
26 // This file contains functions designed to operate on, or compute, orientations.
27 // These may be in rotation matrix form, quaternion form, or Euler angles.
28 // It also includes functions designed to operate with specify reference frames
29 // (Android, Windows 8, NED).
30 //
31 
32 #include "math.h"
33 #include "math_constants.h"
34 #include "matrix.h"
35 #include "fusion_types.h"
36 #include "approximations.h"
37 #include "orientation.h"
38 
39 // compile time constants that are private to this file
40 #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm
41 #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
42 #define SMALLMODULUS 0.01F // limit where rounding errors may appear
43 
44 #define ONEOVER48 0.02083333333F // 1 / 48
45 #define ONEOVER3840 0.0002604166667F // 1 / 3840
46 
47 // Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
48 void f3DOFTiltNED(float fR[][3], float fGp[])
49 {
50  // the NED self-consistency twist occurs at 90 deg pitch
51 
52  // local variables
53  int16 i; // counter
54  float fmodGxyz; // modulus of the x, y, z accelerometer readings
55  float fmodGyz; // modulus of the y, z accelerometer readings
56  float frecipmodGxyz; // reciprocal of modulus
57  float ftmp; // scratch variable
58 
59  // compute the accelerometer squared magnitudes
60  fmodGyz = fGp[Y] * fGp[Y] + fGp[Z] * fGp[Z];
61  fmodGxyz = fmodGyz + fGp[X] * fGp[X];
62 
63  // check for freefall special case where no solution is possible
64  if (fmodGxyz == 0.0F)
65  {
66  f3x3matrixAeqI(fR);
67  return;
68  }
69 
70  // check for vertical up or down gimbal lock case
71  if (fmodGyz == 0.0F)
72  {
73  f3x3matrixAeqScalar(fR, 0.0F);
74  fR[Y][Y] = 1.0F;
75  if (fGp[X] >= 0.0F)
76  {
77  fR[X][Z] = 1.0F;
78  fR[Z][X] = -1.0F;
79  }
80  else
81  {
82  fR[X][Z] = -1.0F;
83  fR[Z][X] = 1.0F;
84  }
85  return;
86  }
87 
88  // compute moduli for the general case
89  fmodGyz = sqrtf(fmodGyz);
90  fmodGxyz = sqrtf(fmodGxyz);
91  frecipmodGxyz = 1.0F / fmodGxyz;
92  ftmp = fmodGxyz / fmodGyz;
93 
94  // normalize the accelerometer reading into the z column
95  for (i = X; i <= Z; i++)
96  {
97  fR[i][Z] = fGp[i] * frecipmodGxyz;
98  }
99 
100  // construct x column of orientation matrix
101  fR[X][X] = fmodGyz * frecipmodGxyz;
102  fR[Y][X] = -fR[X][Z] * fR[Y][Z] * ftmp;
103  fR[Z][X] = -fR[X][Z] * fR[Z][Z] * ftmp;
104 
105  // // construct y column of orientation matrix
106  fR[X][Y] = 0.0F;
107  fR[Y][Y] = fR[Z][Z] * ftmp;
108  fR[Z][Y] = -fR[Y][Z] * ftmp;
109 
110  return;
111 }
112 
113 // Android accelerometer 3DOF tilt function computing rotation matrix fR
114 void f3DOFTiltAndroid(float fR[][3], float fGp[])
115 {
116  // the Android tilt matrix is mathematically identical to the NED tilt matrix
117  // the Android self-consistency twist occurs at 90 deg roll
118  f3DOFTiltNED(fR, fGp);
119  return;
120 }
121 
122 // Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR
123 void f3DOFTiltWin8(float fR[][3], float fGp[])
124 {
125  // the Win8 self-consistency twist occurs at 90 deg roll
126 
127  // local variables
128  float fmodGxyz; // modulus of the x, y, z accelerometer readings
129  float fmodGxz; // modulus of the x, z accelerometer readings
130  float frecipmodGxyz; // reciprocal of modulus
131  float ftmp; // scratch variable
132  int8 i; // counter
133 
134  // compute the accelerometer squared magnitudes
135  fmodGxz = fGp[X] * fGp[X] + fGp[Z] * fGp[Z];
136  fmodGxyz = fmodGxz + fGp[Y] * fGp[Y];
137 
138  // check for freefall special case where no solution is possible
139  if (fmodGxyz == 0.0F)
140  {
141  f3x3matrixAeqI(fR);
142  return;
143  }
144 
145  // check for vertical up or down gimbal lock case
146  if (fmodGxz == 0.0F)
147  {
148  f3x3matrixAeqScalar(fR, 0.0F);
149  fR[X][X] = 1.0F;
150  if (fGp[Y] >= 0.0F)
151  {
152  fR[Y][Z] = -1.0F;
153  fR[Z][Y] = 1.0F;
154  }
155  else
156  {
157  fR[Y][Z] = 1.0F;
158  fR[Z][Y] = -1.0F;
159  }
160  return;
161  }
162 
163  // compute moduli for the general case
164  fmodGxz = sqrtf(fmodGxz);
165  fmodGxyz = sqrtf(fmodGxyz);
166  frecipmodGxyz = 1.0F / fmodGxyz;
167  ftmp = fmodGxyz / fmodGxz;
168  if (fGp[Z] < 0.0F)
169  {
170  ftmp = -ftmp;
171  }
172 
173  // normalize the negated accelerometer reading into the z column
174  for (i = X; i <= Z; i++)
175  {
176  fR[i][Z] = -fGp[i] * frecipmodGxyz;
177  }
178 
179  // construct x column of orientation matrix
180  fR[X][X] = -fR[Z][Z] * ftmp;
181  fR[Y][X] = 0.0F;
182  fR[Z][X] = fR[X][Z] * ftmp;;
183 
184  // // construct y column of orientation matrix
185  fR[X][Y] = fR[X][Z] * fR[Y][Z] * ftmp;
186  fR[Y][Y] = -fmodGxz * frecipmodGxyz;
187  if (fGp[Z] < 0.0F)
188  {
189  fR[Y][Y] = -fR[Y][Y];
190  }
191  fR[Z][Y] = fR[Y][Z] * fR[Z][Z] * ftmp;
192 
193  return;
194 }
195 
196 // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
197 void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
198 {
199  // local variables
200  float fmodBxy; // modulus of the x, y magnetometer readings
201 
202  // compute the magnitude of the horizontal (x and y) magnetometer reading
203  fmodBxy = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y]);
204 
205  // check for zero field special case where no solution is possible
206  if (fmodBxy == 0.0F)
207  {
208  f3x3matrixAeqI(fR);
209  return;
210  }
211 
212  // define the fixed entries in the z row and column
213  fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
214  fR[Z][Z] = 1.0F;
215 
216  // define the remaining entries
217  fR[X][X] = fR[Y][Y] = fBc[X] / fmodBxy;
218  fR[Y][X] = fBc[Y] / fmodBxy;
219  fR[X][Y] = -fR[Y][X];
220 
221  return;
222 }
223 
224 // Android magnetometer 3DOF flat eCompass function computing rotation matrix fR
225 void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
226 {
227  // local variables
228  float fmodBxy; // modulus of the x, y magnetometer readings
229 
230  // compute the magnitude of the horizontal (x and y) magnetometer reading
231  fmodBxy = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y]);
232 
233  // check for zero field special case where no solution is possible
234  if (fmodBxy == 0.0F)
235  {
236  f3x3matrixAeqI(fR);
237  return;
238  }
239 
240  // define the fixed entries in the z row and column
241  fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
242  fR[Z][Z] = 1.0F;
243 
244  // define the remaining entries
245  fR[X][X] = fR[Y][Y] = fBc[Y] / fmodBxy;
246  fR[X][Y] = fBc[X] / fmodBxy;
247  fR[Y][X] = -fR[X][Y];
248 
249  return;
250 }
251 
252 // Windows 8 magnetometer 3DOF flat eCompass function computing rotation matrix fR
253 void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
254 {
255  // call the Android function since it is identical to the Windows 8 matrix
257 
258  return;
259 }
260 
261 // NED: 6DOF e-Compass function computing rotation matrix fR
262 void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
263 {
264  // local variables
265  float fmod[3]; // column moduli
266  float fmodBc; // modulus of Bc
267  float fGdotBc; // dot product of vectors G.Bc
268  float ftmp; // scratch variable
269  int8 i, j; // loop counters
270 
271  // set the inclination angle to zero in case it is not computed later
272  *pfDelta = 0.0F;
273 
274  // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
275  for (i = X; i <= Z; i++)
276  {
277  fR[i][Z] = fGp[i];
278  fR[i][X] = fBc[i];
279  }
280 
281  // set y vector to vector product of z and x vectors
282  fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
283  fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
284  fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
285 
286  // set x vector to vector product of y and z vectors
287  fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
288  fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
289  fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
290 
291  // calculate the rotation matrix column moduli
292  fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
293  fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
294  fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
295 
296  // normalize the rotation matrix columns
297  if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
298  {
299  // loop over columns j
300  for (j = X; j <= Z; j++)
301  {
302  ftmp = 1.0F / fmod[j];
303  // loop over rows i
304  for (i = X; i <= Z; i++)
305  {
306  // normalize by the column modulus
307  fR[i][j] *= ftmp;
308  }
309  }
310  }
311  else
312  {
313  // no solution is possible to set rotation to identity matrix
314  f3x3matrixAeqI(fR);
315  return;
316  }
317 
318  // compute the geomagnetic inclination angle
319  fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
320  fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
321  if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
322  {
323  *pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
324  }
325 
326  return;
327 }
328 
329 // Android: 6DOF e-Compass function computing rotation matrix fR
330 void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
331 {
332  // local variables
333  float fmod[3]; // column moduli
334  float fmodBc; // modulus of Bc
335  float fGdotBc; // dot product of vectors G.Bc
336  float ftmp; // scratch variable
337  int8 i, j; // loop counters
338 
339  // set the inclination angle to zero in case it is not computed later
340  *pfDelta = 0.0F;
341 
342  // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and y axes
343  for (i = X; i <= Z; i++)
344  {
345  fR[i][Z] = fGp[i];
346  fR[i][Y] = fBc[i];
347  }
348 
349  // set x vector to vector product of y and z vectors
350  fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
351  fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
352  fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
353 
354  // set y vector to vector product of z and x vectors
355  fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
356  fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
357  fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
358 
359  // calculate the rotation matrix column moduli
360  fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
361  fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
362  fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
363 
364  // normalize the rotation matrix columns
365  if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
366  {
367  // loop over columns j
368  for (j = X; j <= Z; j++)
369  {
370  ftmp = 1.0F / fmod[j];
371  // loop over rows i
372  for (i = X; i <= Z; i++)
373  {
374  // normalize by the column modulus
375  fR[i][j] *= ftmp;
376  }
377  }
378  }
379  else
380  {
381  // no solution is possible so set rotation to identity matrix
382  f3x3matrixAeqI(fR);
383  return;
384  }
385 
386  // compute the geomagnetic inclination angle
387  fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
388  fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
389  if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
390  {
391  *pfDelta = fasin_deg(-fGdotBc / (fmod[Z] * fmodBc));
392  }
393 
394  return;
395 }
396 
397 // Win8: 6DOF e-Compass function computing rotation matrix fR
398 void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
399 {
400  // local variables
401  float fmod[3]; // column moduli
402  float fmodBc; // modulus of Bc
403  float fGdotBc; // dot product of vectors G.Bc
404  float ftmp; // scratch variable
405  int8 i, j; // loop counters
406 
407  // set the inclination angle to zero in case it is not computed later
408  *pfDelta = 0.0F;
409 
410  // place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
411  for (i = X; i <= Z; i++)
412  {
413  fR[i][Z] = -fGp[i];
414  fR[i][Y] = fBc[i];
415  }
416 
417  // set x vector to vector product of y and z vectors
418  fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
419  fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
420  fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
421 
422  // set y vector to vector product of z and x vectors
423  fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
424  fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
425  fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
426 
427  // calculate the rotation matrix column moduli
428  fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
429  fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
430  fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
431 
432  // normalize the rotation matrix columns
433  if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
434  {
435  // loop over columns j
436  for (j = X; j <= Z; j++)
437  {
438  ftmp = 1.0F / fmod[j];
439  // loop over rows i
440  for (i = X; i <= Z; i++)
441  {
442  // normalize by the column modulus
443  fR[i][j] *= ftmp;
444  }
445  }
446  }
447  else
448  {
449  // no solution is possible to set rotation to identity matrix
450  f3x3matrixAeqI(fR);
451  return;
452  }
453 
454  // compute the geomagnetic inclination angle
455  fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
456  fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
457  if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
458  {
459  *pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
460  }
461 
462  return;
463 }
464 
465 // extract the NED angles in degrees from the NED rotation matrix
466 void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
467  float *pfRhoDeg, float *pfChiDeg)
468 {
469  // calculate the pitch angle -90.0 <= Theta <= 90.0 deg
470  *pfTheDeg = fasin_deg(-R[X][Z]);
471 
472  // calculate the roll angle range -180.0 <= Phi < 180.0 deg
473  *pfPhiDeg = fatan2_deg(R[Y][Z], R[Z][Z]);
474 
475  // map +180 roll onto the functionally equivalent -180 deg roll
476  if (*pfPhiDeg == 180.0F)
477  {
478  *pfPhiDeg = -180.0F;
479  }
480 
481  // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
482  if (*pfTheDeg == 90.0F)
483  {
484  // vertical upwards gimbal lock case
485  *pfPsiDeg = fatan2_deg(R[Z][Y], R[Y][Y]) + *pfPhiDeg;
486  }
487  else if (*pfTheDeg == -90.0F)
488  {
489  // vertical downwards gimbal lock case
490  *pfPsiDeg = fatan2_deg(-R[Z][Y], R[Y][Y]) - *pfPhiDeg;
491  }
492  else
493  {
494  // general case
495  *pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]);
496  }
497 
498  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
499  if (*pfPsiDeg < 0.0F)
500  {
501  *pfPsiDeg += 360.0F;
502  }
503 
504  // check for rounding errors mapping small negative angle to 360 deg
505  if (*pfPsiDeg >= 360.0F)
506  {
507  *pfPsiDeg = 0.0F;
508  }
509 
510  // for NED, the compass heading Rho equals the yaw angle Psi
511  *pfRhoDeg = *pfPsiDeg;
512 
513  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
514  *pfChiDeg = facos_deg(R[Z][Z]);
515 
516  return;
517 }
518 
519 // extract the Android angles in degrees from the Android rotation matrix
520 void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
521  float *pfRhoDeg, float *pfChiDeg)
522 {
523  // calculate the roll angle -90.0 <= Phi <= 90.0 deg
524  *pfPhiDeg = fasin_deg(R[X][Z]);
525 
526  // calculate the pitch angle -180.0 <= The < 180.0 deg
527  *pfTheDeg = fatan2_deg(-R[Y][Z], R[Z][Z]);
528 
529  // map +180 pitch onto the functionally equivalent -180 deg pitch
530  if (*pfTheDeg == 180.0F)
531  {
532  *pfTheDeg = -180.0F;
533  }
534 
535  // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
536  if (*pfPhiDeg == 90.0F)
537  {
538  // vertical downwards gimbal lock case
539  *pfPsiDeg = fatan2_deg(R[Y][X], R[Y][Y]) - *pfTheDeg;
540  }
541  else if (*pfPhiDeg == -90.0F)
542  {
543  // vertical upwards gimbal lock case
544  *pfPsiDeg = fatan2_deg(R[Y][X], R[Y][Y]) + *pfTheDeg;
545  }
546  else
547  {
548  // general case
549  *pfPsiDeg = fatan2_deg(-R[X][Y], R[X][X]);
550  }
551 
552  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
553  if (*pfPsiDeg < 0.0F)
554  {
555  *pfPsiDeg += 360.0F;
556  }
557 
558  // check for rounding errors mapping small negative angle to 360 deg
559  if (*pfPsiDeg >= 360.0F)
560  {
561  *pfPsiDeg = 0.0F;
562  }
563 
564  // the compass heading angle Rho equals the yaw angle Psi
565  // this definition is compliant with Motorola Xoom tablet behavior
566  *pfRhoDeg = *pfPsiDeg;
567 
568  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
569  *pfChiDeg = facos_deg(R[Z][Z]);
570 
571  return;
572 }
573 
574 // extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
575 void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
576  float *pfRhoDeg, float *pfChiDeg)
577 {
578  // calculate the roll angle -90.0 <= Phi <= 90.0 deg
579  if (R[Z][Z] == 0.0F)
580  {
581  if (R[X][Z] >= 0.0F)
582  {
583  // tan(phi) is -infinity
584  *pfPhiDeg = -90.0F;
585  }
586  else
587  {
588  // tan(phi) is +infinity
589  *pfPhiDeg = 90.0F;
590  }
591  }
592  else
593  {
594  // general case
595  *pfPhiDeg = fatan_deg(-R[X][Z] / R[Z][Z]);
596  }
597 
598  // first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
599  *pfTheDeg = fasin_deg(R[Y][Z]);
600 
601  // use R[Z][Z]=cos(Phi)*cos(The) to correct the quadrant of The remembering
602  // cos(Phi) is non-negative so that cos(The) has the same sign as R[Z][Z].
603  if (R[Z][Z] < 0.0F)
604  {
605  // wrap The around +90 deg and -90 deg giving result 90 to 270 deg
606  *pfTheDeg = 180.0F - *pfTheDeg;
607  }
608 
609  // map the pitch angle The to the range -180.0 <= The < 180.0 deg
610  if (*pfTheDeg >= 180.0F)
611  {
612  *pfTheDeg -= 360.0F;
613  }
614 
615  // calculate the yaw angle Psi
616  if (*pfTheDeg == 90.0F)
617  {
618  // vertical upwards gimbal lock case: -270 <= Psi < 90 deg
619  *pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) - *pfPhiDeg;
620  }
621  else if (*pfTheDeg == -90.0F)
622  {
623  // vertical downwards gimbal lock case: -270 <= Psi < 90 deg
624  *pfPsiDeg = fatan2_deg(R[X][Y], R[X][X]) + *pfPhiDeg;
625  }
626  else
627  {
628  // general case: -180 <= Psi < 180 deg
629  *pfPsiDeg = fatan2_deg(-R[Y][X], R[Y][Y]);
630 
631  // correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
632  if (fabs(*pfTheDeg) >= 90.0F)
633  {
634  *pfPsiDeg += 180.0F;
635  }
636  }
637 
638  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
639  if (*pfPsiDeg < 0.0F)
640  {
641  *pfPsiDeg += 360.0F;
642  }
643 
644  // check for any rounding error mapping small negative angle to 360 deg
645  if (*pfPsiDeg >= 360.0F)
646  {
647  *pfPsiDeg = 0.0F;
648  }
649 
650  // compute the compass angle Rho = 360 - Psi
651  *pfRhoDeg = 360.0F - *pfPsiDeg;
652 
653  // check for rounding errors mapping small negative angle to 360 deg and zero degree case
654  if (*pfRhoDeg >= 360.0F)
655  {
656  *pfRhoDeg = 0.0F;
657  }
658 
659  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
660  *pfChiDeg = facos_deg(R[Z][Z]);
661 
662  return;
663 }
664 
665 // computes normalized rotation quaternion from a rotation vector (deg)
666 void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
667 {
668  float fetadeg; // rotation angle (deg)
669  float fetarad; // rotation angle (rad)
670  float fetarad2; // eta (rad)^2
671  float fetarad4; // eta (rad)^4
672  float sinhalfeta; // sin(eta/2)
673  float fvecsq; // q1^2+q2^2+q3^2
674  float ftmp; // scratch variable
675 
676  // compute the scaled rotation angle eta (deg) which can be both positve or negative
677  fetadeg = fscaling * sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]);
678  fetarad = fetadeg * FDEGTORAD;
679  fetarad2 = fetarad * fetarad;
680 
681  // calculate the sine and cosine using small angle approximations or exact
682  // angles under sqrt(0.02)=0.141 rad is 8.1 deg and 1620 deg/s (=936deg/s in 3 axes) at 200Hz and 405 deg/s at 50Hz
683  if (fetarad2 <= 0.02F)
684  {
685  // use MacLaurin series up to and including third order
686  sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2);
687  }
688  else if (fetarad2 <= 0.06F)
689  {
690  // use MacLaurin series up to and including fifth order
691  // angles under sqrt(0.06)=0.245 rad is 14.0 deg and 2807 deg/s (=1623deg/s in 3 axes) at 200Hz and 703 deg/s at 50Hz
692  fetarad4 = fetarad2 * fetarad2;
693  sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4);
694  }
695  else
696  {
697  // use exact calculation
698  sinhalfeta = (float)sinf(0.5F * fetarad);
699  }
700 
701  // compute the vector quaternion components q1, q2, q3
702  if (fetadeg != 0.0F)
703  {
704  // general case with non-zero rotation angle
705  ftmp = fscaling * sinhalfeta / fetadeg;
706  pq->q1 = rvecdeg[X] * ftmp; // q1 = nx * sin(eta/2)
707  pq->q2 = rvecdeg[Y] * ftmp; // q2 = ny * sin(eta/2)
708  pq->q3 = rvecdeg[Z] * ftmp; // q3 = nz * sin(eta/2)
709  }
710  else
711  {
712  // zero rotation angle giving zero vector component
713  pq->q1 = pq->q2 = pq->q3 = 0.0F;
714  }
715 
716  // compute the scalar quaternion component q0 by explicit normalization
717  // taking care to avoid rounding errors giving negative operand to sqrt
718  fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3;
719  if (fvecsq <= 1.0F)
720  {
721  // normal case
722  pq->q0 = sqrtf(1.0F - fvecsq);
723  }
724  else
725  {
726  // rounding errors are present
727  pq->q0 = 0.0F;
728  }
729 
730  return;
731 }
732 
733 // compute the orientation quaternion from a 3x3 rotation matrix
734 void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
735 {
736  float fq0sq; // q0^2
737  float recip4q0; // 1/4q0
738 
739  // the quaternion is not explicitly normalized in this function on the assumption that it
740  // is supplied with a normalized rotation matrix. if the rotation matrix is normalized then
741  // the quaternion will also be normalized even if the case of small q0
742 
743  // get q0^2 and q0
744  fq0sq = 0.25F * (1.0F + R[X][X] + R[Y][Y] + R[Z][Z]);
745  pq->q0 = sqrtf(fabs(fq0sq));
746 
747  // normal case when q0 is not small meaning rotation angle not near 180 deg
748  if (pq->q0 > SMALLQ0)
749  {
750  // calculate q1 to q3
751  recip4q0 = 0.25F / pq->q0;
752  pq->q1 = recip4q0 * (R[Y][Z] - R[Z][Y]);
753  pq->q2 = recip4q0 * (R[Z][X] - R[X][Z]);
754  pq->q3 = recip4q0 * (R[X][Y] - R[Y][X]);
755  } // end of general case
756  else
757  {
758  // special case of near 180 deg corresponds to nearly symmetric matrix
759  // which is not numerically well conditioned for division by small q0
760  // instead get absolute values of q1 to q3 from leading diagonal
761  pq->q1 = sqrtf(fabs(0.5F * (1.0F + R[X][X]) - fq0sq));
762  pq->q2 = sqrtf(fabs(0.5F * (1.0F + R[Y][Y]) - fq0sq));
763  pq->q3 = sqrtf(fabs(0.5F * (1.0F + R[Z][Z]) - fq0sq));
764 
765  // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms
766  if ((R[Y][Z] - R[Z][Y]) < 0.0F) pq->q1 = -pq->q1;
767  if ((R[Z][X] - R[X][Z]) < 0.0F) pq->q2 = -pq->q2;
768  if ((R[X][Y] - R[Y][X]) < 0.0F) pq->q3 = -pq->q3;
769  } // end of special case
770 
771  return;
772 }
773 
774 // compute the rotation matrix from an orientation quaternion
775 void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
776 {
777  float f2q;
778  float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
779  float f2q1q1, f2q1q2, f2q1q3;
780  float f2q2q2, f2q2q3;
781  float f2q3q3;
782 
783  // calculate products
784  f2q = 2.0F * pq->q0;
785  f2q0q0 = f2q * pq->q0;
786  f2q0q1 = f2q * pq->q1;
787  f2q0q2 = f2q * pq->q2;
788  f2q0q3 = f2q * pq->q3;
789  f2q = 2.0F * pq->q1;
790  f2q1q1 = f2q * pq->q1;
791  f2q1q2 = f2q * pq->q2;
792  f2q1q3 = f2q * pq->q3;
793  f2q = 2.0F * pq->q2;
794  f2q2q2 = f2q * pq->q2;
795  f2q2q3 = f2q * pq->q3;
796  f2q3q3 = 2.0F * pq->q3 * pq->q3;
797 
798  // calculate the rotation matrix assuming the quaternion is normalized
799  R[X][X] = f2q0q0 + f2q1q1 - 1.0F;
800  R[X][Y] = f2q1q2 + f2q0q3;
801  R[X][Z] = f2q1q3 - f2q0q2;
802  R[Y][X] = f2q1q2 - f2q0q3;
803  R[Y][Y] = f2q0q0 + f2q2q2 - 1.0F;
804  R[Y][Z] = f2q2q3 + f2q0q1;
805  R[Z][X] = f2q1q3 + f2q0q2;
806  R[Z][Y] = f2q2q3 - f2q0q1;
807  R[Z][Z] = f2q0q0 + f2q3q3 - 1.0F;
808 
809  return;
810 }
811 
812 // function calculate the rotation vector from a rotation matrix
813 void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
814 {
815  float ftrace; // trace of the rotation matrix
816  float fetadeg; // rotation angle eta (deg)
817  float fmodulus; // modulus of axis * angle vector = 2|sin(eta)|
818  float ftmp; // scratch variable
819 
820  // calculate the trace of the rotation matrix = 1+2cos(eta) in range -1 to +3
821  // and eta (deg) in range 0 to 180 deg inclusive
822  // checking for rounding errors that might take the trace outside this range
823  ftrace = R[X][X] + R[Y][Y] + R[Z][Z];
824  if (ftrace >= 3.0F)
825  {
826  fetadeg = 0.0F;
827  }
828  else if (ftrace <= -1.0F)
829  {
830  fetadeg = 180.0F;
831  }
832  else
833  {
834  fetadeg = acosf(0.5F * (ftrace - 1.0F)) * FRADTODEG;
835  }
836 
837  // set the rvecdeg vector to differences across the diagonal = 2*n*sin(eta)
838  // and calculate its modulus equal to 2|sin(eta)|
839  // the modulus approaches zero near 0 and 180 deg (when sin(eta) approaches zero)
840  rvecdeg[X] = R[Y][Z] - R[Z][Y];
841  rvecdeg[Y] = R[Z][X] - R[X][Z];
842  rvecdeg[Z] = R[X][Y] - R[Y][X];
843  fmodulus = sqrtf(rvecdeg[X] * rvecdeg[X] + rvecdeg[Y] * rvecdeg[Y] + rvecdeg[Z] * rvecdeg[Z]);
844 
845  // normalize the rotation vector for general, 0 deg and 180 deg rotation cases
846  if (fmodulus > SMALLMODULUS)
847  {
848  // general case away from 0 and 180 deg rotation
849  ftmp = fetadeg / fmodulus;
850  rvecdeg[X] *= ftmp; // set x component to eta(deg) * nx
851  rvecdeg[Y] *= ftmp; // set y component to eta(deg) * ny
852  rvecdeg[Z] *= ftmp; // set z component to eta(deg) * nz
853  } // end of general case
854  else if (ftrace >= 0.0F)
855  {
856  // near 0 deg rotation (trace = 3): matrix is nearly identity matrix
857  // R[Y][Z]-R[Z][Y]=2*nx*eta(rad) and similarly for other components
858  ftmp = 0.5F * FRADTODEG;
859  rvecdeg[X] *= ftmp;
860  rvecdeg[Y] *= ftmp;
861  rvecdeg[Z] *= ftmp;
862  } // end of zero deg case
863  else
864  {
865  // near 180 deg (trace = -1): matrix is nearly symmetric
866  // calculate the absolute value of the components of the axis-angle vector
867  rvecdeg[X] = 180.0F * sqrtf(fabs(0.5F * (R[X][X] + 1.0F)));
868  rvecdeg[Y] = 180.0F * sqrtf(fabs(0.5F * (R[Y][Y] + 1.0F)));
869  rvecdeg[Z] = 180.0F * sqrtf(fabs(0.5F * (R[Z][Z] + 1.0F)));
870 
871  // correct the signs of the three components by examining the signs of differenced off-diagonal terms
872  if ((R[Y][Z] - R[Z][Y]) < 0.0F) rvecdeg[X] = -rvecdeg[X];
873  if ((R[Z][X] - R[X][Z]) < 0.0F) rvecdeg[Y] = -rvecdeg[Y];
874  if ((R[X][Y] - R[Y][X]) < 0.0F) rvecdeg[Z] = -rvecdeg[Z];
875 
876  } // end of 180 deg case
877 
878  return;
879 }
880 
881 // computes rotation vector (deg) from rotation quaternion
882 void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
883 {
884  float fetarad; // rotation angle (rad)
885  float fetadeg; // rotation angle (deg)
886  float sinhalfeta; // sin(eta/2)
887  float ftmp; // scratch variable
888 
889  // calculate the rotation angle in the range 0 <= eta < 360 deg
890  if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F))
891  {
892  // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg
893  fetarad = 0.0F;
894  fetadeg = 0.0F;
895  }
896  else
897  {
898  // general case returning 0 < eta < 360 deg
899  fetarad = 2.0F * acosf(pq->q0);
900  fetadeg = fetarad * FRADTODEG;
901  }
902 
903  // map the rotation angle onto the range -180 deg <= eta < 180 deg
904  if (fetadeg >= 180.0F)
905  {
906  fetadeg -= 360.0F;
907  fetarad = fetadeg * FDEGTORAD;
908  }
909 
910  // calculate sin(eta/2) which will be in the range -1 to +1
911  sinhalfeta = (float)sinf(0.5F * fetarad);
912 
913  // calculate the rotation vector (deg)
914  if (sinhalfeta == 0.0F)
915  {
916  // the rotation angle eta is zero and the axis is irrelevant
917  rvecdeg[X] = rvecdeg[Y] = rvecdeg[Z] = 0.0F;
918  }
919  else
920  {
921  // general case with non-zero rotation angle
922  ftmp = fetadeg / sinhalfeta;
923  rvecdeg[X] = pq->q1 * ftmp;
924  rvecdeg[Y] = pq->q2 * ftmp;
925  rvecdeg[Z] = pq->q3 * ftmp;
926  }
927 
928  return;
929 }
930 
931 // function low pass filters an orientation quaternion and computes virtual gyro rotation rate
932 void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat,
933  float fOmega[], int32 loopcounter)
934 {
935  // local variables
936  struct fquaternion fdeltaq; // delta rotation quaternion
937  float rvecdeg[3]; // rotation vector (deg)
938  float ftmp; // scratch variable
939 
940  // initialize delay line on first pass: LPq[n]=q[n]
941  if (loopcounter == 0)
942  {
943  *pLPq = *pq;
944  }
945 
946  // set fdeltaqn to the delta rotation quaternion conjg(fLPq[n-1) . fqn
947  fdeltaq = qconjgAxB(pLPq, pq);
948  if (fdeltaq.q0 < 0.0F)
949  {
950  fdeltaq.q0 = -fdeltaq.q0;
951  fdeltaq.q1 = -fdeltaq.q1;
952  fdeltaq.q2 = -fdeltaq.q2;
953  fdeltaq.q3 = -fdeltaq.q3;
954  }
955 
956  // set ftmp to a scaled lpf value which equals flpf in the limit of small rotations (q0=1)
957  // but which rises as the delta rotation angle increases (q0 tends to zero)
958  ftmp = flpf + 0.75F * (1.0F - fdeltaq.q0);
959  if (ftmp > 1.0F)
960  {
961  ftmp = 1.0F;
962  }
963 
964  // scale the delta rotation by the corrected lpf value
965  fdeltaq.q1 *= ftmp;
966  fdeltaq.q2 *= ftmp;
967  fdeltaq.q3 *= ftmp;
968 
969  // compute the scalar component q0
970  ftmp = fdeltaq.q1 * fdeltaq.q1 + fdeltaq.q2 * fdeltaq.q2 + fdeltaq.q3 * fdeltaq.q3;
971  if (ftmp <= 1.0F)
972  {
973  // normal case
974  fdeltaq.q0 = sqrtf(1.0F - ftmp);
975  }
976  else
977  {
978  // rounding errors present so simply set scalar component to 0
979  fdeltaq.q0 = 0.0F;
980  }
981 
982  // calculate the delta rotation vector from fdeltaqn and the virtual gyro angular velocity (deg/s)
983  fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg);
984  ftmp = 1.0F / fdeltat;
985  fOmega[X] = rvecdeg[X] * ftmp;
986  fOmega[Y] = rvecdeg[Y] * ftmp;
987  fOmega[Z] = rvecdeg[Z] * ftmp;
988 
989  // set LPq[n] = LPq[n-1] . deltaq[n]
990  qAeqAxB(pLPq, &fdeltaq);
991 
992  // renormalize the low pass filtered quaternion to prevent error accumulation
993  // the renormalization function ensures that q0 is non-negative
994  fqAeqNormqA(pLPq);
995 
996  return;
997 }
998 
999 // function low pass filters a scalar
1000 void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
1001 {
1002  // set S[LP,n]=S[n] on first pass
1003  if (loopcounter == 0)
1004  {
1005  *pfLPS = *pfS;
1006  }
1007 
1008  // apply the exponential low pass filter
1009  *pfLPS += flpf * (*pfS - *pfLPS);
1010 
1011  return;
1012 }
1013 
1014 // function compute the quaternion product qA * qB
1015 void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
1016 {
1017  pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3;
1018  pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2;
1019  pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1;
1020  pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0;
1021 
1022  return;
1023 }
1024 
1025 // function compute the quaternion product qA = qA * qB
1026 void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
1027 {
1028  struct fquaternion qProd;
1029 
1030  // perform the quaternion product
1031  qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3;
1032  qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2;
1033  qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1;
1034  qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0;
1035 
1036  // copy the result back into qA
1037  *pqA = qProd;
1038 
1039  return;
1040 }
1041 
1042 // function compute the quaternion product conjg(qA) * qB
1043 struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
1044 {
1045  struct fquaternion qProd;
1046 
1047  qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3;
1048  qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2;
1049  qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1;
1050  qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0;
1051 
1052  return qProd;
1053 }
1054 
1055 // function normalizes a rotation quaternion and ensures q0 is non-negative
1056 void fqAeqNormqA(struct fquaternion *pqA)
1057 {
1058  float fNorm; // quaternion Norm
1059 
1060  // calculate the quaternion Norm
1061  fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3);
1062  if (fNorm > CORRUPTQUAT)
1063  {
1064  // general case
1065  fNorm = 1.0F / fNorm;
1066  pqA->q0 *= fNorm;
1067  pqA->q1 *= fNorm;
1068  pqA->q2 *= fNorm;
1069  pqA->q3 *= fNorm;
1070  }
1071  else
1072  {
1073  // return with identity quaternion since the quaternion is corrupted
1074  pqA->q0 = 1.0F;
1075  pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1076  }
1077 
1078  // correct a negative scalar component if the function was called with negative q0
1079  if (pqA->q0 < 0.0F)
1080  {
1081  pqA->q0 = -pqA->q0;
1082  pqA->q1 = -pqA->q1;
1083  pqA->q2 = -pqA->q2;
1084  pqA->q3 = -pqA->q3;
1085  }
1086 
1087  return;
1088 }
1089 
1090 // set a quaternion to the unit quaternion
1091 void fqAeq1(struct fquaternion *pqA)
1092 {
1093  pqA->q0 = 1.0F;
1094  pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1095 
1096  return;
1097 }
#define FDEGTORAD
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:197
void f3DOFTiltNED(float fR[][3], float fGp[])
Definition: orientation.c:48
float fatan_deg(float x)
float fasin_deg(float x)
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:520
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:466
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1091
Definition: matrix.h:33
signed char int8
Definition: basic_types.h:12
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:253
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:882
void fRotationVectorDegFromRotationMatrix(float R[][3], float rvecdeg[])
Definition: orientation.c:813
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:262
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1015
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:330
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1056
void f3DOFTiltAndroid(float fR[][3], float fGp[])
Definition: orientation.c:114
#define CORRUPTQUAT
Definition: orientation.c:41
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:666
#define SMALLMODULUS
Definition: orientation.c:42
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1026
long int32
This defines int32 as long.
Definition: isf_types.h:32
Definition: matrix.h:34
short int16
This defines int16 as short.
Definition: isf_types.h:23
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1043
Definition: matrix.h:35
#define FRADTODEG
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter)
Definition: orientation.c:932
#define ONEOVER3840
Definition: orientation.c:45
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:76
#define SMALLQ0
Definition: orientation.c:40
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:775
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:734
float facos_deg(float x)
void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
Definition: orientation.c:1000
void f3DOFTiltWin8(float fR[][3], float fGp[])
Definition: orientation.c:123
float fatan2_deg(float y, float x)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:225
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
Definition: orientation.c:398
#define ONEOVER48
Definition: orientation.c:44
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:575