ISF  2.1
Intelligent Sensing Framework for Kinetis with Processor Expert
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
orientation.c File Reference
#include "math.h"
#include "math_constants.h"
#include "matrix.h"
#include "fusion_types.h"
#include "approximations.h"
#include "orientation.h"
Include dependency graph for orientation.c:

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)
 

Macro Definition Documentation

#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().

Function Documentation

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

void f3DOFMagnetometerMatrixWin8 ( float  fR[][3],
float  fBc[] 
)

Definition at line 253 of file orientation.c.

References f3DOFMagnetometerMatrixAndroid().

Referenced by fRun_3DOF_B_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

void fRotationVectorDegFromQuaternion ( struct fquaternion pq,
float  rvecdeg[] 
)
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().

Here is the call graph for this function:

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function:

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().

Here is the caller graph for this function: