Functions to convert between various orientation representations. More...

#include "stdio.h"
#include "math.h"
#include "stdlib.h"
#include "time.h"
#include "string.h"
#include "sensor_fusion.h"
#include "orientation.h"
#include "fusion.h"
#include "matrix.h"
#include "approximations.h"

Include dependency graph for orientation.c:

Go to the source code of this file.

Macros
#define	SMALLQ0 1E-4F

#define	CORRUPTQUAT 0.001F

#define	SMALLMODULUS 0.01F

Functions
void	f3DOFMagnetometerMatrixNED (float fR[][3], float fBc[])
	Aerospace NED magnetometer 3DOF flat eCompass function, computing rotation matrix fR. More...

void	f3DOFMagnetometerMatrixAndroid (float fR[][3], float fBc[])
	Android magnetometer 3DOF flat eCompass function, computing rotation matrix fR. More...

void	f3DOFMagnetometerMatrixWin8 (float fR[][3], float fBc[])
	Windows 8 magnetometer 3DOF flat eCompass function, computing rotation matrix fR. More...

void	feCompassNED (float fR[][3], float pfDelta, float pfsinDelta, float pfcosDelta, float fBc[], float fGc[], float pfmodBc, float *pfmodGc)
	NED: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta. More...

void	feCompassAndroid (float fR[][3], float pfDelta, float pfsinDelta, float pfcosDelta, float fBc[], float fGc[], float pfmodBc, float *pfmodGc)
	Android: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta. More...

void	feCompassWin8 (float fR[][3], float pfDelta, float pfsinDelta, float pfcosDelta, float fBc[], float fGc[], float pfmodBc, float *pfmodGc)
	Win8: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta. More...

void	fNEDAnglesDegFromRotationMatrix (float R[][3], float pfPhiDeg, float pfTheDeg, float pfPsiDeg, float pfRhoDeg, float *pfChiDeg)
	extract the NED angles in degrees from the NED rotation matrix More...

void	fAndroidAnglesDegFromRotationMatrix (float R[][3], float pfPhiDeg, float pfTheDeg, float pfPsiDeg, float pfRhoDeg, float *pfChiDeg)
	extract the Android angles in degrees from the Android rotation matrix More...

void	fWin8AnglesDegFromRotationMatrix (float R[][3], float pfPhiDeg, float pfTheDeg, float pfPsiDeg, float pfRhoDeg, float *pfChiDeg)
	extract the Windows 8 angles in degrees from the Windows 8 rotation matrix More...

void	fQuaternionFromRotationVectorDeg (Quaternion *pq, const float rvecdeg[], float fscaling)
	computes normalized rotation quaternion from a rotation vector (deg) More...

void	fQuaternionFromRotationMatrix (float R[][3], Quaternion *pq)
	compute the orientation quaternion from a 3x3 rotation matrix More...

void	fRotationMatrixFromQuaternion (float R[][3], const Quaternion *pq)
	compute the rotation matrix from an orientation quaternion More...

void	fRotationVectorDegFromQuaternion (Quaternion *pq, float rvecdeg[])
	computes rotation vector (deg) from rotation quaternion More...

void	fLPFOrientationQuaternion (Quaternion pq, Quaternion pLPq, float flpf, float fdeltat, float fOmega[])
	function low pass filters an orientation quaternion and computes virtual gyro rotation rate More...

void	qAeqBxC (Quaternion pqA, const Quaternion pqB, const Quaternion *pqC)
	function compute the quaternion product qB * qC More...

void	qAeqAxB (Quaternion pqA, const Quaternion pqB)
	function compute the quaternion product qA = qA * qB More...

Quaternion	qconjgAxB (const Quaternion pqA, const Quaternion pqB)
	function compute the quaternion product conjg(qA) * qB More...

void	fqAeqNormqA (Quaternion *pqA)
	function normalizes a rotation quaternion and ensures q0 is non-negative More...

void	fqAeq1 (Quaternion *pqA)
	set a quaternion to the unit quaternion More...

void	fveqconjgquq (Quaternion *pfq, float fu[], float fv[])

Detailed Description

Functions to convert between various orientation representations.

Functions to convert between various orientation representations. Also includes functions for manipulating quaternions.

Definition in file orientation.c.

Macro Definition Documentation

◆ CORRUPTQUAT

#define CORRUPTQUAT 0.001F

Definition at line 36 of file orientation.c.

Referenced by fqAeqNormqA().

◆ SMALLMODULUS

#define SMALLMODULUS 0.01F

Definition at line 37 of file orientation.c.

◆ SMALLQ0

#define SMALLQ0 1E-4F

Definition at line 35 of file orientation.c.

Referenced by fQuaternionFromRotationMatrix().

Function Documentation

◆ f3DOFMagnetometerMatrixAndroid()

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

Android magnetometer 3DOF flat eCompass function, computing rotation matrix fR.

Parameters

fR	computed rotation matrix (output)
fBc	calibrated magnetometer reading (input)

Definition at line 229 of file orientation.c.

References CHX, CHY, CHZ, and f3x3matrixAeqI().

Referenced by f3DOFMagnetometerMatrixWin8(), fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ f3DOFMagnetometerMatrixNED()

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

Aerospace NED magnetometer 3DOF flat eCompass function, computing rotation matrix fR.

Parameters

fR	computed rotation matrix (output)
fBc	calibrated magnetometer reading (input)

Definition at line 199 of file orientation.c.

References CHX, CHY, CHZ, and f3x3matrixAeqI().

Referenced by fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ f3DOFMagnetometerMatrixWin8()

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

Windows 8 magnetometer 3DOF flat eCompass function, computing rotation matrix fR.

Parameters

fR	computed rotation matrix (output)
fBc	calibrated magnetometer reading (input)

Definition at line 259 of file orientation.c.

References f3DOFMagnetometerMatrixAndroid().

Referenced by fInit_3DOF_B_BASIC(), and fRun_3DOF_B_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fAndroidAnglesDegFromRotationMatrix()

void fAndroidAnglesDegFromRotationMatrix	(	float	R[][3],
		float *	pfPhiDeg,
		float *	pfTheDeg,
		float *	pfPsiDeg,
		float *	pfRhoDeg,
		float *	pfChiDeg
	)

extract the Android angles in degrees from the Android rotation matrix

Parameters

R	rotation matrix input
pfPhiDeg	the roll angle -90.0 <= Phi <= 90.0 deg
pfTheDeg	the pitch angle -180.0 <= The < 180.0 deg
pfPsiDeg	yaw angle Psi with range 0.0 <= Psi < 360.0 deg
pfRhoDeg	the compass heading angle Rho equals the yaw angle Psi
pfChiDeg	the tilt angle from vertical Chi (0 <= Chi <= 180 deg)

Definition at line 548 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(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ feCompassAndroid()

void feCompassAndroid	(	float	fR[][3],
		float *	pfDelta,
		float *	pfsinDelta,
		float *	pfcosDelta,
		float	fBc[],
		float	fGc[],
		float *	pfmodBc,
		float *	pfmodGc
	)

Android: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta.

Parameters

fR	computed rotation matrix (output)
pfDelta	magnetic inclination angle (output)
pfsinDelta	sin of the inclination angle
pfcosDelta	cos of the inclination angle
fBc	calibrated magnetometer reading (input)
fGc	calibrated accelerometer input vector (input)
pfmodBc	modulus of the calibrated magnetic vector
pfmodGc	modulus of the calibrated accelerometer vector

Definition at line 343 of file orientation.c.

References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().

Referenced by fInit_6DOF_GB_BASIC(), fInit_9DOF_GBY_KALMAN(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ feCompassNED()

void feCompassNED	(	float	fR[][3],
		float *	pfDelta,
		float *	pfsinDelta,
		float *	pfcosDelta,
		float	fBc[],
		float	fGc[],
		float *	pfmodBc,
		float *	pfmodGc
	)

NED: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta.

Parameters

fR	computed rotation matrix (output)
pfDelta	magnetic inclination angle (output)
pfsinDelta	sin of the inclination angle
pfcosDelta	cos of the inclination angle
fBc	calibrated magnetometer vector (input)
fGc	calibrated accelerometer input vector (input)
pfmodBc	modulus of the calibrated magnetic vector
pfmodGc	modulus of the calibrated accelerometer vector

Definition at line 270 of file orientation.c.

References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().

Referenced by fInit_6DOF_GB_BASIC(), fInit_9DOF_GBY_KALMAN(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ feCompassWin8()

void feCompassWin8	(	float	fR[][3],
		float *	pfDelta,
		float *	pfsinDelta,
		float *	pfcosDelta,
		float	fBc[],
		float	fGc[],
		float *	pfmodBc,
		float *	pfmodGc
	)

Win8: basic 6DOF e-Compass function, computing rotation matrix fR and magnetic inclination angle fDelta.

Parameters

fR	computed rotation matrix (output)
pfDelta	magnetic inclination angle (output)
pfsinDelta	sin of the inclination angle
pfcosDelta	cos of the inclination angle
fBc	calibrated magnetometer reading (input)
fGc	calibrated accelerometer input vector (input)
pfmodBc	modulus of the calibrated magnetic vector
pfmodGc	modulus of the calibrated accelerometer vector

Definition at line 417 of file orientation.c.

References CHX, CHY, CHZ, f3x3matrixAeqI(), and fasin_deg().

Referenced by fInit_6DOF_GB_BASIC(), fInit_9DOF_GBY_KALMAN(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fLPFOrientationQuaternion()

void fLPFOrientationQuaternion	(	Quaternion *	pq,
		Quaternion *	pLPq,
		float	flpf,
		float	fdeltat,
		float	fOmega[]
	)

function low pass filters an orientation quaternion and computes virtual gyro rotation rate

Definition at line 896 of file orientation.c.

References CHX, CHY, CHZ, fqAeqNormqA(), fRotationVectorDegFromQuaternion(), Quaternion::q0, Quaternion::q1, Quaternion::q2, Quaternion::q3, qAeqAxB(), and qconjgAxB().

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:

◆ fNEDAnglesDegFromRotationMatrix()

void fNEDAnglesDegFromRotationMatrix	(	float	R[][3],
		float *	pfPhiDeg,
		float *	pfTheDeg,
		float *	pfPsiDeg,
		float *	pfRhoDeg,
		float *	pfChiDeg
	)

extract the NED angles in degrees from the NED rotation matrix

Parameters

R	rotation matrix input
pfPhiDeg	output: the roll angle range -180.0 <= Phi < 180.0 deg
pfTheDeg	output: the pitch angle -90.0 <= Theta <= 90.0 deg
pfPsiDeg	output: the yaw (compass) angle 0.0 <= Psi < 360.0 deg
pfRhoDeg	output: For NED, the compass heading Rho equals the yaw angle Psi
pfChiDeg	output: the tilt angle from vertical Chi (0 <= Chi <= 180 deg)

Definition at line 492 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(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fqAeq1()

void fqAeq1 ( Quaternion * pqA )

set a quaternion to the unit quaternion

Definition at line 1018 of file orientation.c.

References Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fInit_3DOF_Y_BASIC().

Here is the caller graph for this function:

◆ fqAeqNormqA()

void fqAeqNormqA ( Quaternion * pqA )

function normalizes a rotation quaternion and ensures q0 is non-negative

Definition at line 983 of file orientation.c.

References CORRUPTQUAT, Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fLPFOrientationQuaternion(), fQuaternionFromRotationMatrix(), fRun_3DOF_Y_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function:

◆ fQuaternionFromRotationMatrix()

void fQuaternionFromRotationMatrix	(	float	R[][3],
		Quaternion *	pq
	)

compute the orientation quaternion from a 3x3 rotation matrix

Parameters

R	Rotation matrix (input)
pq	Quaternion (output)

Definition at line 765 of file orientation.c.

References CHX, CHY, CHZ, fqAeqNormqA(), Quaternion::q0, Quaternion::q1, Quaternion::q2, Quaternion::q3, and SMALLQ0.

Referenced by fInit_3DOF_B_BASIC(), fInit_3DOF_G_BASIC(), fInit_6DOF_GB_BASIC(), fInit_6DOF_GY_KALMAN(), fInit_9DOF_GBY_KALMAN(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ fQuaternionFromRotationVectorDeg()

void fQuaternionFromRotationVectorDeg	(	Quaternion *	pq,
		const float	rvecdeg[],
		float	fscaling
	)

computes normalized rotation quaternion from a rotation vector (deg)

Parameters

pq	quaternion (output)
rvecdeg	rotation vector in degrees
fscaling	delta Time

Definition at line 697 of file orientation.c.

References CHX, CHY, CHZ, FPIOVER180, ONEOVER3840, ONEOVER48, Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fRun_3DOF_Y_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function:

◆ fRotationMatrixFromQuaternion()

void fRotationMatrixFromQuaternion	(	float	R[][3],
		const Quaternion *	pq
	)

compute the rotation matrix from an orientation quaternion

Parameters

R	Rotation matrix (output)
pq	Quaternion (input)

Definition at line 806 of file orientation.c.

References CHX, CHY, CHZ, Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function:

◆ fRotationVectorDegFromQuaternion()

void fRotationVectorDegFromQuaternion	(	Quaternion *	pq,
		float	rvecdeg[]
	)

computes rotation vector (deg) from rotation quaternion

Parameters

pq	quaternion (input)
rvecdeg	rotation vector in degrees (output)

Definition at line 846 of file orientation.c.

References CHX, CHY, CHZ, F180OVERPI, FPIOVER180, Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fLPFOrientationQuaternion(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), and fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function:

◆ fveqconjgquq()

void fveqconjgquq	(	Quaternion *	pfq,
		float	fu[],
		float	fv[]
	)

function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)}

Definition at line 1028 of file orientation.c.

References CHX, CHY, CHZ, ONEOVERSQRT2, Quaternion::q0, Quaternion::q1, Quaternion::q2, and Quaternion::q3.

Referenced by fRun_6DOF_GY_KALMAN().

Here is the caller graph for this function:

◆ fWin8AnglesDegFromRotationMatrix()

void fWin8AnglesDegFromRotationMatrix	(	float	R[][3],
		float *	pfPhiDeg,
		float *	pfTheDeg,
		float *	pfPsiDeg,
		float *	pfRhoDeg,
		float *	pfChiDeg
	)

extract the Windows 8 angles in degrees from the Windows 8 rotation matrix

Parameters

R	rotation matrix input
pfPhiDeg	the roll angle -90.0 <= Phi <= 90.0 deg
pfTheDeg	pitch angle Theta in the range -180.0 <= The < 180.0 deg
pfPsiDeg	yaw angle Psi in range 0.0 <= Psi < 360.0 deg
pfRhoDeg	the compass angle Rho = 360 - Psi
pfChiDeg	tilt angle from vertical Chi (0 <= Chi <= 180 deg)