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[]
	)

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

Here is the caller graph for this function:

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: