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

#include <math.h>
#include "sensor_fusion.h"
#include "orientation.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.

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

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

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.

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.

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.

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

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

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

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

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

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

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

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

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

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

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

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

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

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 31 of file orientation.c.

◆ SMALLMODULUS

#define SMALLMODULUS 0.01F

Definition at line 32 of file orientation.c.

◆ SMALLQ0

#define SMALLQ0 1E-4F

Definition at line 30 of file orientation.c.

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 224 of file orientation.c.

◆ 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 194 of file orientation.c.

◆ 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 254 of file orientation.c.

◆ 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 543 of file orientation.c.

◆ 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 338 of file orientation.c.

◆ 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 265 of file orientation.c.

◆ 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 412 of file orientation.c.

◆ 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 891 of file orientation.c.

◆ 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 487 of file orientation.c.

◆ fqAeq1()

void fqAeq1 ( Quaternion * pqA )

set a quaternion to the unit quaternion

Definition at line 1013 of file orientation.c.

◆ fqAeqNormqA()

void fqAeqNormqA ( Quaternion * pqA )

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

Definition at line 978 of file orientation.c.

◆ 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 760 of file orientation.c.

◆ 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 692 of file orientation.c.

◆ 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 801 of file orientation.c.

◆ 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 841 of file orientation.c.

◆ 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 1023 of file orientation.c.

◆ 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)

Definition at line 600 of file orientation.c.

◆ qAeqAxB()

void qAeqAxB	(	Quaternion *	pqA,
		const Quaternion *	pqB
	)

function compute the quaternion product qA = qA * qB

Definition at line 948 of file orientation.c.

◆ qAeqBxC()

void qAeqBxC	(	Quaternion *	pqA,
		const Quaternion *	pqB,
		const Quaternion *	pqC
	)

function compute the quaternion product qB * qC

Definition at line 937 of file orientation.c.

◆ qconjgAxB()

Quaternion qconjgAxB	(	const Quaternion *	pqA,
		const Quaternion *	pqB
	)

function compute the quaternion product conjg(qA) * qB

Definition at line 965 of file orientation.c.

Macros

Functions

Detailed Description

Macro Definition Documentation

◆ CORRUPTQUAT

◆ SMALLMODULUS

◆ SMALLQ0

Function Documentation

◆ f3DOFMagnetometerMatrixAndroid()

◆ f3DOFMagnetometerMatrixNED()

◆ f3DOFMagnetometerMatrixWin8()

◆ fAndroidAnglesDegFromRotationMatrix()

◆ feCompassAndroid()

◆ feCompassNED()

◆ feCompassWin8()

◆ fLPFOrientationQuaternion()

◆ fNEDAnglesDegFromRotationMatrix()

◆ fqAeq1()

◆ fqAeqNormqA()

◆ fQuaternionFromRotationMatrix()

◆ fQuaternionFromRotationVectorDeg()

◆ fRotationMatrixFromQuaternion()

◆ fRotationVectorDegFromQuaternion()

◆ fveqconjgquq()

◆ fWin8AnglesDegFromRotationMatrix()

◆ qAeqAxB()

◆ qAeqBxC()

◆ qconjgAxB()