ISF  2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
orientation.c
Go to the documentation of this file.
1 // Copyright (c) 2014, 2015, 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 #include "math.h"
32 #include "math_constants.h"
33 #include "matrix.h"
34 #include "fusion_types.h"
35 #include "approximations.h"
36 #include "orientation.h"
37 
38 // compile time constants that are private to this file
39 #define SMALLQ0 0.01F // limit of quaternion scalar component requiring special algorithm
40 #define CORRUPTQUAT 0.001F // threshold for deciding rotation quaternion is corrupt
41 #define SMALLMODULUS 0.01F // limit where rounding errors may appear
42 
43 // Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
44 void f3DOFTiltNED(float fR[][3], float fGs[])
45 {
46  // the NED self-consistency twist occurs at 90 deg pitch
47 
48  // local variables
49  int16 i; // counter
50  float fmodGxyz; // modulus of the x, y, z accelerometer readings
51  float fmodGyz; // modulus of the y, z accelerometer readings
52  float frecipmodGxyz; // reciprocal of modulus
53  float ftmp; // scratch variable
54 
55  // compute the accelerometer squared magnitudes
56  fmodGyz = fGs[CHY] * fGs[CHY] + fGs[CHZ] * fGs[CHZ];
57  fmodGxyz = fmodGyz + fGs[CHX] * fGs[CHX];
58 
59  // check for freefall special case where no solution is possible
60  if (fmodGxyz == 0.0F)
61  {
62  f3x3matrixAeqI(fR);
63  return;
64  }
65 
66  // check for vertical up or down gimbal lock case
67  if (fmodGyz == 0.0F)
68  {
69  f3x3matrixAeqScalar(fR, 0.0F);
70  fR[CHY][CHY] = 1.0F;
71  if (fGs[CHX] >= 0.0F)
72  {
73  fR[CHX][CHZ] = 1.0F;
74  fR[CHZ][CHX] = -1.0F;
75  }
76  else
77  {
78  fR[CHX][CHZ] = -1.0F;
79  fR[CHZ][CHX] = 1.0F;
80  }
81  return;
82  }
83 
84  // compute moduli for the general case
85  fmodGyz = sqrtf(fmodGyz);
86  fmodGxyz = sqrtf(fmodGxyz);
87  frecipmodGxyz = 1.0F / fmodGxyz;
88  ftmp = fmodGxyz / fmodGyz;
89 
90  // normalize the accelerometer reading into the z column
91  for (i = CHX; i <= CHZ; i++)
92  {
93  fR[i][CHZ] = fGs[i] * frecipmodGxyz;
94  }
95 
96  // construct x column of orientation matrix
97  fR[CHX][CHX] = fmodGyz * frecipmodGxyz;
98  fR[CHY][CHX] = -fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
99  fR[CHZ][CHX] = -fR[CHX][CHZ] * fR[CHZ][CHZ] * ftmp;
100 
101  // construct y column of orientation matrix
102  fR[CHX][CHY] = 0.0F;
103  fR[CHY][CHY] = fR[CHZ][CHZ] * ftmp;
104  fR[CHZ][CHY] = -fR[CHY][CHZ] * ftmp;
105 
106  return;
107 }
108 
109 // Android accelerometer 3DOF tilt function computing rotation matrix fR
110 void f3DOFTiltAndroid(float fR[][3], float fGs[])
111 {
112  // the Android tilt matrix is mathematically identical to the NED tilt matrix
113  // the Android self-consistency twist occurs at 90 deg roll
114  f3DOFTiltNED(fR, fGs);
115  return;
116 }
117 
118 // Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR
119 void f3DOFTiltWin8(float fR[][3], float fGs[])
120 {
121  // the Win8 self-consistency twist occurs at 90 deg roll
122 
123  // local variables
124  float fmodGxyz; // modulus of the x, y, z accelerometer readings
125  float fmodGxz; // modulus of the x, z accelerometer readings
126  float frecipmodGxyz; // reciprocal of modulus
127  float ftmp; // scratch variable
128  int8 i; // counter
129 
130  // compute the accelerometer squared magnitudes
131  fmodGxz = fGs[CHX] * fGs[CHX] + fGs[CHZ] * fGs[CHZ];
132  fmodGxyz = fmodGxz + fGs[CHY] * fGs[CHY];
133 
134  // check for freefall special case where no solution is possible
135  if (fmodGxyz == 0.0F)
136  {
137  f3x3matrixAeqI(fR);
138  return;
139  }
140 
141  // check for vertical up or down gimbal lock case
142  if (fmodGxz == 0.0F)
143  {
144  f3x3matrixAeqScalar(fR, 0.0F);
145  fR[CHX][CHX] = 1.0F;
146  if (fGs[CHY] >= 0.0F)
147  {
148  fR[CHY][CHZ] = -1.0F;
149  fR[CHZ][CHY] = 1.0F;
150  }
151  else
152  {
153  fR[CHY][CHZ] = 1.0F;
154  fR[CHZ][CHY] = -1.0F;
155  }
156  return;
157  }
158 
159  // compute moduli for the general case
160  fmodGxz = sqrtf(fmodGxz);
161  fmodGxyz = sqrtf(fmodGxyz);
162  frecipmodGxyz = 1.0F / fmodGxyz;
163  ftmp = fmodGxyz / fmodGxz;
164  if (fGs[CHZ] < 0.0F)
165  {
166  ftmp = -ftmp;
167  }
168 
169  // normalize the negated accelerometer reading into the z column
170  for (i = CHX; i <= CHZ; i++)
171  {
172  fR[i][CHZ] = -fGs[i] * frecipmodGxyz;
173  }
174 
175  // construct x column of orientation matrix
176  fR[CHX][CHX] = -fR[CHZ][CHZ] * ftmp;
177  fR[CHY][CHX] = 0.0F;
178  fR[CHZ][CHX] = fR[CHX][CHZ] * ftmp;;
179 
180  // // construct y column of orientation matrix
181  fR[CHX][CHY] = fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
182  fR[CHY][CHY] = -fmodGxz * frecipmodGxyz;
183  if (fGs[CHZ] < 0.0F)
184  {
185  fR[CHY][CHY] = -fR[CHY][CHY];
186  }
187  fR[CHZ][CHY] = fR[CHY][CHZ] * fR[CHZ][CHZ] * ftmp;
188 
189  return;
190 }
191 
192 // Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
193 void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
194 {
195  // local variables
196  float fmodBxy; // modulus of the x, y magnetometer readings
197 
198  // compute the magnitude of the horizontal (x and y) magnetometer reading
199  fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
200 
201  // check for zero field special case where no solution is possible
202  if (fmodBxy == 0.0F)
203  {
204  f3x3matrixAeqI(fR);
205  return;
206  }
207 
208  // define the fixed entries in the z row and column
209  fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
210  fR[CHZ][CHZ] = 1.0F;
211 
212  // define the remaining entries
213  fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHX] / fmodBxy;
214  fR[CHY][CHX] = fBc[CHY] / fmodBxy;
215  fR[CHX][CHY] = -fR[CHY][CHX];
216 
217  return;
218 }
219 
220 // Android magnetometer 3DOF flat eCompass function computing rotation matrix fR
221 void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
222 {
223  // local variables
224  float fmodBxy; // modulus of the x, y magnetometer readings
225 
226  // compute the magnitude of the horizontal (x and y) magnetometer reading
227  fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
228 
229  // check for zero field special case where no solution is possible
230  if (fmodBxy == 0.0F)
231  {
232  f3x3matrixAeqI(fR);
233  return;
234  }
235 
236  // define the fixed entries in the z row and column
237  fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
238  fR[CHZ][CHZ] = 1.0F;
239 
240  // define the remaining entries
241  fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHY] / fmodBxy;
242  fR[CHX][CHY] = fBc[CHX] / fmodBxy;
243  fR[CHY][CHX] = -fR[CHX][CHY];
244 
245  return;
246 }
247 
248 // Windows 8 magnetometer 3DOF flat eCompass function computing rotation matrix fR
249 void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
250 {
251  // call the Android function since it is identical to the Windows 8 matrix
253 
254  return;
255 }
256 
257 // NED: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
258 void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
259 {
260  // local variables
261  float fmod[3]; // column moduli
262  float fmodBc; // modulus of Bc
263  float fGsdotBc; // dot product of vectors G.Bc
264  float ftmp; // scratch variable
265  int8 i, j; // loop counters
266 
267  // set the inclination angle to zero in case it is not computed later
268  *pfDelta = 0.0F;
269 
270  // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
271  for (i = CHX; i <= CHZ; i++)
272  {
273  fR[i][CHZ] = fGs[i];
274  fR[i][CHX] = fBc[i];
275  }
276 
277  // set y vector to vector product of z and x vectors
278  fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
279  fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
280  fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
281 
282  // set x vector to vector product of y and z vectors
283  fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
284  fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
285  fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
286 
287  // calculate the rotation matrix column moduli
288  fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
289  fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
290  fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
291 
292  // normalize the rotation matrix columns
293  if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
294  {
295  // loop over columns j
296  for (j = CHX; j <= CHZ; j++)
297  {
298  ftmp = 1.0F / fmod[j];
299  // loop over rows i
300  for (i = CHX; i <= CHZ; i++)
301  {
302  // normalize by the column modulus
303  fR[i][j] *= ftmp;
304  }
305  }
306  }
307  else
308  {
309  // no solution is possible so set rotation to identity matrix
310  f3x3matrixAeqI(fR);
311  return;
312  }
313 
314  // compute the geomagnetic inclination angle (deg)
315  fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
316  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
317  if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
318  {
319  *pfDelta = fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
320  }
321 
322  return;
323 }
324 
325 // Android: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
326 void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
327 {
328  // local variables
329  float fmod[3]; // column moduli
330  float fmodBc; // modulus of Bc
331  float fGsdotBc; // dot product of vectors G.Bc
332  float ftmp; // scratch variable
333  int8 i, j; // loop counters
334 
335  // set the inclination angle to zero in case it is not computed later
336  *pfDelta = 0.0F;
337 
338  // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and y axes
339  for (i = CHX; i <= CHZ; i++)
340  {
341  fR[i][CHZ] = fGs[i];
342  fR[i][CHY] = fBc[i];
343  }
344 
345  // set x vector to vector product of y and z vectors
346  fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
347  fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
348  fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
349 
350  // set y vector to vector product of z and x vectors
351  fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
352  fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
353  fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
354 
355  // calculate the rotation matrix column moduli
356  fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
357  fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
358  fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
359 
360  // normalize the rotation matrix columns
361  if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
362  {
363  // loop over columns j
364  for (j = CHX; j <= CHZ; j++)
365  {
366  ftmp = 1.0F / fmod[j];
367  // loop over rows i
368  for (i = CHX; i <= CHZ; i++)
369  {
370  // normalize by the column modulus
371  fR[i][j] *= ftmp;
372  }
373  }
374  }
375  else
376  {
377  // no solution is possible so set rotation to identity matrix
378  f3x3matrixAeqI(fR);
379  return;
380  }
381 
382  // compute the geomagnetic inclination angle (deg)
383  fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
384  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
385  if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
386  {
387  *pfDelta = -fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
388  }
389 
390  return;
391 }
392 
393 // Win8: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
394 void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
395 {
396  // local variables
397  float fmod[3]; // column moduli
398  float fmodBc; // modulus of Bc
399  float fGsdotBc; // dot product of vectors G.Bc
400  float ftmp; // scratch variable
401  int8 i, j; // loop counters
402 
403  // set the inclination angle to zero in case it is not computed later
404  *pfDelta = 0.0F;
405 
406  // place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
407  for (i = CHX; i <= CHZ; i++)
408  {
409  fR[i][CHZ] = -fGs[i];
410  fR[i][CHY] = fBc[i];
411  }
412 
413  // set x vector to vector product of y and z vectors
414  fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
415  fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
416  fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
417 
418  // set y vector to vector product of z and x vectors
419  fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
420  fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
421  fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
422 
423  // calculate the rotation matrix column moduli
424  fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
425  fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
426  fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
427 
428  // normalize the rotation matrix columns
429  if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
430  {
431  // loop over columns j
432  for (j = CHX; j <= CHZ; j++)
433  {
434  ftmp = 1.0F / fmod[j];
435  // loop over rows i
436  for (i = CHX; i <= CHZ; i++)
437  {
438  // normalize by the column modulus
439  fR[i][j] *= ftmp;
440  }
441  }
442  }
443  else
444  {
445  // no solution is possible so set rotation to identity matrix
446  f3x3matrixAeqI(fR);
447  return;
448  }
449 
450  // compute the geomagnetic inclination angle (deg)
451  fmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
452  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
453  if (!((fmod[CHZ] == 0.0F) || (fmodBc == 0.0F)))
454  {
455  *pfDelta = fasin_deg(fGsdotBc / (fmod[CHZ] * fmodBc));
456  }
457 
458  return;
459 }
460 
461 // NED: 6DOF e-Compass function computing least squares fit to orientation quaternion fq
462 // on the assumption that the geomagnetic field fB and magnetic inclination angle fDelta are known
463 void fLeastSquareseCompassNED(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta,
464  float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
465 {
466  // local variables
467  float fK[4][4]; // K measurement matrix
468  float eigvec[4][4]; // matrix of eigenvectors of K
469  float eigval[4]; // vector of eigenvalues of K
470  float fmodGsSq; // modulus of fGs[] squared
471  float fmodBcSq; // modulus of fBc[] squared
472  float fmodGs; // modulus of fGs[]
473  float fmodBc; // modulus of fBc[]
474  float fGsdotBc; // scalar product of Gs and Bc
475  float fag, fam; // relative weightings
476  float fagOvermodGs; // a0 / |Gs|
477  float famOvermodBc; // a1 / |Bc|
478  float famOvermodBccosDelta; // a1 / |Bc| * cos(Delta)
479  float famOvermodBcsinDelta; // a1 / |Bc| * sin(Delta)
480  float ftmp; // scratch
481  int8 i; // loop counter
482 
483  // calculate the measurement vector moduli and return with identity quaternion if either is null.
484  fmodGsSq = fGs[CHX] * fGs[CHX] + fGs[CHY] * fGs[CHY] + fGs[CHZ] * fGs[CHZ];
485  fmodBcSq = fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ];
486  fmodGs = sqrtf(fmodGsSq);
487  fmodBc = sqrtf(fmodBcSq);
488  if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
489  {
490  pfq->q0 = 1.0F;
491  pfq->q1 = pfq->q2 = pfq->q3 = 0.0F;
492  return;
493  }
494 
495  // calculate the accelerometer and magnetometer noise covariances (units rad^2) and least squares weightings
496  *pfQvGQa = fabsf(fmodGsSq - 1.0F);
497  *pfQvBQd = fabsf(fmodBcSq - fB * fB);
498  fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
499  fam = 1.0F - fag;
500 
501  // compute useful ratios to reduce computation
502  fagOvermodGs = fag / fmodGs;
503  famOvermodBc = fam / fmodBc;
504  famOvermodBccosDelta = famOvermodBc * fcosDelta;
505  famOvermodBcsinDelta = famOvermodBc * fsinDelta;
506 
507  // compute the scalar product Gs.Bc and 6DOF accelerometer plus magnetometer geomagnetic inclination angle (deg)
508  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
509  *pfDelta6DOF = fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
510 
511  // set the K matrix to the non-zero accelerometer components
512  fK[0][0] = fK[3][3] = fagOvermodGs * fGs[CHZ];
513  fK[1][1] = fK[2][2] = -fK[0][0];
514  fK[0][1] = fK[2][3] = fagOvermodGs * fGs[CHY];
515  fK[1][3] = fagOvermodGs * fGs[CHX];
516  fK[0][2] = -fK[1][3];
517 
518  // update the K matrix with the magnetometer component
519  ftmp = famOvermodBcsinDelta * fBc[CHY];
520  fK[0][1] += ftmp;
521  fK[2][3] += ftmp;
522  fK[1][2] = famOvermodBccosDelta * fBc[CHY];
523  fK[0][3] = -fK[1][2];
524  ftmp = famOvermodBccosDelta * fBc[CHX];
525  fK[0][0] += ftmp;
526  fK[1][1] += ftmp;
527  fK[2][2] -= ftmp;
528  fK[3][3] -= ftmp;
529  ftmp = famOvermodBcsinDelta * fBc[CHZ];
530  fK[0][0] += ftmp;
531  fK[1][1] -= ftmp;
532  fK[2][2] -= ftmp;
533  fK[3][3] += ftmp;
534  ftmp = famOvermodBccosDelta * fBc[CHZ];
535  fK[0][2] += ftmp;
536  fK[1][3] += ftmp;
537  ftmp = famOvermodBcsinDelta * fBc[CHX];
538  fK[0][2] -= ftmp;
539  fK[1][3] += ftmp;
540 
541  // copy above diagonal elements to below diagonal
542  fK[1][0] = fK[0][1];
543  fK[2][0] = fK[0][2];
544  fK[2][1] = fK[1][2];
545  fK[3][0] = fK[0][3];
546  fK[3][1] = fK[1][3];
547  fK[3][2] = fK[2][3];
548 
549  // set eigval to the unsorted eigenvalues and eigvec to the unsorted normalized eigenvectors of fK
550  eigencompute4(fK, eigval, eigvec, 4);
551 
552  // copy the largest eigenvector into the orientation quaternion fq
553  i = 0;
554  if (eigval[1] > eigval[i]) i = 1;
555  if (eigval[2] > eigval[i]) i = 2;
556  if (eigval[3] > eigval[i]) i = 3;
557  pfq->q0 = eigvec[0][i];
558  pfq->q1 = eigvec[1][i];
559  pfq->q2 = eigvec[2][i];
560  pfq->q3 = eigvec[3][i];
561 
562  // force q0 to be non-negative
563  if (pfq->q0 < 0.0F)
564  {
565  pfq->q0 = -pfq->q0;
566  pfq->q1 = -pfq->q1;
567  pfq->q2 = -pfq->q2;
568  pfq->q3 = -pfq->q3;
569  }
570 
571  return;
572 }
573 
574 // Android: 6DOF e-Compass function computing least squares fit to orientation quaternion fq
575 // on the assumption that the geomagnetic field fB and magnetic inclination angle fDelta are known
576 void fLeastSquareseCompassAndroid(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta,
577  float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
578 {
579  // local variables
580  float fK[4][4]; // K measurement matrix
581  float eigvec[4][4]; // matrix of eigenvectors of K
582  float eigval[4]; // vector of eigenvalues of K
583  float fmodGsSq; // modulus of fGs[] squared
584  float fmodBcSq; // modulus of fBc[] squared
585  float fmodGs; // modulus of fGs[]
586  float fmodBc; // modulus of fBc[]
587  float fGsdotBc; // scalar product of Gs and Bc
588  float fag, fam; // relative weightings
589  float fagOvermodGs; // a0 / |Gs|
590  float famOvermodBc; // a1 / |Bc|
591  float famOvermodBccosDelta; // a1 / |Bc| * cos(Delta)
592  float famOvermodBcsinDelta; // a1 / |Bc| * sin(Delta)
593  float ftmp; // scratch
594  int8 i; // loop counter
595 
596  // calculate the measurement vector moduli and return with identity quaternion if either is null.
597  fmodGsSq = fGs[CHX] * fGs[CHX] + fGs[CHY] * fGs[CHY] + fGs[CHZ] * fGs[CHZ];
598  fmodBcSq = fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ];
599  fmodGs = sqrtf(fmodGsSq);
600  fmodBc = sqrtf(fmodBcSq);
601  if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
602  {
603  pfq->q0 = 1.0F;
604  pfq->q1 = pfq->q2 = pfq->q3 = 0.0F;
605  return;
606  }
607 
608  // calculate the accelerometer and magnetometer noise covariances (units rad^2) and least squares weightings
609  *pfQvGQa = fabsf(fmodGsSq - 1.0F);
610  *pfQvBQd = fabsf(fmodBcSq - fB * fB);
611  fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
612  fam = 1.0F - fag;
613 
614  // compute useful ratios to reduce computation
615  fagOvermodGs = fag / fmodGs;
616  famOvermodBc = fam / fmodBc;
617  famOvermodBccosDelta = famOvermodBc * fcosDelta;
618  famOvermodBcsinDelta = famOvermodBc * fsinDelta;
619 
620  // compute the scalar product Gs.Bc and 6DOF accelerometer plus magnetometer geomagnetic inclination angle (deg)
621  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
622  *pfDelta6DOF = -fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
623 
624  // set the K matrix to the non-zero accelerometer components
625  fK[0][0] = fK[3][3] = fagOvermodGs * fGs[CHZ];
626  fK[1][1] = fK[2][2] = -fK[0][0];
627  fK[0][1] = fK[2][3] = fagOvermodGs * fGs[CHY];
628  fK[1][3] = fagOvermodGs * fGs[CHX];
629  fK[0][2] = -fK[1][3];
630 
631  // update the K matrix with the magnetometer component
632  ftmp = famOvermodBcsinDelta * fBc[CHX];
633  fK[0][2] += ftmp;
634  fK[1][3] -= ftmp;
635  fK[0][3] = fK[1][2] = famOvermodBccosDelta * fBc[CHX];
636  ftmp = famOvermodBccosDelta * fBc[CHY];
637  fK[0][0] += ftmp;
638  fK[1][1] -= ftmp;
639  fK[2][2] += ftmp;
640  fK[3][3] -= ftmp;
641  ftmp = famOvermodBcsinDelta * fBc[CHZ];
642  fK[0][0] -= ftmp;
643  fK[1][1] += ftmp;
644  fK[2][2] += ftmp;
645  fK[3][3] -= ftmp;
646  ftmp = famOvermodBccosDelta * fBc[CHZ];
647  fK[0][1] -= ftmp;
648  fK[2][3] += ftmp;
649  ftmp = famOvermodBcsinDelta * fBc[CHY];
650  fK[0][1] -= ftmp;
651  fK[2][3] -= ftmp;
652 
653  // copy above diagonal elements to below diagonal
654  fK[1][0] = fK[0][1];
655  fK[2][0] = fK[0][2];
656  fK[2][1] = fK[1][2];
657  fK[3][0] = fK[0][3];
658  fK[3][1] = fK[1][3];
659  fK[3][2] = fK[2][3];
660 
661  // set eigval to the unsorted eigenvalues and eigvec to the unsorted normalized eigenvectors of fK
662  eigencompute4(fK, eigval, eigvec, 4);
663 
664  // copy the largest eigenvector into the orientation quaternion fq
665  i = 0;
666  if (eigval[1] > eigval[i]) i = 1;
667  if (eigval[2] > eigval[i]) i = 2;
668  if (eigval[3] > eigval[i]) i = 3;
669  pfq->q0 = eigvec[0][i];
670  pfq->q1 = eigvec[1][i];
671  pfq->q2 = eigvec[2][i];
672  pfq->q3 = eigvec[3][i];
673 
674  // force q0 to be non-negative
675  if (pfq->q0 < 0.0F)
676  {
677  pfq->q0 = -pfq->q0;
678  pfq->q1 = -pfq->q1;
679  pfq->q2 = -pfq->q2;
680  pfq->q3 = -pfq->q3;
681  }
682 
683  return;
684 }
685 
686 // Win8: 6DOF e-Compass function computing least squares fit to orientation quaternion fq
687 // on the assumption that the geomagnetic field fB and magnetic inclination angle fDelta are known
688 void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta,
689  float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
690 {
691  // local variables
692  float fK[4][4]; // K measurement matrix
693  float eigvec[4][4]; // matrix of eigenvectors of K
694  float eigval[4]; // vector of eigenvalues of K
695  float fmodGsSq; // modulus of fGs[] squared
696  float fmodBcSq; // modulus of fBc[] squared
697  float fmodGs; // modulus of fGs[]
698  float fmodBc; // modulus of fBc[]
699  float fGsdotBc; // scalar product of Gs and Bc
700  float fag, fam; // relative weightings
701  float fagOvermodGs; // a0 / |Gs|
702  float famOvermodBc; // a1 / |Bc|
703  float famOvermodBccosDelta; // a1 / |Bc| * cos(Delta)
704  float famOvermodBcsinDelta; // a1 / |Bc| * sin(Delta)
705  float ftmp; // scratch
706  int8 i; // loop counter
707 
708  // calculate the measurement vector moduli and return with identity quaternion if either is null.
709  fmodGsSq = fGs[CHX] * fGs[CHX] + fGs[CHY] * fGs[CHY] + fGs[CHZ] * fGs[CHZ];
710  fmodBcSq = fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ];
711  fmodGs = sqrtf(fmodGsSq);
712  fmodBc = sqrtf(fmodBcSq);
713  if ((fmodGs == 0.0F) || (fmodBc == 0.0F) || (fB == 0.0))
714  {
715  pfq->q0 = 1.0F;
716  pfq->q1 = pfq->q2 = pfq->q3 = 0.0F;
717  return;
718  }
719 
720  // calculate the accelerometer and magnetometer noise covariances (units rad^2) and least squares weightings
721  *pfQvGQa = fabsf(fmodGsSq - 1.0F);
722  *pfQvBQd = fabsf(fmodBcSq - fB * fB);
723  fag = *pfQvBQd / (fB * fB * *pfQvGQa + *pfQvBQd);
724  fam = 1.0F - fag;
725 
726  // compute useful ratios to reduce computation
727  fagOvermodGs = fag / fmodGs;
728  famOvermodBc = fam / fmodBc;
729  famOvermodBccosDelta = famOvermodBc * fcosDelta;
730  famOvermodBcsinDelta = famOvermodBc * fsinDelta;
731 
732  // compute the scalar product Gs.Bc and 6DOF accelerometer plus magnetometer geomagnetic inclination angle (deg)
733  fGsdotBc = fGs[CHX] * fBc[CHX] + fGs[CHY] * fBc[CHY] + fGs[CHZ] * fBc[CHZ];
734  *pfDelta6DOF = fasin_deg((fGsdotBc) / (fmodGs * fmodBc));
735 
736  // set the K matrix to the non-zero accelerometer components
737  fK[0][0] = fK[3][3] = -fagOvermodGs * fGs[CHZ];
738  fK[1][1] = fK[2][2] = -fK[0][0];
739  fK[0][1] = fK[2][3] = -fagOvermodGs * fGs[CHY];
740  fK[1][3] = -fagOvermodGs * fGs[CHX];
741  fK[0][2] = -fK[1][3];
742 
743  // update the K matrix with the magnetometer component
744  ftmp = famOvermodBcsinDelta * fBc[CHX];
745  fK[0][2] += ftmp;
746  fK[1][3] -= ftmp;
747  fK[0][3] = fK[1][2] = famOvermodBccosDelta * fBc[CHX];
748  ftmp = famOvermodBccosDelta * fBc[CHY];
749  fK[0][0] += ftmp;
750  fK[1][1] -= ftmp;
751  fK[2][2] += ftmp;
752  fK[3][3] -= ftmp;
753  ftmp = famOvermodBcsinDelta * fBc[CHZ];
754  fK[0][0] -= ftmp;
755  fK[1][1] += ftmp;
756  fK[2][2] += ftmp;
757  fK[3][3] -= ftmp;
758  ftmp = famOvermodBccosDelta * fBc[CHZ];
759  fK[0][1] -= ftmp;
760  fK[2][3] += ftmp;
761  ftmp = famOvermodBcsinDelta * fBc[CHY];
762  fK[0][1] -= ftmp;
763  fK[2][3] -= ftmp;
764 
765  // copy above diagonal elements to below diagonal
766  fK[1][0] = fK[0][1];
767  fK[2][0] = fK[0][2];
768  fK[2][1] = fK[1][2];
769  fK[3][0] = fK[0][3];
770  fK[3][1] = fK[1][3];
771  fK[3][2] = fK[2][3];
772 
773  // set eigval to the unsorted eigenvalues and eigvec to the unsorted normalized eigenvectors of fK
774  eigencompute4(fK, eigval, eigvec, 4);
775 
776  // copy the largest eigenvector into the orientation quaternion fq
777  i = 0;
778  if (eigval[1] > eigval[i]) i = 1;
779  if (eigval[2] > eigval[i]) i = 2;
780  if (eigval[3] > eigval[i]) i = 3;
781  pfq->q0 = eigvec[0][i];
782  pfq->q1 = eigvec[1][i];
783  pfq->q2 = eigvec[2][i];
784  pfq->q3 = eigvec[3][i];
785 
786  // force q0 to be non-negative
787  if (pfq->q0 < 0.0F)
788  {
789  pfq->q0 = -pfq->q0;
790  pfq->q1 = -pfq->q1;
791  pfq->q2 = -pfq->q2;
792  pfq->q3 = -pfq->q3;
793  }
794 
795  return;
796 }
797 
798 // extract the NED angles in degrees from the NED rotation matrix
799 void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
800  float *pfRhoDeg, float *pfChiDeg)
801 {
802  // calculate the pitch angle -90.0 <= Theta <= 90.0 deg
803  *pfTheDeg = fasin_deg(-R[CHX][CHZ]);
804 
805  // calculate the roll angle range -180.0 <= Phi < 180.0 deg
806  *pfPhiDeg = fatan2_deg(R[CHY][CHZ], R[CHZ][CHZ]);
807 
808  // map +180 roll onto the functionally equivalent -180 deg roll
809  if (*pfPhiDeg == 180.0F)
810  {
811  *pfPhiDeg = -180.0F;
812  }
813 
814  // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
815  if (*pfTheDeg == 90.0F)
816  {
817  // vertical upwards gimbal lock case
818  *pfPsiDeg = fatan2_deg(R[CHZ][CHY], R[CHY][CHY]) + *pfPhiDeg;
819  }
820  else if (*pfTheDeg == -90.0F)
821  {
822  // vertical downwards gimbal lock case
823  *pfPsiDeg = fatan2_deg(-R[CHZ][CHY], R[CHY][CHY]) - *pfPhiDeg;
824  }
825  else
826  {
827  // general case
828  *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]);
829  }
830 
831  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
832  if (*pfPsiDeg < 0.0F)
833  {
834  *pfPsiDeg += 360.0F;
835  }
836 
837  // check for rounding errors mapping small negative angle to 360 deg
838  if (*pfPsiDeg >= 360.0F)
839  {
840  *pfPsiDeg = 0.0F;
841  }
842 
843  // for NED, the compass heading Rho equals the yaw angle Psi
844  *pfRhoDeg = *pfPsiDeg;
845 
846  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
847  *pfChiDeg = facos_deg(R[CHZ][CHZ]);
848 
849  return;
850 }
851 
852 // extract the Android angles in degrees from the Android rotation matrix
853 void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
854  float *pfRhoDeg, float *pfChiDeg)
855 {
856  // calculate the roll angle -90.0 <= Phi <= 90.0 deg
857  *pfPhiDeg = fasin_deg(R[CHX][CHZ]);
858 
859  // calculate the pitch angle -180.0 <= The < 180.0 deg
860  *pfTheDeg = fatan2_deg(-R[CHY][CHZ], R[CHZ][CHZ]);
861 
862  // map +180 pitch onto the functionally equivalent -180 deg pitch
863  if (*pfTheDeg == 180.0F)
864  {
865  *pfTheDeg = -180.0F;
866  }
867 
868  // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
869  if (*pfPhiDeg == 90.0F)
870  {
871  // vertical downwards gimbal lock case
872  *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) - *pfTheDeg;
873  }
874  else if (*pfPhiDeg == -90.0F)
875  {
876  // vertical upwards gimbal lock case
877  *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) + *pfTheDeg;
878  }
879  else
880  {
881  // general case
882  *pfPsiDeg = fatan2_deg(-R[CHX][CHY], R[CHX][CHX]);
883  }
884 
885  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
886  if (*pfPsiDeg < 0.0F)
887  {
888  *pfPsiDeg += 360.0F;
889  }
890 
891  // check for rounding errors mapping small negative angle to 360 deg
892  if (*pfPsiDeg >= 360.0F)
893  {
894  *pfPsiDeg = 0.0F;
895  }
896 
897  // the compass heading angle Rho equals the yaw angle Psi
898  // this definition is compliant with Motorola Xoom tablet behavior
899  *pfRhoDeg = *pfPsiDeg;
900 
901  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
902  *pfChiDeg = facos_deg(R[CHZ][CHZ]);
903 
904  return;
905 }
906 
907 // extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
908 void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
909  float *pfRhoDeg, float *pfChiDeg)
910 {
911  // calculate the roll angle -90.0 <= Phi <= 90.0 deg
912  if (R[CHZ][CHZ] == 0.0F)
913  {
914  if (R[CHX][CHZ] >= 0.0F)
915  {
916  // tan(phi) is -infinity
917  *pfPhiDeg = -90.0F;
918  }
919  else
920  {
921  // tan(phi) is +infinity
922  *pfPhiDeg = 90.0F;
923  }
924  }
925  else
926  {
927  // general case
928  *pfPhiDeg = fatan_deg(-R[CHX][CHZ] / R[CHZ][CHZ]);
929  }
930 
931  // first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
932  *pfTheDeg = fasin_deg(R[CHY][CHZ]);
933 
934  // use R[CHZ][CHZ]=cos(Phi)*cos(The) to correct the quadrant of The remembering
935  // cos(Phi) is non-negative so that cos(The) has the same sign as R[CHZ][CHZ].
936  if (R[CHZ][CHZ] < 0.0F)
937  {
938  // wrap The around +90 deg and -90 deg giving result 90 to 270 deg
939  *pfTheDeg = 180.0F - *pfTheDeg;
940  }
941 
942  // map the pitch angle The to the range -180.0 <= The < 180.0 deg
943  if (*pfTheDeg >= 180.0F)
944  {
945  *pfTheDeg -= 360.0F;
946  }
947 
948  // calculate the yaw angle Psi
949  if (*pfTheDeg == 90.0F)
950  {
951  // vertical upwards gimbal lock case: -270 <= Psi < 90 deg
952  *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) - *pfPhiDeg;
953  }
954  else if (*pfTheDeg == -90.0F)
955  {
956  // vertical downwards gimbal lock case: -270 <= Psi < 90 deg
957  *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) + *pfPhiDeg;
958  }
959  else
960  {
961  // general case: -180 <= Psi < 180 deg
962  *pfPsiDeg = fatan2_deg(-R[CHY][CHX], R[CHY][CHY]);
963 
964  // correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
965  if (fabsf(*pfTheDeg) >= 90.0F)
966  {
967  *pfPsiDeg += 180.0F;
968  }
969  }
970 
971  // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
972  if (*pfPsiDeg < 0.0F)
973  {
974  *pfPsiDeg += 360.0F;
975  }
976 
977  // check for any rounding error mapping small negative angle to 360 deg
978  if (*pfPsiDeg >= 360.0F)
979  {
980  *pfPsiDeg = 0.0F;
981  }
982 
983  // compute the compass angle Rho = 360 - Psi
984  *pfRhoDeg = 360.0F - *pfPsiDeg;
985 
986  // check for rounding errors mapping small negative angle to 360 deg and zero degree case
987  if (*pfRhoDeg >= 360.0F)
988  {
989  *pfRhoDeg = 0.0F;
990  }
991 
992  // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
993  *pfChiDeg = facos_deg(R[CHZ][CHZ]);
994 
995  return;
996 }
997 
998 // computes normalized rotation quaternion from a rotation vector (deg)
999 void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
1000 {
1001  float fetadeg; // rotation angle (deg)
1002  float fetarad; // rotation angle (rad)
1003  float fetarad2; // eta (rad)^2
1004  float fetarad4; // eta (rad)^4
1005  float sinhalfeta; // sin(eta/2)
1006  float fvecsq; // q1^2+q2^2+q3^2
1007  float ftmp; // scratch variable
1008 
1009  // compute the scaled rotation angle eta (deg) which can be both positve or negative
1010  fetadeg = fscaling * sqrtf(rvecdeg[CHX] * rvecdeg[CHX] + rvecdeg[CHY] * rvecdeg[CHY] + rvecdeg[CHZ] * rvecdeg[CHZ]);
1011  fetarad = fetadeg * FPIOVER180;
1012  fetarad2 = fetarad * fetarad;
1013 
1014  // calculate the sine and cosine using small angle approximations or exact
1015  // 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
1016  if (fetarad2 <= 0.02F)
1017  {
1018  // use MacLaurin series up to and including third order
1019  sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2);
1020  }
1021  else if (fetarad2 <= 0.06F)
1022  {
1023  // use MacLaurin series up to and including fifth order
1024  // 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
1025  fetarad4 = fetarad2 * fetarad2;
1026  sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4);
1027  }
1028  else
1029  {
1030  // use exact calculation
1031  sinhalfeta = (float)sinf(0.5F * fetarad);
1032  }
1033 
1034  // compute the vector quaternion components q1, q2, q3
1035  if (fetadeg != 0.0F)
1036  {
1037  // general case with non-zero rotation angle
1038  ftmp = fscaling * sinhalfeta / fetadeg;
1039  pq->q1 = rvecdeg[CHX] * ftmp; // q1 = nx * sin(eta/2)
1040  pq->q2 = rvecdeg[CHY] * ftmp; // q2 = ny * sin(eta/2)
1041  pq->q3 = rvecdeg[CHZ] * ftmp; // q3 = nz * sin(eta/2)
1042  }
1043  else
1044  {
1045  // zero rotation angle giving zero vector component
1046  pq->q1 = pq->q2 = pq->q3 = 0.0F;
1047  }
1048 
1049  // compute the scalar quaternion component q0 by explicit normalization
1050  // taking care to avoid rounding errors giving negative operand to sqrt
1051  fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3;
1052  if (fvecsq <= 1.0F)
1053  {
1054  // normal case
1055  pq->q0 = sqrtf(1.0F - fvecsq);
1056  }
1057  else
1058  {
1059  // rounding errors are present
1060  pq->q0 = 0.0F;
1061  }
1062 
1063  return;
1064 }
1065 
1066 // compute the orientation quaternion from a 3x3 rotation matrix
1067 void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
1068 {
1069  float fq0sq; // q0^2
1070  float recip4q0; // 1/4q0
1071 
1072  // the quaternion is not explicitly normalized in this function on the assumption that it
1073  // is supplied with a normalized rotation matrix. if the rotation matrix is normalized then
1074  // the quaternion will also be normalized even if the case of small q0
1075 
1076  // get q0^2 and q0
1077  fq0sq = 0.25F * (1.0F + R[CHX][CHX] + R[CHY][CHY] + R[CHZ][CHZ]);
1078  pq->q0 = sqrtf(fabsf(fq0sq));
1079 
1080  // normal case when q0 is not small meaning rotation angle not near 180 deg
1081  if (pq->q0 > SMALLQ0)
1082  {
1083  // calculate q1 to q3
1084  recip4q0 = 0.25F / pq->q0;
1085  pq->q1 = recip4q0 * (R[CHY][CHZ] - R[CHZ][CHY]);
1086  pq->q2 = recip4q0 * (R[CHZ][CHX] - R[CHX][CHZ]);
1087  pq->q3 = recip4q0 * (R[CHX][CHY] - R[CHY][CHX]);
1088  } // end of general case
1089  else
1090  {
1091  // special case of near 180 deg corresponds to nearly symmetric matrix
1092  // which is not numerically well conditioned for division by small q0
1093  // instead get absolute values of q1 to q3 from leading diagonal
1094  pq->q1 = sqrtf(fabsf(0.5F * (1.0F + R[CHX][CHX]) - fq0sq));
1095  pq->q2 = sqrtf(fabsf(0.5F * (1.0F + R[CHY][CHY]) - fq0sq));
1096  pq->q3 = sqrtf(fabsf(0.5F * (1.0F + R[CHZ][CHZ]) - fq0sq));
1097 
1098  // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms
1099  if ((R[CHY][CHZ] - R[CHZ][CHY]) < 0.0F) pq->q1 = -pq->q1;
1100  if ((R[CHZ][CHX] - R[CHX][CHZ]) < 0.0F) pq->q2 = -pq->q2;
1101  if ((R[CHX][CHY] - R[CHY][CHX]) < 0.0F) pq->q3 = -pq->q3;
1102  } // end of special case
1103 
1104  return;
1105 }
1106 
1107 // compute the rotation matrix from an orientation quaternion
1108 void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
1109 {
1110  float f2q;
1111  float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
1112  float f2q1q1, f2q1q2, f2q1q3;
1113  float f2q2q2, f2q2q3;
1114  float f2q3q3;
1115 
1116  // set f2q to 2*q0 and calculate products
1117  f2q = 2.0F * pq->q0;
1118  f2q0q0 = f2q * pq->q0;
1119  f2q0q1 = f2q * pq->q1;
1120  f2q0q2 = f2q * pq->q2;
1121  f2q0q3 = f2q * pq->q3;
1122  // set f2q to 2*q1 and calculate products
1123  f2q = 2.0F * pq->q1;
1124  f2q1q1 = f2q * pq->q1;
1125  f2q1q2 = f2q * pq->q2;
1126  f2q1q3 = f2q * pq->q3;
1127  // set f2q to 2*q2 and calculate products
1128  f2q = 2.0F * pq->q2;
1129  f2q2q2 = f2q * pq->q2;
1130  f2q2q3 = f2q * pq->q3;
1131  f2q3q3 = 2.0F * pq->q3 * pq->q3;
1132 
1133  // calculate the rotation matrix assuming the quaternion is normalized
1134  R[CHX][CHX] = f2q0q0 + f2q1q1 - 1.0F;
1135  R[CHX][CHY] = f2q1q2 + f2q0q3;
1136  R[CHX][CHZ] = f2q1q3 - f2q0q2;
1137  R[CHY][CHX] = f2q1q2 - f2q0q3;
1138  R[CHY][CHY] = f2q0q0 + f2q2q2 - 1.0F;
1139  R[CHY][CHZ] = f2q2q3 + f2q0q1;
1140  R[CHZ][CHX] = f2q1q3 + f2q0q2;
1141  R[CHZ][CHY] = f2q2q3 - f2q0q1;
1142  R[CHZ][CHZ] = f2q0q0 + f2q3q3 - 1.0F;
1143 
1144  return;
1145 }
1146 
1147 // computes rotation vector (deg) from rotation quaternion
1148 void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
1149 {
1150  float fetarad; // rotation angle (rad)
1151  float fetadeg; // rotation angle (deg)
1152  float sinhalfeta; // sin(eta/2)
1153  float ftmp; // scratch variable
1154 
1155  // calculate the rotation angle in the range 0 <= eta < 360 deg
1156  if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F))
1157  {
1158  // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg
1159  fetarad = 0.0F;
1160  fetadeg = 0.0F;
1161  }
1162  else
1163  {
1164  // general case returning 0 < eta < 360 deg
1165  fetarad = 2.0F * acosf(pq->q0);
1166  fetadeg = fetarad * F180OVERPI;
1167  }
1168 
1169  // map the rotation angle onto the range -180 deg <= eta < 180 deg
1170  if (fetadeg >= 180.0F)
1171  {
1172  fetadeg -= 360.0F;
1173  fetarad = fetadeg * FPIOVER180;
1174  }
1175 
1176  // calculate sin(eta/2) which will be in the range -1 to +1
1177  sinhalfeta = (float)sinf(0.5F * fetarad);
1178 
1179  // calculate the rotation vector (deg)
1180  if (sinhalfeta == 0.0F)
1181  {
1182  // the rotation angle eta is zero and the axis is irrelevant
1183  rvecdeg[CHX] = rvecdeg[CHY] = rvecdeg[CHZ] = 0.0F;
1184  }
1185  else
1186  {
1187  // general case with non-zero rotation angle
1188  ftmp = fetadeg / sinhalfeta;
1189  rvecdeg[CHX] = pq->q1 * ftmp;
1190  rvecdeg[CHY] = pq->q2 * ftmp;
1191  rvecdeg[CHZ] = pq->q3 * ftmp;
1192  }
1193 
1194  return;
1195 }
1196 
1197 // function low pass filters an orientation quaternion and computes virtual gyro rotation rate
1198 void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
1199 {
1200  // local variables
1201  struct fquaternion fdeltaq; // delta rotation quaternion
1202  float rvecdeg[3]; // rotation vector (deg)
1203  float ftmp; // scratch variable
1204 
1205  // set fdeltaqn to the delta rotation quaternion conjg(fLPq[n-1) . fqn
1206  fdeltaq = qconjgAxB(pLPq, pq);
1207  if (fdeltaq.q0 < 0.0F)
1208  {
1209  fdeltaq.q0 = -fdeltaq.q0;
1210  fdeltaq.q1 = -fdeltaq.q1;
1211  fdeltaq.q2 = -fdeltaq.q2;
1212  fdeltaq.q3 = -fdeltaq.q3;
1213  }
1214 
1215  // set ftmp to a scaled lpf value which equals flpf in the limit of small rotations (q0=1)
1216  // but which rises to 1 (all pass) as the delta rotation angle increases (q0 tends to zero)
1217  ftmp = flpf + (1.0F - flpf) * (1.0F - fdeltaq.q0);
1218 
1219  // scale the delta rotation by the corrected lpf value
1220  fdeltaq.q1 *= ftmp;
1221  fdeltaq.q2 *= ftmp;
1222  fdeltaq.q3 *= ftmp;
1223 
1224  // compute the scalar component q0
1225  ftmp = fdeltaq.q1 * fdeltaq.q1 + fdeltaq.q2 * fdeltaq.q2 + fdeltaq.q3 * fdeltaq.q3;
1226  if (ftmp <= 1.0F)
1227  {
1228  // normal case
1229  fdeltaq.q0 = sqrtf(1.0F - ftmp);
1230  }
1231  else
1232  {
1233  // rounding errors present so simply set scalar component to 0
1234  fdeltaq.q0 = 0.0F;
1235  }
1236 
1237  // calculate the delta rotation vector from fdeltaqn and the virtual gyro angular velocity (deg/s)
1238  fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg);
1239  ftmp = 1.0F / fdeltat;
1240  fOmega[CHX] = rvecdeg[CHX] * ftmp;
1241  fOmega[CHY] = rvecdeg[CHY] * ftmp;
1242  fOmega[CHZ] = rvecdeg[CHZ] * ftmp;
1243 
1244  // set LPq[n] = LPq[n-1] . deltaq[n]
1245  qAeqAxB(pLPq, &fdeltaq);
1246 
1247  // renormalize the low pass filtered quaternion to prevent error accumulation
1248  // the renormalization function ensures that q0 is non-negative
1249  fqAeqNormqA(pLPq);
1250 
1251  return;
1252 }
1253 
1254 // function compute the quaternion product qA * qB
1255 void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
1256 {
1257  pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3;
1258  pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2;
1259  pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1;
1260  pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0;
1261 
1262  return;
1263 }
1264 
1265 // function compute the quaternion product qA = qA * qB
1266 void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
1267 {
1268  struct fquaternion qProd;
1269 
1270  // perform the quaternion product
1271  qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3;
1272  qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2;
1273  qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1;
1274  qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0;
1275 
1276  // copy the result back into qA
1277  *pqA = qProd;
1278 
1279  return;
1280 }
1281 
1282 // function compute the quaternion product conjg(qA) * qB
1283 struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
1284 {
1285  struct fquaternion qProd;
1286 
1287  qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3;
1288  qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2;
1289  qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1;
1290  qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0;
1291 
1292  return qProd;
1293 }
1294 
1295 // function normalizes a rotation quaternion and ensures q0 is non-negative
1296 void fqAeqNormqA(struct fquaternion *pqA)
1297 {
1298  float fNorm; // quaternion Norm
1299 
1300  // calculate the quaternion Norm
1301  fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3);
1302  if (fNorm > CORRUPTQUAT)
1303  {
1304  // general case
1305  fNorm = 1.0F / fNorm;
1306  pqA->q0 *= fNorm;
1307  pqA->q1 *= fNorm;
1308  pqA->q2 *= fNorm;
1309  pqA->q3 *= fNorm;
1310  }
1311  else
1312  {
1313  // return with identity quaternion since the quaternion is corrupted
1314  pqA->q0 = 1.0F;
1315  pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1316  }
1317 
1318  // correct a negative scalar component if the function was called with negative q0
1319  if (pqA->q0 < 0.0F)
1320  {
1321  pqA->q0 = -pqA->q0;
1322  pqA->q1 = -pqA->q1;
1323  pqA->q2 = -pqA->q2;
1324  pqA->q3 = -pqA->q3;
1325  }
1326 
1327  return;
1328 }
1329 
1330 // set a quaternion to the unit quaternion
1331 void fqAeq1(struct fquaternion *pqA)
1332 {
1333  pqA->q0 = 1.0F;
1334  pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
1335 
1336  return;
1337 }
1338 
1339 // function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q
1340 // using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)}
1341 void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
1342 {
1343  float fuxv[3]; // vector product u x v
1344  float fsqrt1plusudotv; // sqrt(1 + u.v)
1345  float ftmp; // scratch
1346 
1347  // compute sqrt(1 + u.v) and scalar quaternion component q0 (valid for all angles including 180 deg)
1348  fsqrt1plusudotv = sqrtf(fabsf(1.0F + fu[CHX] * fv[CHX] + fu[CHY] * fv[CHY] + fu[CHZ] * fv[CHZ]));
1349  pfq->q0 = ONEOVERSQRT2 * fsqrt1plusudotv;
1350 
1351  // calculate the vector product uxv
1352  fuxv[CHX] = fu[CHY] * fv[CHZ] - fu[CHZ] * fv[CHY];
1353  fuxv[CHY] = fu[CHZ] * fv[CHX] - fu[CHX] * fv[CHZ];
1354  fuxv[CHZ] = fu[CHX] * fv[CHY] - fu[CHY] * fv[CHX];
1355 
1356  // compute the vector component of the quaternion
1357  if (fsqrt1plusudotv != 0.0F)
1358  {
1359  // general case where u and v are not anti-parallel where u.v=-1
1360  ftmp = ONEOVERSQRT2 / fsqrt1plusudotv;
1361  pfq->q1 = -fuxv[CHX] * ftmp;
1362  pfq->q2 = -fuxv[CHY] * ftmp;
1363  pfq->q3 = -fuxv[CHZ] * ftmp;
1364  }
1365  else
1366  {
1367  // degenerate case where u and v are anti-aligned and the 180 deg rotation quaternion is not uniquely defined.
1368  // first calculate the un-normalized vector component (the scalar component q0 is already set to zero)
1369  pfq->q1 = fu[CHY] - fu[CHZ];
1370  pfq->q2 = fu[CHZ] - fu[CHX];
1371  pfq->q3 = fu[CHX] - fu[CHY];
1372  // and normalize the quaternion for this case checking for fu[CHX]=fu[CHY]=fuCHZ] where q1=q2=q3=0.
1373  ftmp = sqrtf(fabsf(pfq->q1 * pfq->q1 + pfq->q2 * pfq->q2 + pfq->q3 * pfq->q3));
1374  if (ftmp != 0.0F)
1375  {
1376  // normal case where all three components of fu (and fv=-fu) are not equal
1377  ftmp = 1.0F / ftmp;
1378  pfq->q1 *= ftmp;
1379  pfq->q2 *= ftmp;
1380  pfq->q3 *= ftmp;
1381  }
1382  else
1383  {
1384  // final case where the three entries are equal but since the vectors are known to be normalized
1385  // the vector u must be 1/root(3)*{1, 1, 1} or -1/root(3)*{1, 1, 1} so simply set the
1386  // rotation vector to 1/root(2)*{1, -1, 0} to cover both cases
1387  pfq->q1 = ONEOVERSQRT2;
1388  pfq->q2 = -ONEOVERSQRT2;
1389  pfq->q3 = 0.0F;
1390  }
1391  }
1392 
1393  return;
1394 }
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:193
void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:688
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:853
#define ONEOVER3840
Definition: fusion_types.h:44
void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
Definition: orientation.c:1341
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:799
Definition: matrix.h:35
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1331
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:258
void f3DOFTiltNED(float fR[][3], float fGs[])
Definition: orientation.c:44
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:249
Definition: matrix.h:34
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:1148
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
Definition: orientation.c:1198
void fLeastSquareseCompassAndroid(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:576
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1255
signed short int int16
Definition: isf_types.h:73
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:394
void f3DOFTiltAndroid(float fR[][3], float fGs[])
Definition: orientation.c:110
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1296
#define F180OVERPI
Definition: fusion_types.h:38
#define CORRUPTQUAT
Definition: orientation.c:40
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:999
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1266
void f3DOFTiltWin8(float fR[][3], float fGs[])
Definition: orientation.c:119
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1283
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
Definition: matrix.c:353
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:96
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:326
#define SMALLQ0
Definition: orientation.c:39
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:1108
#define FPIOVER180
Definition: fusion_types.h:37
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:1067
float facos_deg(float x)
void fLeastSquareseCompassNED(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
Definition: orientation.c:463
#define ONEOVERSQRT2
Definition: fusion_types.h:45
#define ONEOVER48
Definition: fusion_types.h:42
float fatan2_deg(float y, float x)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:221
signed char int8
Definition: isf_types.h:72
Definition: matrix.h:33
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:908