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

Go to the source code of this file.

Macros

#define FQVA_6DOF_GY_KALMAN   2E-6F
 
#define FQVG_6DOF_GY_KALMAN   0.3F
 
#define FQWB_6DOF_GY_KALMAN   1E-9F
 
#define FQWA_6DOF_GY_KALMAN   1E-4F
 
#define FQWINITTHTH_6DOF_GY_KALMAN   2000E-5F
 
#define FQWINITBB_6DOF_GY_KALMAN   250E-3F
 
#define FQWINITTHB_6DOF_GY_KALMAN   0.0F
 
#define FQWINITAA_6DOF_GY_KALMAN   10E-5F
 
#define FCA_6DOF_GY_KALMAN   0.5F
 
#define FQVA_9DOF_GBY_KALMAN   2E-6F
 
#define FQVM_9DOF_GBY_KALMAN   0.1F
 
#define FQVG_9DOF_GBY_KALMAN   0.3F
 
#define FQWB_9DOF_GBY_KALMAN   1E-9F
 
#define FQWA_9DOF_GBY_KALMAN   1E-4F
 
#define FQWD_9DOF_GBY_KALMAN   0.5F
 
#define FQWINITTHTH_9DOF_GBY_KALMAN   2000E-5F
 
#define FQWINITBB_9DOF_GBY_KALMAN   250E-3F
 
#define FQWINITTHB_9DOF_GBY_KALMAN   0.0F
 
#define FQWINITAA_9DOF_GBY_KALMAN   10E-5F
 
#define FQWINITDD_9DOF_GBY_KALMAN   600E-3F
 
#define FCA_9DOF_GBY_KALMAN   0.5F
 
#define FCD_9DOF_GBY_KALMAN   0.5F
 
#define SINDELTAMAX   0.9063078F
 
#define COSDELTAMAX   0.4226183F
 

Functions

void fInit_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
 
void fInit_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, int16 ithisCoordSystem, int16 iSensorFS, int16 iOverSampleRatio)
 
void fRun_1DOF_P_BASIC (struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
 
void fRun_3DOF_G_BASIC (struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
 
void fRun_3DOF_B_BASIC (struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
 
void fRun_3DOF_Y_BASIC (struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
 
void fRun_6DOF_GB_BASIC (struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
 
void fRun_6DOF_GY_KALMAN (struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro, int16 ithisCoordSystem, int16 iOverSampleRatio)
 
void fRun_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal, int16 ithisCoordSystem, int16 iOverSampleRatio)
 

Macro Definition Documentation

#define COSDELTAMAX   0.4226183F

Definition at line 78 of file fusion.c.

Referenced by fRun_9DOF_GBY_KALMAN().

#define FCA_6DOF_GY_KALMAN   0.5F

Definition at line 55 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN(), and fRun_6DOF_GY_KALMAN().

#define FCA_9DOF_GBY_KALMAN   0.5F

Definition at line 74 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().

#define FCD_9DOF_GBY_KALMAN   0.5F

Definition at line 75 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQVA_6DOF_GY_KALMAN   2E-6F

Definition at line 45 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQVA_9DOF_GBY_KALMAN   2E-6F

Definition at line 61 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQVG_6DOF_GY_KALMAN   0.3F

Definition at line 46 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQVG_9DOF_GBY_KALMAN   0.3F

Definition at line 63 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQVM_9DOF_GBY_KALMAN   0.1F

Definition at line 62 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWA_6DOF_GY_KALMAN   1E-4F

Definition at line 48 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN(), and fRun_6DOF_GY_KALMAN().

#define FQWA_9DOF_GBY_KALMAN   1E-4F

Definition at line 65 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().

#define FQWB_6DOF_GY_KALMAN   1E-9F

Definition at line 47 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN(), and fRun_6DOF_GY_KALMAN().

#define FQWB_9DOF_GBY_KALMAN   1E-9F

Definition at line 64 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().

#define FQWD_9DOF_GBY_KALMAN   0.5F

Definition at line 66 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN(), and fRun_9DOF_GBY_KALMAN().

#define FQWINITAA_6DOF_GY_KALMAN   10E-5F

Definition at line 53 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQWINITAA_9DOF_GBY_KALMAN   10E-5F

Definition at line 71 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWINITBB_6DOF_GY_KALMAN   250E-3F

Definition at line 51 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQWINITBB_9DOF_GBY_KALMAN   250E-3F

Definition at line 69 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWINITDD_9DOF_GBY_KALMAN   600E-3F

Definition at line 72 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWINITTHB_6DOF_GY_KALMAN   0.0F

Definition at line 52 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQWINITTHB_9DOF_GBY_KALMAN   0.0F

Definition at line 70 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define FQWINITTHTH_6DOF_GY_KALMAN   2000E-5F

Definition at line 50 of file fusion.c.

Referenced by fInit_6DOF_GY_KALMAN().

#define FQWINITTHTH_9DOF_GBY_KALMAN   2000E-5F

Definition at line 68 of file fusion.c.

Referenced by fInit_9DOF_GBY_KALMAN().

#define SINDELTAMAX   0.9063078F

Definition at line 77 of file fusion.c.

Referenced by fRun_9DOF_GBY_KALMAN().

Function Documentation

void fInit_1DOF_P_BASIC ( struct SV_1DOF_P_BASIC pthisSV,
float  flpftimesecs,
int16  iSensorFS,
int16  iOverSampleRatio 
)

Definition at line 81 of file fusion.c.

References SV_1DOF_P_BASIC::fdeltat, SV_1DOF_P_BASIC::flpf, and SV_1DOF_P_BASIC::resetflag.

Referenced by fRun_1DOF_P_BASIC().

Here is the caller graph for this function:

void fInit_3DOF_B_BASIC ( struct SV_3DOF_B_BASIC pthisSV,
float  flpftimesecs,
int16  iSensorFS,
int16  iOverSampleRatio 
)

Definition at line 114 of file fusion.c.

References SV_3DOF_B_BASIC::fdeltat, SV_3DOF_B_BASIC::flpf, and SV_3DOF_B_BASIC::resetflag.

Referenced by fRun_3DOF_B_BASIC().

Here is the caller graph for this function:

void fInit_3DOF_G_BASIC ( struct SV_3DOF_G_BASIC pthisSV,
float  flpftimesecs,
int16  iSensorFS,
int16  iOverSampleRatio 
)

Definition at line 97 of file fusion.c.

References SV_3DOF_G_BASIC::fdeltat, SV_3DOF_G_BASIC::flpf, and SV_3DOF_G_BASIC::resetflag.

Referenced by fRun_3DOF_G_BASIC().

Here is the caller graph for this function:

void fInit_3DOF_Y_BASIC ( struct SV_3DOF_Y_BASIC pthisSV,
int16  iSensorFS,
int16  iOverSampleRatio 
)

Definition at line 131 of file fusion.c.

References f3x3matrixAeqI(), SV_3DOF_Y_BASIC::fdeltat, SV_3DOF_Y_BASIC::fFastdeltat, SV_3DOF_Y_BASIC::fq, fqAeq1(), SV_3DOF_Y_BASIC::fR, 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:

void fInit_6DOF_GB_BASIC ( struct SV_6DOF_GB_BASIC pthisSV,
float  flpftimesecs,
int16  iSensorFS,
int16  iOverSampleRatio 
)

Definition at line 148 of file fusion.c.

References SV_6DOF_GB_BASIC::fdeltat, SV_6DOF_GB_BASIC::flpf, and SV_6DOF_GB_BASIC::resetflag.

Referenced by fRun_6DOF_GB_BASIC().

Here is the caller graph for this function:

void fRun_1DOF_P_BASIC ( struct SV_1DOF_P_BASIC pthisSV,
struct PressureSensor pthisPressure,
int32  loopcounter 
)

Definition at line 323 of file fusion.c.

References PressureSensor::fHp, fInit_1DOF_P_BASIC(), SV_1DOF_P_BASIC::flpf, fLPFScalar(), SV_1DOF_P_BASIC::fLPHp, SV_1DOF_P_BASIC::fLPTp, PressureSensor::fTp, GYRO_OVERSAMPLE_RATIO, SV_1DOF_P_BASIC::resetflag, and SENSORFS.

Referenced by Fusion_Run().

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_6DOF_GY_KALMAN ( struct SV_6DOF_GY_KALMAN pthisSV,
struct AccelSensor pthisAccel,
struct GyroSensor pthisGyro,
int16  ithisCoordSystem,
int16  iOverSampleRatio 
)

Definition at line 593 of file fusion.c.

References ANDROID, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), SV_6DOF_GY_KALMAN::faErrSePl, fAndroidAnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::faSeMi, SV_6DOF_GY_KALMAN::faSePl, SV_6DOF_GY_KALMAN::fbErrPl, SV_6DOF_GY_KALMAN::fbPl, SV_6DOF_GY_KALMAN::fC3x9, FCA_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fcasq, SV_6DOF_GY_KALMAN::fChiPl, FDEGTORAD, SV_6DOF_GY_KALMAN::fDeltaq, SV_6DOF_GY_KALMAN::fdeltat, SV_6DOF_GY_KALMAN::fdeltatsq, SV_6DOF_GY_KALMAN::fFastdeltat, SV_6DOF_GY_KALMAN::fgErrSeMi, AccelSensor::fGp, SV_6DOF_GY_KALMAN::fgSeGyMi, fInit_6DOF_GY_KALMAN(), SV_6DOF_GY_KALMAN::fK9x3, fmatrixAeqInvA(), fNEDAnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fOmega, SV_6DOF_GY_KALMAN::fPhiPl, SV_6DOF_GY_KALMAN::fPPlus9x9, SV_6DOF_GY_KALMAN::fPsiPl, fqAeqNormqA(), SV_6DOF_GY_KALMAN::fqMi, SV_6DOF_GY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), SV_6DOF_GY_KALMAN::fQvAA, SV_6DOF_GY_KALMAN::fQw9x9, FQWA_6DOF_GY_KALMAN, FQWB_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQwbplusQvG, SV_6DOF_GY_KALMAN::fRhoPl, SV_6DOF_GY_KALMAN::fRMi, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_6DOF_GY_KALMAN::fRPl, SV_6DOF_GY_KALMAN::fRVecPl, SV_6DOF_GY_KALMAN::fThePl, SV_6DOF_GY_KALMAN::fThErrPl, fWin8AnglesDegFromRotationMatrix(), GyroSensor::fYpBuffer, GYRO_OVERSAMPLE_RATIO, SV_6DOF_GY_KALMAN::iFirstOrientationLock, NED, qAeqAxB(), qAeqBxC(), SV_6DOF_GY_KALMAN::resetflag, SENSORFS, WIN8, X, Y, and Z.

Referenced by Fusion_Run().

Here is the call graph for this function:

Here is the caller graph for this function:

void fRun_9DOF_GBY_KALMAN ( struct SV_9DOF_GBY_KALMAN pthisSV,
struct AccelSensor pthisAccel,
struct MagSensor pthisMag,
struct GyroSensor pthisGyro,
struct MagCalibration pthisMagCal,
int16  ithisCoordSystem,
int16  iOverSampleRatio 
)

Definition at line 1110 of file fusion.c.

References ANDROID, COSDELTAMAX, SV_9DOF_GBY_KALMAN::faErrSePl, SV_9DOF_GBY_KALMAN::faGlPl, fAndroidAnglesDegFromRotationMatrix(), SV_9DOF_GBY_KALMAN::faSeMi, SV_9DOF_GBY_KALMAN::faSePl, fasin_deg(), MagCalibration::fB, MagSensor::fBc, SV_9DOF_GBY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fC6x12, FCA_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fcasq, SV_9DOF_GBY_KALMAN::fcdsq, SV_9DOF_GBY_KALMAN::fChiPl, FDEGTORAD, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_9DOF_GBY_KALMAN::fDeltaq, SV_9DOF_GBY_KALMAN::fdeltat, SV_9DOF_GBY_KALMAN::fdeltatsq, SV_9DOF_GBY_KALMAN::fdErrGlPl, SV_9DOF_GBY_KALMAN::fdErrSePl, feCompassAndroid(), feCompassNED(), feCompassWin8(), SV_9DOF_GBY_KALMAN::fFastdeltat, MagCalibration::fFourBsq, SV_9DOF_GBY_KALMAN::fgErrSeMi, AccelSensor::fGp, SV_9DOF_GBY_KALMAN::fgSeGyMi, fInit_9DOF_GBY_KALMAN(), SV_9DOF_GBY_KALMAN::fK12x6, fmatrixAeqInvA(), SV_9DOF_GBY_KALMAN::fmErrSeMi, SV_9DOF_GBY_KALMAN::fmGl, SV_9DOF_GBY_KALMAN::fmSeGyMi, fNEDAnglesDegFromRotationMatrix(), SV_9DOF_GBY_KALMAN::fOmega, SV_9DOF_GBY_KALMAN::fPhiPl, SV_9DOF_GBY_KALMAN::fPPlus12x12, SV_9DOF_GBY_KALMAN::fPsiPl, fqAeqNormqA(), SV_9DOF_GBY_KALMAN::fqMi, SV_9DOF_GBY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), fQuaternionFromRotationVectorDeg(), SV_9DOF_GBY_KALMAN::fQvAA, SV_9DOF_GBY_KALMAN::fQvMM, SV_9DOF_GBY_KALMAN::fQw12x12, FQWA_9DOF_GBY_KALMAN, FQWB_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fQwbplusQvG, FQWD_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fRhoPl, SV_9DOF_GBY_KALMAN::fRMi, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_9DOF_GBY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fRVecPl, SV_9DOF_GBY_KALMAN::fThePl, SV_9DOF_GBY_KALMAN::fThErrPl, fWin8AnglesDegFromRotationMatrix(), GyroSensor::fYpBuffer, GYRO_OVERSAMPLE_RATIO, SV_9DOF_GBY_KALMAN::iFirstOrientationLock, MagCalibration::iValidMagCal, NED, qAeqAxB(), qAeqBxC(), SV_9DOF_GBY_KALMAN::resetflag, SENSORFS, SINDELTAMAX, THISCOORDSYSTEM, WIN8, X, Y, and Z.

Referenced by Fusion_Run().

Here is the call graph for this function:

Here is the caller graph for this function: