![]() |
ISF
2.1
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 |
#define | ONEOVER48 0.02083333333F |
#define | ONEOVER3840 0.0002604166667F |
Functions | |
void | f3DOFTiltNED (float fR[][3], float fGp[]) |
void | f3DOFTiltAndroid (float fR[][3], float fGp[]) |
void | f3DOFTiltWin8 (float fR[][3], float fGp[]) |
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 fGp[]) |
void | feCompassAndroid (float fR[][3], float *pfDelta, float fBc[], float fGp[]) |
void | feCompassWin8 (float fR[][3], float *pfDelta, float fBc[], float fGp[]) |
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 | fRotationVectorDegFromRotationMatrix (float R[][3], float rvecdeg[]) |
void | fRotationVectorDegFromQuaternion (struct fquaternion *pq, float rvecdeg[]) |
void | fLPFOrientationQuaternion (struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter) |
void | fLPFScalar (float *pfS, float *pfLPS, float flpf, int32 loopcounter) |
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) |
#define CORRUPTQUAT 0.001F |
Definition at line 41 of file orientation.c.
Referenced by fqAeqNormqA().
#define ONEOVER3840 0.0002604166667F |
Definition at line 45 of file orientation.c.
Referenced by fQuaternionFromRotationVectorDeg().
#define ONEOVER48 0.02083333333F |
Definition at line 44 of file orientation.c.
Referenced by fQuaternionFromRotationVectorDeg().
#define SMALLMODULUS 0.01F |
Definition at line 42 of file orientation.c.
Referenced by fRotationVectorDegFromRotationMatrix().
#define SMALLQ0 0.01F |
Definition at line 40 of file orientation.c.
Referenced by fQuaternionFromRotationMatrix().
void f3DOFMagnetometerMatrixAndroid | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 225 of file orientation.c.
References f3x3matrixAeqI(), X, Y, and Z.
Referenced by f3DOFMagnetometerMatrixWin8(), and fRun_3DOF_B_BASIC().
void f3DOFMagnetometerMatrixNED | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 197 of file orientation.c.
References f3x3matrixAeqI(), X, Y, and Z.
Referenced by fRun_3DOF_B_BASIC().
void f3DOFMagnetometerMatrixWin8 | ( | float | fR[][3], |
float | fBc[] | ||
) |
Definition at line 253 of file orientation.c.
References f3DOFMagnetometerMatrixAndroid().
Referenced by fRun_3DOF_B_BASIC().
void f3DOFTiltAndroid | ( | float | fR[][3], |
float | fGp[] | ||
) |
Definition at line 114 of file orientation.c.
References f3DOFTiltNED().
Referenced by fRun_3DOF_G_BASIC(), and fRun_6DOF_GY_KALMAN().
void f3DOFTiltNED | ( | float | fR[][3], |
float | fGp[] | ||
) |
Definition at line 48 of file orientation.c.
References f3x3matrixAeqI(), f3x3matrixAeqScalar(), X, Y, and Z.
Referenced by f3DOFTiltAndroid(), fRun_3DOF_G_BASIC(), and fRun_6DOF_GY_KALMAN().
void f3DOFTiltWin8 | ( | float | fR[][3], |
float | fGp[] | ||
) |
Definition at line 123 of file orientation.c.
References f3x3matrixAeqI(), f3x3matrixAeqScalar(), X, Y, and Z.
Referenced by fRun_3DOF_G_BASIC(), and fRun_6DOF_GY_KALMAN().
void fAndroidAnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 520 of file orientation.c.
References facos_deg(), fasin_deg(), fatan2_deg(), X, Y, and Z.
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 | fGp[] | ||
) |
Definition at line 330 of file orientation.c.
References f3x3matrixAeqI(), fasin_deg(), X, Y, and Z.
Referenced by fRun_6DOF_GB_BASIC(), and fRun_9DOF_GBY_KALMAN().
void feCompassNED | ( | float | fR[][3], |
float * | pfDelta, | ||
float | fBc[], | ||
float | fGp[] | ||
) |
Definition at line 262 of file orientation.c.
References f3x3matrixAeqI(), fasin_deg(), X, Y, and Z.
Referenced by fRun_6DOF_GB_BASIC(), and fRun_9DOF_GBY_KALMAN().
void feCompassWin8 | ( | float | fR[][3], |
float * | pfDelta, | ||
float | fBc[], | ||
float | fGp[] | ||
) |
Definition at line 398 of file orientation.c.
References f3x3matrixAeqI(), fasin_deg(), X, Y, and Z.
Referenced by fRun_6DOF_GB_BASIC(), and fRun_9DOF_GBY_KALMAN().
void fLPFOrientationQuaternion | ( | struct fquaternion * | pq, |
struct fquaternion * | pLPq, | ||
float | flpf, | ||
float | fdeltat, | ||
float | fOmega[], | ||
int32 | loopcounter | ||
) |
Definition at line 932 of file orientation.c.
References fqAeqNormqA(), fRotationVectorDegFromQuaternion(), fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, qAeqAxB(), qconjgAxB(), X, Y, and Z.
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), and fRun_6DOF_GB_BASIC().
void fLPFScalar | ( | float * | pfS, |
float * | pfLPS, | ||
float | flpf, | ||
int32 | loopcounter | ||
) |
Definition at line 1000 of file orientation.c.
Referenced by fRun_1DOF_P_BASIC(), and fRun_6DOF_GB_BASIC().
void fNEDAnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 466 of file orientation.c.
References facos_deg(), fasin_deg(), fatan2_deg(), X, Y, and Z.
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 1091 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fInit_3DOF_Y_BASIC(), fInit_6DOF_GY_KALMAN(), and fInit_9DOF_GBY_KALMAN().
void fqAeqNormqA | ( | struct fquaternion * | pqA | ) |
Definition at line 1056 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 734 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, SMALLQ0, X, Y, and Z.
Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), and fRun_9DOF_GBY_KALMAN().
void fQuaternionFromRotationVectorDeg | ( | struct fquaternion * | pq, |
const float | rvecdeg[], | ||
float | fscaling | ||
) |
Definition at line 666 of file orientation.c.
References FDEGTORAD, ONEOVER3840, ONEOVER48, fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, X, Y, and Z.
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 775 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, X, Y, and Z.
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 fRotationVectorDegFromQuaternion | ( | struct fquaternion * | pq, |
float | rvecdeg[] | ||
) |
Definition at line 882 of file orientation.c.
References FDEGTORAD, FRADTODEG, fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, X, Y, and Z.
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 fRotationVectorDegFromRotationMatrix | ( | float | R[][3], |
float | rvecdeg[] | ||
) |
Definition at line 813 of file orientation.c.
References FRADTODEG, SMALLMODULUS, X, Y, and Z.
void fWin8AnglesDegFromRotationMatrix | ( | float | R[][3], |
float * | pfPhiDeg, | ||
float * | pfTheDeg, | ||
float * | pfPsiDeg, | ||
float * | pfRhoDeg, | ||
float * | pfChiDeg | ||
) |
Definition at line 575 of file orientation.c.
References facos_deg(), fasin_deg(), fatan2_deg(), fatan_deg(), X, Y, and Z.
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 1026 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 1015 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 1043 of file orientation.c.
References fquaternion::q0, fquaternion::q1, fquaternion::q2, and fquaternion::q3.
Referenced by fLPFOrientationQuaternion().