fusion.c File Reference

Lower level sensor fusion interface. More...

#include "stdio.h"
#include "math.h"
#include "stdlib.h"
#include "sensor_fusion.h"
#include "fusion.h"
#include "orientation.h"
#include "matrix.h"
#include "approximations.h"
#include "drivers.h"
#include "control.h"

Include dependency graph for fusion.c:

Go to the source code of this file.

Functions
void	fInitializeFusion (SensorFusionGlobals *sfg)
void	fFuseSensors (struct SV_1DOF_P_BASIC *pthisSV_1DOF_P_BASIC, struct SV_3DOF_G_BASIC *pthisSV_3DOF_G_BASIC, struct SV_3DOF_B_BASIC *pthisSV_3DOF_B_BASIC, struct SV_3DOF_Y_BASIC *pthisSV_3DOF_Y_BASIC, struct SV_6DOF_GB_BASIC *pthisSV_6DOF_GB_BASIC, struct SV_6DOF_GY_KALMAN *pthisSV_6DOF_GY_KALMAN, struct SV_9DOF_GBY_KALMAN *pthisSV_9DOF_GBY_KALMAN, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct PressureSensor *pthisPressure, struct MagCalibration *pthisMagCal)
void	fInit_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
void	fInit_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
void	fInit_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV)
void	fInit_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
void	fInit_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
void	fInit_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
void	fRun_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
void	fRun_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
void	fRun_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
void	fRun_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
void	fRun_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
void	fRun_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)

Detailed Description

Lower level sensor fusion interface.

Definition in file fusion.c.

Function Documentation

fFuseSensors()

void fFuseSensors	(	struct SV_1DOF_P_BASIC *	pthisSV_1DOF_P_BASIC,
		struct SV_3DOF_G_BASIC *	pthisSV_3DOF_G_BASIC,
		struct SV_3DOF_B_BASIC *	pthisSV_3DOF_B_BASIC,
		struct SV_3DOF_Y_BASIC *	pthisSV_3DOF_Y_BASIC,
		struct SV_6DOF_GB_BASIC *	pthisSV_6DOF_GB_BASIC,
		struct SV_6DOF_GY_KALMAN *	pthisSV_6DOF_GY_KALMAN,
		struct SV_9DOF_GBY_KALMAN *	pthisSV_9DOF_GBY_KALMAN,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct PressureSensor *	pthisPressure,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 68 of file fusion.c.

References ARM_systick_elapsed_ticks(), ARM_systick_start_ticks(), fRun_1DOF_P_BASIC(), fRun_3DOF_B_BASIC(), fRun_3DOF_G_BASIC(), fRun_3DOF_Y_BASIC(), fRun_6DOF_GB_BASIC(), fRun_6DOF_GY_KALMAN(), fRun_9DOF_GBY_KALMAN(), SV_1DOF_P_BASIC::systick, SV_3DOF_G_BASIC::systick, SV_3DOF_B_BASIC::systick, SV_3DOF_Y_BASIC::systick, SV_6DOF_GB_BASIC::systick, SV_6DOF_GY_KALMAN::systick, and SV_9DOF_GBY_KALMAN::systick.

Referenced by runFusion().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_1DOF_P_BASIC()

void fInit_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure,
		float	flpftimesecs
	)

Definition at line 154 of file fusion.c.

References SV_1DOF_P_BASIC::fdeltat, PressureSensor::fH, SV_1DOF_P_BASIC::flpf, SV_1DOF_P_BASIC::fLPH, SV_1DOF_P_BASIC::fLPT, PressureSensor::fT, FUSION_HZ, and SV_1DOF_P_BASIC::resetflag.

Referenced by fRun_1DOF_P_BASIC().

Here is the caller graph for this function:

fInit_3DOF_B_BASIC()

void fInit_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 207 of file fusion.c.

References f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3DOFMagnetometerMatrixWin8(), MagSensor::fBc, SV_3DOF_B_BASIC::fdeltat, SV_3DOF_B_BASIC::flpf, SV_3DOF_B_BASIC::fLPq, SV_3DOF_B_BASIC::fLPR, fQuaternionFromRotationMatrix(), FUSION_HZ, and SV_3DOF_B_BASIC::resetflag.

Referenced by fRun_3DOF_B_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_3DOF_G_BASIC()

void fInit_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		float	flpftimesecs
	)

Definition at line 179 of file fusion.c.

References f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), SV_3DOF_G_BASIC::fdeltat, AccelSensor::fGc, SV_3DOF_G_BASIC::flpf, SV_3DOF_G_BASIC::fLPq, SV_3DOF_G_BASIC::fLPR, fQuaternionFromRotationMatrix(), FUSION_HZ, and SV_3DOF_G_BASIC::resetflag.

Referenced by fRun_3DOF_G_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_3DOF_Y_BASIC()

void fInit_3DOF_Y_BASIC

(

struct SV_3DOF_Y_BASIC *

pthisSV

)

Definition at line 235 of file fusion.c.

References f3x3matrixAeqI(), SV_3DOF_Y_BASIC::fdeltat, SV_3DOF_Y_BASIC::fq, fqAeq1(), SV_3DOF_Y_BASIC::fR, FUSION_HZ, and SV_3DOF_Y_BASIC::resetflag.

Referenced by fRun_3DOF_Y_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_6DOF_GB_BASIC()

void fInit_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		float	flpftimesecs
	)

Definition at line 250 of file fusion.c.

References MagSensor::fBc, SV_6DOF_GB_BASIC::fdeltat, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGc, SV_6DOF_GB_BASIC::fLPDelta, SV_6DOF_GB_BASIC::flpf, SV_6DOF_GB_BASIC::fLPq, SV_6DOF_GB_BASIC::fLPR, fQuaternionFromRotationMatrix(), FUSION_HZ, and SV_6DOF_GB_BASIC::resetflag.

Referenced by fRun_6DOF_GB_BASIC().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_6DOF_GY_KALMAN()

void fInit_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 282 of file fusion.c.

References CALIBRATION_NVM_ADDR, CHX, CHZ, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), SV_6DOF_GY_KALMAN::fAlphaOver2, SV_6DOF_GY_KALMAN::fAlphaQwbOver6, SV_6DOF_GY_KALMAN::fAlphaSqOver4, SV_6DOF_GY_KALMAN::fAlphaSqQvYQwbOver12, SV_6DOF_GY_KALMAN::fbErrPl, SV_6DOF_GY_KALMAN::fbPl, SV_6DOF_GY_KALMAN::fdeltat, AccelSensor::fGc, FMAX_6DOF_GY_BPL, SV_6DOF_GY_KALMAN::fMaxGyroOffsetChange, FMIN_6DOF_GY_BPL, FPIOVER180, SV_6DOF_GY_KALMAN::fqgErrPl, SV_6DOF_GY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), FQVY_6DOF_GY_KALMAN, FQWB_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQwbOver3, SV_6DOF_GY_KALMAN::fRPl, FUSION_HZ, GyroSensor::fYs, GYRO_NVM_OFFSET, and SV_6DOF_GY_KALMAN::resetflag.

Referenced by fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

fInit_9DOF_GBY_KALMAN()

void fInit_9DOF_GBY_KALMAN	(	struct SV_9DOF_GBY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct MagSensor *	pthisMag,
		struct GyroSensor *	pthisGyro,
		struct MagCalibration *	pthisMagCal
	)

Definition at line 346 of file fusion.c.

