| ISF 2.2 rev 5 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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
252 f3DOFMagnetometerMatrixAndroid(fR, fBc);
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 }
f3DOFMagnetometerMatrixNED
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
Definition: orientation.c:193
fLeastSquareseCompassWin8
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
fatan_deg
float fatan_deg(float x)
Definition: approximations.c:64
fasin_deg
float fasin_deg(float x)
Definition: approximations.c:35
fAndroidAnglesDegFromRotationMatrix
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:853
ONEOVER3840
#define ONEOVER3840
Definition: fusion_types.h:44
fveqconjgquq
void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
Definition: orientation.c:1341
orientation.h
math_constants.h
fNEDAnglesDegFromRotationMatrix
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:799
CHZ
Definition: matrix.h:35
fquaternion::q1
float q1
Definition: fusion_types.h:61
fqAeq1
void fqAeq1(struct fquaternion *pqA)
Definition: orientation.c:1331
feCompassNED
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:258
f3DOFTiltNED
void f3DOFTiltNED(float fR[][3], float fGs[])
Definition: orientation.c:44
f3DOFMagnetometerMatrixWin8
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
Definition: orientation.c:249
fquaternion
Definition: fusion_types.h:58
CHY
Definition: matrix.h:34
fRotationVectorDegFromQuaternion
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
Definition: orientation.c:1148
fLPFOrientationQuaternion
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
Definition: orientation.c:1198
fLeastSquareseCompassAndroid
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
qAeqBxC
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
Definition: orientation.c:1255
int16
signed short int int16
Definition: isf_types.h:73
approximations.h
feCompassWin8
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:394
f3DOFTiltAndroid
void f3DOFTiltAndroid(float fR[][3], float fGs[])
Definition: orientation.c:110
matrix.h
fqAeqNormqA
void fqAeqNormqA(struct fquaternion *pqA)
Definition: orientation.c:1296
F180OVERPI
#define F180OVERPI
Definition: fusion_types.h:38
CORRUPTQUAT
#define CORRUPTQUAT
Definition: orientation.c:40
fQuaternionFromRotationVectorDeg
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
Definition: orientation.c:999
fusion_types.h
qAeqAxB
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1266
f3DOFTiltWin8
void f3DOFTiltWin8(float fR[][3], float fGs[])
Definition: orientation.c:119
fquaternion::q2
float q2
Definition: fusion_types.h:62
f3x3matrixAeqI
void f3x3matrixAeqI(float A[][3])
Definition: matrix.c:36
qconjgAxB
struct fquaternion qconjgAxB(const struct fquaternion *pqA, const struct fquaternion *pqB)
Definition: orientation.c:1283
eigencompute4
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
Definition: matrix.c:353
fquaternion::q0
float q0
Definition: fusion_types.h:60
f3x3matrixAeqScalar
void f3x3matrixAeqScalar(float A[][3], float Scalar)
Definition: matrix.c:96
feCompassAndroid
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
Definition: orientation.c:326
SMALLQ0
#define SMALLQ0
Definition: orientation.c:39
fRotationMatrixFromQuaternion
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
Definition: orientation.c:1108
fquaternion::q3
float q3
Definition: fusion_types.h:63
FPIOVER180
#define FPIOVER180
Definition: fusion_types.h:37
fQuaternionFromRotationMatrix
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
Definition: orientation.c:1067
facos_deg
float facos_deg(float x)
Definition: approximations.c:49
fLeastSquareseCompassNED
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
ONEOVERSQRT2
#define ONEOVERSQRT2
Definition: fusion_types.h:45
ONEOVER48
#define ONEOVER48
Definition: fusion_types.h:42
fatan2_deg
float fatan2_deg(float y, float x)
Definition: approximations.c:115
f3DOFMagnetometerMatrixAndroid
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
Definition: orientation.c:221
int8
signed char int8
Definition: isf_types.h:72
CHX
Definition: matrix.h:33
fWin8AnglesDegFromRotationMatrix
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
Definition: orientation.c:908
- © Freescale Semiconductor, Inc. 2016. All Rights Reserved.