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