| ISF 2.1 Intelligent Sensing Framework for Kinetis with Processor Expert

- Main Page
- Modules
- Data Structures
- Files

- File List
- Globals

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