![]() |
ISF
2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
|
#include "math.h"
#include "math_constants.h"
#include "matrix.h"
#include "fusion_types.h"
#include "approximations.h"
#include "orientation.h"
Go to the source code of this file.
Macros | |
#define | SMALLQ0 0.01F |
#define | CORRUPTQUAT 0.001F |
#define | SMALLMODULUS 0.01F |
Functions | |
void | f3DOFTiltNED (float fR[][3], float fGs[]) |
void | f3DOFTiltAndroid (float fR[][3], float fGs[]) |
void | f3DOFTiltWin8 (float fR[][3], float fGs[]) |
void | f3DOFMagnetometerMatrixNED (float fR[][3], float fBc[]) |
void | f3DOFMagnetometerMatrixAndroid (float fR[][3], float fBc[]) |
void | f3DOFMagnetometerMatrixWin8 (float fR[][3], float fBc[]) |
void | feCompassNED (float fR[][3], float *pfDelta, float fBc[], float fGs[]) |
void | feCompassAndroid (float fR[][3], float *pfDelta, float fBc[], float fGs[]) |
void | feCompassWin8 (float fR[][3], float *pfDelta, float fBc[], float fGs[]) |
void | fLeastSquareseCompassNED (struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa) |
void | fLeastSquareseCompassAndroid (struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa) |
void | fLeastSquareseCompassWin8 (struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa) |
void | fNEDAnglesDegFromRotationMatrix (float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg) |
void | fAndroidAnglesDegFromRotationMatrix (float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg) |
void | fWin8AnglesDegFromRotationMatrix (float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg) |
void | fQuaternionFromRotationVectorDeg (struct fquaternion *pq, const float rvecdeg[], float fscaling) |
void | fQuaternionFromRotationMatrix (float R[][3], struct fquaternion *pq) |
void | fRotationMatrixFromQuaternion (float R[][3], const struct fquaternion *pq) |
void | fRotationVectorDegFromQuaternion (struct fquaternion *pq, float rvecdeg[]) |
void | fLPFOrientationQuaternion (struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[]) |
void | qAeqBxC (struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC) |
void | qAeqAxB (struct fquaternion *pqA, const struct fquaternion *pqB) |
struct fquaternion | qconjgAxB (const struct fquaternion *pqA, const struct fquaternion *pqB) |
void | fqAeqNormqA (struct fquaternion *pqA) |
void | fqAeq1 (struct fquaternion *pqA) |
void | fveqconjgquq (struct fquaternion *pfq, float fu[], float fv[]) |
#define CORRUPTQUAT 0.001F |
Definition at line 40 of file orientation.c.
Referenced by fqAeqNormqA().
#define SMALLMODULUS 0.01F |
Definition at line 41 of file orientation.c.
#define SMALLQ0 0.01F |
Definition at line 39 of file orientation.c.
Referenced by fQuaternionFromRotationMatrix().
void f3DOFMagnetometerMatrixAndroid | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 221 of file orientation.c.
References CHX, CHY, CHZ, and f3x3matrixAeqI().
Referenced by f3DOFMagnetometerMatrixWin8(), fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().
void f3DOFMagnetometerMatrixNED | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 193 of file orientation.c.
References CHX, CHY, CHZ, and f3x3matrixAeqI().
Referenced by fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().
void f3DOFMagnetometerMatrixWin8 | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 249 of file orientation.c.
References f3DOFMagnetometerMatrixAndroid().
Referenced by fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().
void f3DOFTiltAndroid | ( | float | fR[][3], |
float | fGs[] | ||
) |
Definition at line 110 of file orientation.c.
References f3DOFTiltNED().
Referenced by fInit_3DOF_G_BASIC(), fInit_6DOF_GY_KALMAN(), and fRun_3DOF_G_BASIC().
void f3DOFTiltNED | ( | float | fR[][3], |
float | fGs[] | ||
) |
Definition at line 44 of file orientation.c.
References CHX, CHY, CHZ, f3x3matrixAeqI(), and f3x3matrixAeqScalar().
Referenced by f3DOFTiltAndroid(), fInit_3DOF_G_BASIC(), fInit_6DOF_GY_KALMAN(), and fRun_3DOF_G_BASIC().
void f3DOFTiltWin8 | ( | float | fR[][3], |
float | fGs[] | ||
) |
Definition at line 119 of file orientation.c.
References CHX, CHY, CHZ, f3x3matrixAeqI(), and f3x3matrixAeqScalar().
Referenced by fInit_3DOF_G_BASIC(), fInit_6DOF_GY_KALMAN(), and fRun_3DOF_G_BASIC().
void fAndroidAnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 853 of file orientation.c.
References CHX, CHY, CHZ, facos_deg(), fasin_deg(), and fatan2_deg().
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void feCompassAndroid | ( | float | fR[][3], |
float * | pfDelta, | ||
float | fBc[], | ||
float | fGs[] | ||
) |
Definition at line 326 of file orientation.c.
References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().
Referenced by fInit_6DOF_GB_BASIC(), and fRun_6DOF_GB_BASIC().
void feCompassNED | ( | float | fR[][3], |
float * | pfDelta, | ||
float | fBc[], | ||
float | fGs[] | ||
) |
Definition at line 258 of file orientation.c.
References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().
Referenced by fInit_6DOF_GB_BASIC(), and fRun_6DOF_GB_BASIC().
void feCompassWin8 | ( | float | fR[][3], |
float * | pfDelta, | ||
float | fBc[], | ||
float | fGs[] | ||
) |
Definition at line 394 of file orientation.c.
References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().
Referenced by fInit_6DOF_GB_BASIC(), and fRun_6DOF_GB_BASIC().
void fLeastSquareseCompassAndroid | ( | struct fquaternion * | pfq, |
float | fB, | ||
float | fDelta, | ||
float | fsinDelta, | ||
float | fcosDelta, | ||
float * | pfDelta6DOF, | ||
float | fBc[], | ||
float | fGs[], | ||
float * | pfQvBQd, | ||
float * | pfQvGQa | ||
) |
Definition at line 576 of file orientation.c.
References CHX, CHY, CHZ, eigencompute4(), fasin_deg(), fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fLeastSquareseCompassNED | ( | struct fquaternion * | pfq, |
float | fB, | ||
float | fDelta, | ||
float | fsinDelta, | ||
float | fcosDelta, | ||
float * | pfDelta6DOF, | ||
float | fBc[], | ||
float | fGs[], | ||
float * | pfQvBQd, | ||
float * | pfQvGQa | ||
) |
Definition at line 463 of file orientation.c.
References CHX, CHY, CHZ, eigencompute4(), fasin_deg(), fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fLeastSquareseCompassWin8 | ( | struct fquaternion * | pfq, |
float | fB, | ||
float | fDelta, | ||
float | fsinDelta, | ||
float | fcosDelta, | ||
float * | pfDelta6DOF, | ||
float | fBc[], | ||
float | fGs[], | ||
float * | pfQvBQd, | ||
float * | pfQvGQa | ||
) |
Definition at line 688 of file orientation.c.
References CHX, CHY, CHZ, eigencompute4(), fasin_deg(), fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fLPFOrientationQuaternion | ( | struct fquaternion * | pq, |
struct fquaternion * | pLPq, | ||
float | flpf, | ||
float | fdeltat, | ||
float | fOmega[] | ||
) |
Definition at line 1198 of file orientation.c.
References CHX, CHY, CHZ, fqAeqNormqA(), fRotationVectorDegFromQuaternion(), fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, qAeqAxB(), and qconjgAxB().
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), and fRun_6DOF_GB_BASIC().
void fNEDAnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 799 of file orientation.c.
References CHX, CHY, CHZ, facos_deg(), fasin_deg(), and fatan2_deg().
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fqAeq1 | ( | struct fquaternion * | pqA | ) |
Definition at line 1331 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_3DOF_Y_BASIC().
void fqAeqNormqA | ( | struct fquaternion * | pqA | ) |
Definition at line 1296 of file orientation.c.
References CORRUPTQUAT, fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fLPFOrientationQuaternion(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fQuaternionFromRotationMatrix | ( | float | R[][3], |
struct fquaternion * | pq | ||
) |
Definition at line 1067 of file orientation.c.
References CHX, CHY, CHZ, fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, and SMALLQ0.
Referenced by fInit_3DOF_B_BASIC(), fInit_3DOF_G_BASIC(), fInit_6DOF_GB_BASIC(), fInit_6DOF_GY_KALMAN(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), and fRun_6DOF_GB_BASIC().
void fQuaternionFromRotationVectorDeg | ( | struct fquaternion * | pq, |
const float | rvecdeg[], | ||
float | fscaling | ||
) |
Definition at line 999 of file orientation.c.
References CHX, CHY, CHZ, FPIOVER180, ONEOVER3840, ONEOVER48, fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fRun_3DOF_Y_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fRotationMatrixFromQuaternion | ( | float | R[][3], |
const struct fquaternion * | pq | ||
) |
Definition at line 1108 of file orientation.c.
References CHX, CHY, CHZ, fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_9DOF_GBY_KALMAN(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fRotationVectorDegFromQuaternion | ( | struct fquaternion * | pq, |
float | rvecdeg[] | ||
) |
Definition at line 1148 of file orientation.c.
References CHX, CHY, CHZ, F180OVERPI, FPIOVER180, fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fLPFOrientationQuaternion(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fveqconjgquq | ( | struct fquaternion * | pfq, |
float | fu[], | ||
float | fv[] | ||
) |
Definition at line 1341 of file orientation.c.
References CHX, CHY, CHZ, ONEOVERSQRT2, fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fWin8AnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 908 of file orientation.c.
References CHX, CHY, CHZ, facos_deg(), fasin_deg(), fatan2_deg(), and fatan_deg().
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void qAeqAxB | ( | struct fquaternion * | pqA, |
const struct fquaternion * | pqB | ||
) |
Definition at line 1266 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fLPFOrientationQuaternion(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void qAeqBxC | ( | struct fquaternion * | pqA, |
const struct fquaternion * | pqB, | ||
const struct fquaternion * | pqC | ||
) |
Definition at line 1255 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
struct fquaternion qconjgAxB | ( | const struct fquaternion * | pqA, |
const struct fquaternion * | pqB | ||
) |
Definition at line 1283 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fLPFOrientationQuaternion().