References CALIBRATION_NVM_ADDR, CHX, CHZ, SV_9DOF_GBY_KALMAN::fAlphaOver2, SV_9DOF_GBY_KALMAN::fAlphaQwbOver6, SV_9DOF_GBY_KALMAN::fAlphaSqOver4, SV_9DOF_GBY_KALMAN::fAlphaSqQvYQwbOver12, MagSensor::fBc, SV_9DOF_GBY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fcosDeltaPl, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_9DOF_GBY_KALMAN::fdeltat, SV_9DOF_GBY_KALMAN::fDisGl, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGc, SV_9DOF_GBY_KALMAN::fgdeltat, FMAX_9DOF_GBY_BPL, SV_9DOF_GBY_KALMAN::fMaxGyroOffsetChange, FMIN_9DOF_GBY_BPL, FPIOVER180, SV_9DOF_GBY_KALMAN::fqgErrPl, SV_9DOF_GBY_KALMAN::fqmErrPl, SV_9DOF_GBY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), FQVY_9DOF_GBY_KALMAN, FQWB_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fQwbOver3, SV_9DOF_GBY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fsinDeltaPl, FUSION_HZ, SV_9DOF_GBY_KALMAN::fVelGl, GyroSensor::fYs, GTOMSEC2, GYRO_NVM_OFFSET, SV_9DOF_GBY_KALMAN::iFirstAccelMagLock, and SV_9DOF_GBY_KALMAN::resetflag.

Referenced by fRun_6DOF_GY_KALMAN().

Here is the call graph for this function:

Here is the caller graph for this function:

fInitializeFusion()

void fInitializeFusion

(

SensorFusionGlobals *

sfg

)

Definition at line 33 of file fusion.c.

References SensorFusionGlobals::loopcounter.

Referenced by DecodeCommandBytes(), and initializeFusionEngine().

Here is the caller graph for this function:

fRun_1DOF_P_BASIC()

void fRun_1DOF_P_BASIC	(	struct SV_1DOF_P_BASIC *	pthisSV,
		struct PressureSensor *	pthisPressure
	)

Definition at line 419 of file fusion.c.

References PressureSensor::fH, fInit_1DOF_P_BASIC(), SV_1DOF_P_BASIC::flpf, FLPFSECS_1DOF_P_BASIC, SV_1DOF_P_BASIC::fLPH, SV_1DOF_P_BASIC::fLPT, PressureSensor::fT, and SV_1DOF_P_BASIC::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function:

fRun_3DOF_B_BASIC()

void fRun_3DOF_B_BASIC	(	struct SV_3DOF_B_BASIC *	pthisSV,
		struct MagSensor *	pthisMag
	)

Definition at line 489 of file fusion.c.

References f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3DOFMagnetometerMatrixWin8(), fAndroidAnglesDegFromRotationMatrix(), MagSensor::fBc, SV_3DOF_B_BASIC::fdeltat, fInit_3DOF_B_BASIC(), SV_3DOF_B_BASIC::fLPChi, SV_3DOF_B_BASIC::flpf, fLPFOrientationQuaternion(), FLPFSECS_3DOF_B_BASIC, SV_3DOF_B_BASIC::fLPPhi, SV_3DOF_B_BASIC::fLPPsi, SV_3DOF_B_BASIC::fLPq, SV_3DOF_B_BASIC::fLPR, SV_3DOF_B_BASIC::fLPRho, SV_3DOF_B_BASIC::fLPRVec, SV_3DOF_B_BASIC::fLPThe, fNEDAnglesDegFromRotationMatrix(), SV_3DOF_B_BASIC::fOmega, SV_3DOF_B_BASIC::fq, fQuaternionFromRotationMatrix(), SV_3DOF_B_BASIC::fR, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fWin8AnglesDegFromRotationMatrix(), and SV_3DOF_B_BASIC::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function:

fRun_3DOF_G_BASIC()

void fRun_3DOF_G_BASIC	(	struct SV_3DOF_G_BASIC *	pthisSV,
		struct AccelSensor *	pthisAccel
	)

Definition at line 439 of file fusion.c.

References f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_G_BASIC::fdeltat, AccelSensor::fGc, fInit_3DOF_G_BASIC(), SV_3DOF_G_BASIC::fLPChi, SV_3DOF_G_BASIC::flpf, fLPFOrientationQuaternion(), FLPFSECS_3DOF_G_BASIC, SV_3DOF_G_BASIC::fLPPhi, SV_3DOF_G_BASIC::fLPPsi, SV_3DOF_G_BASIC::fLPq, SV_3DOF_G_BASIC::fLPR, SV_3DOF_G_BASIC::fLPRho, SV_3DOF_G_BASIC::fLPRVec, SV_3DOF_G_BASIC::fLPThe, fNEDAnglesDegFromRotationMatrix(), SV_3DOF_G_BASIC::fOmega, SV_3DOF_G_BASIC::fq, fQuaternionFromRotationMatrix(), SV_3DOF_G_BASIC::fR, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fWin8AnglesDegFromRotationMatrix(), and SV_3DOF_G_BASIC::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function:

fRun_3DOF_Y_BASIC()

void fRun_3DOF_Y_BASIC	(	struct SV_3DOF_Y_BASIC *	pthisSV,
		struct GyroSensor *	pthisGyro
	)

Definition at line 534 of file fusion.c.

References CHX, CHZ, fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_Y_BASIC::fChi, SV_3DOF_Y_BASIC::fdeltat, fInit_3DOF_Y_BASIC(), fNEDAnglesDegFromRotationMatrix(), SV_3DOF_Y_BASIC::fOmega, SV_3DOF_Y_BASIC::fPhi, SV_3DOF_Y_BASIC::fPsi, SV_3DOF_Y_BASIC::fq, fqAeqNormqA(), fQuaternionFromRotationVectorDeg(), SV_3DOF_Y_BASIC::fR, SV_3DOF_Y_BASIC::fRho, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_3DOF_Y_BASIC::fRVec, SV_3DOF_Y_BASIC::fThe, fWin8AnglesDegFromRotationMatrix(), GyroSensor::fYs, qAeqAxB(), and SV_3DOF_Y_BASIC::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function:

fRun_6DOF_GB_BASIC()

void fRun_6DOF_GB_BASIC	(	struct SV_6DOF_GB_BASIC *	pthisSV,
		struct MagSensor *	pthisMag,
		struct AccelSensor *	pthisAccel
	)

Definition at line 582 of file fusion.c.

References fAndroidAnglesDegFromRotationMatrix(), MagSensor::fBc, SV_6DOF_GB_BASIC::fDelta, SV_6DOF_GB_BASIC::fdeltat, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGc, fInit_6DOF_GB_BASIC(), SV_6DOF_GB_BASIC::fLPChi, SV_6DOF_GB_BASIC::fLPDelta, SV_6DOF_GB_BASIC::flpf, fLPFOrientationQuaternion(), FLPFSECS_6DOF_GB_BASIC, SV_6DOF_GB_BASIC::fLPPhi, SV_6DOF_GB_BASIC::fLPPsi, SV_6DOF_GB_BASIC::fLPq, SV_6DOF_GB_BASIC::fLPR, SV_6DOF_GB_BASIC::fLPRho, SV_6DOF_GB_BASIC::fLPRVec, SV_6DOF_GB_BASIC::fLPThe, fNEDAnglesDegFromRotationMatrix(), SV_6DOF_GB_BASIC::fOmega, SV_6DOF_GB_BASIC::fq, fQuaternionFromRotationMatrix(), SV_6DOF_GB_BASIC::fR, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), fWin8AnglesDegFromRotationMatrix(), and SV_6DOF_GB_BASIC::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function:

fRun_6DOF_GY_KALMAN()

void fRun_6DOF_GY_KALMAN	(	struct SV_6DOF_GY_KALMAN *	pthisSV,
		struct AccelSensor *	pthisAccel,
		struct GyroSensor *	pthisGyro
	)

Definition at line 635 of file fusion.c.

References CHX, CHY, CHZ, f3x3matrixAeqB(), SV_6DOF_GY_KALMAN::fAccGl, SV_9DOF_GBY_KALMAN::fAccGl, SV_6DOF_GY_KALMAN::fAlphaOver2, SV_9DOF_GBY_KALMAN::fAlphaOver2, SV_6DOF_GY_KALMAN::fAlphaSqOver4, SV_9DOF_GBY_KALMAN::fAlphaSqOver4, SV_6DOF_GY_KALMAN::fAlphaSqQvYQwbOver12, SV_9DOF_GBY_KALMAN::fAlphaSqQvYQwbOver12, fAndroidAnglesDegFromRotationMatrix(), MagCalibration::fB, MagSensor::fBc, SV_6DOF_GY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbErrPl, SV_6DOF_GY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fbPl, MagCalibration::fBSq, SV_6DOF_GY_KALMAN::fChiPl, SV_9DOF_GBY_KALMAN::fChiPl, SV_9DOF_GBY_KALMAN::fcosDeltaPl, GyroSensor::fDegPerSecPerCount, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_6DOF_GY_KALMAN::fdeltat, SV_9DOF_GBY_KALMAN::fdeltat, SV_9DOF_GBY_KALMAN::fDisGl, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGc, SV_9DOF_GBY_KALMAN::fgdeltat, fInit_6DOF_GY_KALMAN(), fInit_9DOF_GBY_KALMAN(), SV_6DOF_GY_KALMAN::fK6x3, SV_9DOF_GBY_KALMAN::fK9x6, fmatrixAeqInvA(), FMAX_9DOF_GBY_BPL, SV_6DOF_GY_KALMAN::fMaxGyroOffsetChange, SV_9DOF_GBY_KALMAN::fMaxGyroOffsetChange, FMIN_9DOF_GBY_BPL, fNEDAnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fOmega, SV_9DOF_GBY_KALMAN::fOmega, SV_6DOF_GY_KALMAN::fPhiPl, SV_9DOF_GBY_KALMAN::fPhiPl, SV_6DOF_GY_KALMAN::fPsiPl, SV_9DOF_GBY_KALMAN::fPsiPl, fqAeqNormqA(), SV_6DOF_GY_KALMAN::fqgErrPl, SV_9DOF_GBY_KALMAN::fqgErrPl, SV_9DOF_GBY_KALMAN::fqmErrPl, SV_6DOF_GY_KALMAN::fqPl, SV_9DOF_GBY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), SV_6DOF_GY_KALMAN::fQv, SV_9DOF_GBY_KALMAN::fQv6x1, FQVB_9DOF_GBY_KALMAN, FQVG_6DOF_GY_KALMAN, FQVG_9DOF_GBY_KALMAN, SV_6DOF_GY_KALMAN::fQw6x6, SV_9DOF_GBY_KALMAN::fQw9x9, SV_6DOF_GY_KALMAN::fQwbOver3, SV_9DOF_GBY_KALMAN::fQwbOver3, SV_6DOF_GY_KALMAN::fQwCT6x3, SV_9DOF_GBY_KALMAN::fQwCT9x6, SV_6DOF_GY_KALMAN::fRhoPl, SV_9DOF_GBY_KALMAN::fRhoPl, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_6DOF_GY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fRPl, fRun_9DOF_GBY_KALMAN(), SV_6DOF_GY_KALMAN::fRVecPl, SV_9DOF_GBY_KALMAN::fRVecPl, SV_9DOF_GBY_KALMAN::fsinDeltaPl, SV_6DOF_GY_KALMAN::fThePl, SV_9DOF_GBY_KALMAN::fThePl, SV_9DOF_GBY_KALMAN::fVelGl, fveqconjgquq(), fveqRu(), fWin8AnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fZErr, SV_9DOF_GBY_KALMAN::fZErr, GyroSensor::iFIFOCount, SV_9DOF_GBY_KALMAN::iFirstAccelMagLock, MagCalibration::iValidMagCal, GyroSensor::iYs, GyroSensor::iYsFIFO, ONEOVER12, Quaternion::q0, Quaternion::q1, Quaternion::q2, Quaternion::q3, qAeqAxB(), qAeqBxC(), SV_6DOF_GY_KALMAN::resetflag, and SV_9DOF_GBY_KALMAN::resetflag.

Referenced by fFuseSensors().

Here is the call graph for this function:

Here is the caller graph for this function: