![]() |
ISF
2.2 rev 5
Intelligent Sensing Framework for Kinetis with Processor Expert
|
#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 "drivers.h"
Go to the source code of this file.
Functions | |
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) |
void | fInit_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, 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) |
void | fRun_9DOF_GBY_KALMAN (struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal) |
Variables | |
struct ProjectGlobals | globals |
void fInit_1DOF_P_BASIC | ( | struct SV_1DOF_P_BASIC * | pthisSV, |
struct PressureSensor * | pthisPressure, | ||
float | flpftimesecs | ||
) |
Definition at line 84 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, OVERSAMPLE_RATIO, SV_1DOF_P_BASIC::resetflag, and SENSORFS.
Referenced by fRun_1DOF_P_BASIC().
void fInit_3DOF_B_BASIC | ( | struct SV_3DOF_B_BASIC * | pthisSV, |
struct MagSensor * | pthisMag, | ||
float | flpftimesecs | ||
) |
Definition at line 143 of file fusion.c.
References ANDROID, f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3DOFMagnetometerMatrixWin8(), MagSensor::fBcAvg, SV_3DOF_B_BASIC::fdeltat, SV_3DOF_B_BASIC::flpf, SV_3DOF_B_BASIC::fLPq, SV_3DOF_B_BASIC::fLPR, fQuaternionFromRotationMatrix(), globals, NED, OVERSAMPLE_RATIO, Q3M, SV_3DOF_B_BASIC::resetflag, SENSORFS, THISCOORDSYSTEM, and WIN8.
Referenced by fRun_3DOF_B_BASIC().
void fInit_3DOF_G_BASIC | ( | struct SV_3DOF_G_BASIC * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
float | flpftimesecs | ||
) |
Definition at line 106 of file fusion.c.
References ANDROID, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), SV_3DOF_G_BASIC::fdeltat, AccelSensor::fGsAvg, SV_3DOF_G_BASIC::flpf, SV_3DOF_G_BASIC::fLPq, SV_3DOF_G_BASIC::fLPR, fQuaternionFromRotationMatrix(), globals, NED, OVERSAMPLE_RATIO, Q3, SV_3DOF_G_BASIC::resetflag, SENSORFS, THISCOORDSYSTEM, and WIN8.
Referenced by fRun_3DOF_G_BASIC().
void fInit_3DOF_Y_BASIC | ( | struct SV_3DOF_Y_BASIC * | pthisSV | ) |
Definition at line 180 of file fusion.c.
References f3x3matrixAeqI(), SV_3DOF_Y_BASIC::fdeltat, SV_3DOF_Y_BASIC::fGyrodeltat, SV_3DOF_Y_BASIC::fq, fqAeq1(), SV_3DOF_Y_BASIC::fR, globals, OVERSAMPLE_RATIO, Q3G, SV_3DOF_Y_BASIC::resetflag, and SENSORFS.
Referenced by fRun_3DOF_Y_BASIC().
void fInit_6DOF_GB_BASIC | ( | struct SV_6DOF_GB_BASIC * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
struct MagSensor * | pthisMag, | ||
float | flpftimesecs | ||
) |
Definition at line 200 of file fusion.c.
References ANDROID, MagSensor::fBcAvg, SV_6DOF_GB_BASIC::fdeltat, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGsAvg, SV_6DOF_GB_BASIC::fLPDelta, SV_6DOF_GB_BASIC::flpf, SV_6DOF_GB_BASIC::fLPq, SV_6DOF_GB_BASIC::fLPR, fQuaternionFromRotationMatrix(), globals, NED, OVERSAMPLE_RATIO, Q6MA, SV_6DOF_GB_BASIC::resetflag, SENSORFS, THISCOORDSYSTEM, and WIN8.
Referenced by fRun_6DOF_GB_BASIC().
void fInit_6DOF_GY_KALMAN | ( | struct SV_6DOF_GY_KALMAN * | pthisSV, |
struct AccelSensor * | pthisAccel | ||
) |
Definition at line 238 of file fusion.c.
References ANDROID, CHX, CHZ, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), SV_6DOF_GY_KALMAN::fAlphaOver2, SV_6DOF_GY_KALMAN::fAlphaOver2Qwb, SV_6DOF_GY_KALMAN::fAlphaOver2Sq, SV_6DOF_GY_KALMAN::fAlphaOver2SqQvYQwb, SV_6DOF_GY_KALMAN::fbErrPl, SV_6DOF_GY_KALMAN::fbPl, AccelSensor::fGsAvg, SV_6DOF_GY_KALMAN::fGyrodeltat, FPIOVER180, SV_6DOF_GY_KALMAN::fqgErrPl, SV_6DOF_GY_KALMAN::fqPl, fQuaternionFromRotationMatrix(), FQVY_6DOF_GY_KALMAN, FQWB_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fRPl, globals, NED, OVERSAMPLE_RATIO, Q6AG, SV_6DOF_GY_KALMAN::resetflag, SENSORFS, THISCOORDSYSTEM, and WIN8.
Referenced by fRun_6DOF_GY_KALMAN().
void fInit_9DOF_GBY_KALMAN | ( | struct SV_9DOF_GBY_KALMAN * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
struct MagSensor * | pthisMag, | ||
struct MagCalibration * | pthisMagCal | ||
) |
Definition at line 284 of file fusion.c.
References ANDROID, CHX, CHZ, SV_9DOF_GBY_KALMAN::fAlphaOver2, SV_9DOF_GBY_KALMAN::fAlphaOver2Qwb, SV_9DOF_GBY_KALMAN::fAlphaOver2Sq, SV_9DOF_GBY_KALMAN::fAlphaOver2SqQvYQwb, MagCalibration::fB, MagSensor::fBcAvg, SV_9DOF_GBY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fcosDeltaPl, SV_9DOF_GBY_KALMAN::fDeltaErrPl, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_9DOF_GBY_KALMAN::fDisGl, SV_9DOF_GBY_KALMAN::fgKalmandeltat, AccelSensor::fGsAvg, SV_9DOF_GBY_KALMAN::fGyrodeltat, SV_9DOF_GBY_KALMAN::fKalmandeltat, fLeastSquareseCompassAndroid(), fLeastSquareseCompassNED(), fLeastSquareseCompassWin8(), FPIOVER180, SV_9DOF_GBY_KALMAN::fqgErrPl, SV_9DOF_GBY_KALMAN::fqmErrPl, SV_9DOF_GBY_KALMAN::fqPl, FQVY_9DOF_GBY_KALMAN, FQWB_9DOF_GBY_KALMAN, fRotationMatrixFromQuaternion(), SV_9DOF_GBY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fsinDeltaPl, SV_9DOF_GBY_KALMAN::fVelGl, globals, GTOMSEC2, SV_9DOF_GBY_KALMAN::iFirstAccelMagLock, NED, OVERSAMPLE_RATIO, Q9, SV_9DOF_GBY_KALMAN::resetflag, SENSORFS, THISCOORDSYSTEM, and WIN8.
Referenced by fRun_9DOF_GBY_KALMAN().
void fRun_1DOF_P_BASIC | ( | struct SV_1DOF_P_BASIC * | pthisSV, |
struct PressureSensor * | pthisPressure | ||
) |
Definition at line 361 of file fusion.c.
References PressureSensor::fH, fInit_1DOF_P_BASIC(), SV_1DOF_P_BASIC::flpf, SV_1DOF_P_BASIC::fLPH, SV_1DOF_P_BASIC::fLPT, PressureSensor::fT, and SV_1DOF_P_BASIC::resetflag.
Referenced by Fusion_Run().
void fRun_3DOF_B_BASIC | ( | struct SV_3DOF_B_BASIC * | pthisSV, |
struct MagSensor * | pthisMag | ||
) |
Definition at line 437 of file fusion.c.
References ANDROID, f3DOFMagnetometerMatrixAndroid(), f3DOFMagnetometerMatrixNED(), f3DOFMagnetometerMatrixWin8(), fAndroidAnglesDegFromRotationMatrix(), MagSensor::fBcAvg, SV_3DOF_B_BASIC::fdeltat, fInit_3DOF_B_BASIC(), SV_3DOF_B_BASIC::fLPChi, SV_3DOF_B_BASIC::flpf, fLPFOrientationQuaternion(), 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(), NED, SV_3DOF_B_BASIC::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
void fRun_3DOF_G_BASIC | ( | struct SV_3DOF_G_BASIC * | pthisSV, |
struct AccelSensor * | pthisAccel | ||
) |
Definition at line 380 of file fusion.c.
References ANDROID, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_G_BASIC::fdeltat, AccelSensor::fGsAvg, fInit_3DOF_G_BASIC(), SV_3DOF_G_BASIC::fLPChi, SV_3DOF_G_BASIC::flpf, fLPFOrientationQuaternion(), 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(), NED, SV_3DOF_G_BASIC::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
void fRun_3DOF_Y_BASIC | ( | struct SV_3DOF_Y_BASIC * | pthisSV, |
struct GyroSensor * | pthisGyro | ||
) |
Definition at line 489 of file fusion.c.
References ANDROID, CHX, CHZ, fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_Y_BASIC::fChi, GyroSensor::fDegPerSecPerCount, SV_3DOF_Y_BASIC::fGyrodeltat, 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::iYsBuffer, NED, OVERSAMPLE_RATIO, qAeqAxB(), SV_3DOF_Y_BASIC::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
void fRun_6DOF_GB_BASIC | ( | struct SV_6DOF_GB_BASIC * | pthisSV, |
struct MagSensor * | pthisMag, | ||
struct AccelSensor * | pthisAccel | ||
) |
Definition at line 551 of file fusion.c.
References ANDROID, fAndroidAnglesDegFromRotationMatrix(), MagSensor::fBcAvg, SV_6DOF_GB_BASIC::fDelta, SV_6DOF_GB_BASIC::fdeltat, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGsAvg, fInit_6DOF_GB_BASIC(), SV_6DOF_GB_BASIC::fLPChi, SV_6DOF_GB_BASIC::fLPDelta, SV_6DOF_GB_BASIC::flpf, fLPFOrientationQuaternion(), 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(), NED, SV_6DOF_GB_BASIC::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
void fRun_6DOF_GY_KALMAN | ( | struct SV_6DOF_GY_KALMAN * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
struct GyroSensor * | pthisGyro | ||
) |
Definition at line 606 of file fusion.c.
References ANDROID, CHX, CHY, CHZ, SV_6DOF_GY_KALMAN::fAccGl, SV_6DOF_GY_KALMAN::fAlphaOver2, SV_6DOF_GY_KALMAN::fAlphaOver2Qwb, SV_6DOF_GY_KALMAN::fAlphaOver2Sq, SV_6DOF_GY_KALMAN::fAlphaOver2SqQvYQwb, fAndroidAnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fbErrPl, SV_6DOF_GY_KALMAN::fbPl, SV_6DOF_GY_KALMAN::fChiPl, GyroSensor::fDegPerSecPerCount, AccelSensor::fGsAvg, SV_6DOF_GY_KALMAN::fGyrodeltat, fInit_6DOF_GY_KALMAN(), SV_6DOF_GY_KALMAN::fK6x3, fmatrixAeqInvA(), fNEDAnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fOmega, SV_6DOF_GY_KALMAN::fPhiPl, SV_6DOF_GY_KALMAN::fPsiPl, fqAeqNormqA(), SV_6DOF_GY_KALMAN::fqgErrPl, SV_6DOF_GY_KALMAN::fqPl, fQuaternionFromRotationVectorDeg(), SV_6DOF_GY_KALMAN::fQv, SV_6DOF_GY_KALMAN::fQw6x6, FQWB_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQwCT6x3, SV_6DOF_GY_KALMAN::fRhoPl, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_6DOF_GY_KALMAN::fRPl, SV_6DOF_GY_KALMAN::fRVecPl, SV_6DOF_GY_KALMAN::fThePl, fveqconjgquq(), fWin8AnglesDegFromRotationMatrix(), SV_6DOF_GY_KALMAN::fZErr, GyroSensor::iYsBuffer, NED, OVERSAMPLE_RATIO, fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, qAeqAxB(), qAeqBxC(), SV_6DOF_GY_KALMAN::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
void fRun_9DOF_GBY_KALMAN | ( | struct SV_9DOF_GBY_KALMAN * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
struct MagSensor * | pthisMag, | ||
struct GyroSensor * | pthisGyro, | ||
struct MagCalibration * | pthisMagCal | ||
) |
Definition at line 940 of file fusion.c.
References ANDROID, CHX, CHY, CHZ, F180OVERPISQ, f3x3matrixAeqB(), SV_9DOF_GBY_KALMAN::fAccGl, SV_9DOF_GBY_KALMAN::fAlphaOver2, SV_9DOF_GBY_KALMAN::fAlphaOver2Qwb, SV_9DOF_GBY_KALMAN::fAlphaOver2Sq, SV_9DOF_GBY_KALMAN::fAlphaOver2SqQvYQwb, fAndroidAnglesDegFromRotationMatrix(), MagCalibration::fB, MagSensor::fBcAvg, SV_9DOF_GBY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fChiPl, SV_9DOF_GBY_KALMAN::fcosDeltaPl, GyroSensor::fDegPerSecPerCount, SV_9DOF_GBY_KALMAN::fDeltaErrPl, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_9DOF_GBY_KALMAN::fDisGl, SV_9DOF_GBY_KALMAN::fgKalmandeltat, AccelSensor::fGsAvg, FGYRO_OFFSET_MAX_9DOF_GBY_KALMAN, FGYRO_OFFSET_MIN_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fGyrodeltat, fInit_9DOF_GBY_KALMAN(), SV_9DOF_GBY_KALMAN::fK10x7, SV_9DOF_GBY_KALMAN::fKalmandeltat, fLeastSquareseCompassAndroid(), fLeastSquareseCompassNED(), fLeastSquareseCompassWin8(), fmatrixAeqInvA(), fNEDAnglesDegFromRotationMatrix(), SV_9DOF_GBY_KALMAN::fOmega, SV_9DOF_GBY_KALMAN::fPhiPl, FPIOVER180, SV_9DOF_GBY_KALMAN::fPsiPl, fqAeqNormqA(), SV_9DOF_GBY_KALMAN::fqgErrPl, SV_9DOF_GBY_KALMAN::fqmErrPl, SV_9DOF_GBY_KALMAN::fqPl, fQuaternionFromRotationVectorDeg(), SV_9DOF_GBY_KALMAN::fQv7x1, FQVBQD_MIN_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fQw10x10, FQWB_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fQwCT10x7, FQWDLT_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fRhoPl, fRotationMatrixFromQuaternion(), fRotationVectorDegFromQuaternion(), SV_9DOF_GBY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fRVecPl, SV_9DOF_GBY_KALMAN::fsinDeltaPl, SV_9DOF_GBY_KALMAN::fThePl, SV_9DOF_GBY_KALMAN::fVelGl, fveqconjgquq(), fWin8AnglesDegFromRotationMatrix(), SV_9DOF_GBY_KALMAN::fZErr, SV_9DOF_GBY_KALMAN::iFirstAccelMagLock, MagCalibration::iValidMagCal, GyroSensor::iYsBuffer, NED, ONEOVERSQRT2, OVERSAMPLE_RATIO, fquaternion::q0, fquaternion::q1, fquaternion::q2, fquaternion::q3, qAeqAxB(), qAeqBxC(), SV_9DOF_GBY_KALMAN::resetflag, THISCOORDSYSTEM, and WIN8.
Referenced by Fusion_Run().
struct ProjectGlobals globals |