![]() |
ISF
2.1
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"
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) |
#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().
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().
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().
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().
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().
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().
void fInit_6DOF_GY_KALMAN | ( | struct SV_6DOF_GY_KALMAN * | pthisSV, |
int16 | iSensorFS, | ||
int16 | iOverSampleRatio | ||
) |
Definition at line 166 of file fusion.c.
References f3x3matrixAeqI(), SV_6DOF_GY_KALMAN::faErrSePl, 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, FDEGTORAD, SV_6DOF_GY_KALMAN::fdeltat, SV_6DOF_GY_KALMAN::fdeltatsq, SV_6DOF_GY_KALMAN::fFastdeltat, fqAeq1(), SV_6DOF_GY_KALMAN::fqPl, FQVA_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQvAA, FQVG_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQw9x9, FQWA_6DOF_GY_KALMAN, FQWB_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fQwbplusQvG, FQWINITAA_6DOF_GY_KALMAN, FQWINITBB_6DOF_GY_KALMAN, FQWINITTHB_6DOF_GY_KALMAN, FQWINITTHTH_6DOF_GY_KALMAN, SV_6DOF_GY_KALMAN::fRPl, SV_6DOF_GY_KALMAN::fThErrPl, SV_6DOF_GY_KALMAN::iFirstOrientationLock, SV_6DOF_GY_KALMAN::resetflag, X, and Z.
Referenced by fRun_6DOF_GY_KALMAN().
void fInit_9DOF_GBY_KALMAN | ( | struct SV_9DOF_GBY_KALMAN * | pthisSV, |
int16 | ithisCoordSystem, | ||
int16 | iSensorFS, | ||
int16 | iOverSampleRatio | ||
) |
Definition at line 234 of file fusion.c.
References DEFAULTB, f3x3matrixAeqI(), SV_9DOF_GBY_KALMAN::faErrSePl, SV_9DOF_GBY_KALMAN::faSePl, SV_9DOF_GBY_KALMAN::fbErrPl, SV_9DOF_GBY_KALMAN::fbPl, SV_9DOF_GBY_KALMAN::fC6x12, FCA_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fcasq, FCD_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fcdsq, FDEGTORAD, SV_9DOF_GBY_KALMAN::fDeltaPl, SV_9DOF_GBY_KALMAN::fdeltat, SV_9DOF_GBY_KALMAN::fdeltatsq, SV_9DOF_GBY_KALMAN::fdErrSePl, SV_9DOF_GBY_KALMAN::fFastdeltat, SV_9DOF_GBY_KALMAN::fmGl, fqAeq1(), SV_9DOF_GBY_KALMAN::fqPl, FQVA_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fQvAA, FQVG_9DOF_GBY_KALMAN, FQVM_9DOF_GBY_KALMAN, 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, FQWINITAA_9DOF_GBY_KALMAN, FQWINITBB_9DOF_GBY_KALMAN, FQWINITDD_9DOF_GBY_KALMAN, FQWINITTHB_9DOF_GBY_KALMAN, FQWINITTHTH_9DOF_GBY_KALMAN, SV_9DOF_GBY_KALMAN::fRPl, SV_9DOF_GBY_KALMAN::fThErrPl, SV_9DOF_GBY_KALMAN::iFirstOrientationLock, NED, SV_9DOF_GBY_KALMAN::resetflag, X, Y, and Z.
Referenced by fRun_9DOF_GBY_KALMAN().
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().
void fRun_3DOF_B_BASIC | ( | struct SV_3DOF_B_BASIC * | pthisSV, |
struct MagSensor * | pthisMag, | ||
int32 | loopcounter, | ||
int16 | ithisCoordSystem | ||
) |
Definition at line 403 of file fusion.c.
References ANDROID, 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(), 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(), MAG_OVERSAMPLE_RATIO, NED, SV_3DOF_B_BASIC::resetflag, and SENSORFS.
Referenced by Fusion_Run().
void fRun_3DOF_G_BASIC | ( | struct SV_3DOF_G_BASIC * | pthisSV, |
struct AccelSensor * | pthisAccel, | ||
int32 | loopcounter, | ||
int16 | ithisCoordSystem | ||
) |
Definition at line 340 of file fusion.c.
References ACCEL_OVERSAMPLE_RATIO, ANDROID, f3DOFTiltAndroid(), f3DOFTiltNED(), f3DOFTiltWin8(), fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_G_BASIC::fdeltat, AccelSensor::fGp, 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, and SENSORFS.
Referenced by Fusion_Run().
void fRun_3DOF_Y_BASIC | ( | struct SV_3DOF_Y_BASIC * | pthisSV, |
struct GyroSensor * | pthisGyro, | ||
int32 | loopcounter, | ||
int16 | ithisCoordSystem, | ||
int16 | iOverSampleRatio | ||
) |
Definition at line 461 of file fusion.c.
References ANDROID, fAndroidAnglesDegFromRotationMatrix(), SV_3DOF_Y_BASIC::fChi, SV_3DOF_Y_BASIC::fDeltaq, SV_3DOF_Y_BASIC::fFastdeltat, 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::fYpBuffer, GYRO_OVERSAMPLE_RATIO, NED, qAeqAxB(), SV_3DOF_Y_BASIC::resetflag, SENSORFS, X, and Z.
Referenced by Fusion_Run().
void fRun_6DOF_GB_BASIC | ( | struct SV_6DOF_GB_BASIC * | pthisSV, |
struct MagSensor * | pthisMag, | ||
struct AccelSensor * | pthisAccel, | ||
int32 | loopcounter, | ||
int16 | ithisCoordSystem | ||
) |
Definition at line 532 of file fusion.c.
References ANDROID, fAndroidAnglesDegFromRotationMatrix(), MagSensor::fBc, SV_6DOF_GB_BASIC::fDelta, SV_6DOF_GB_BASIC::fdeltat, feCompassAndroid(), feCompassNED(), feCompassWin8(), AccelSensor::fGp, fInit_6DOF_GB_BASIC(), SV_6DOF_GB_BASIC::fLPChi, SV_6DOF_GB_BASIC::fLPDelta, SV_6DOF_GB_BASIC::flpf, fLPFOrientationQuaternion(), fLPFScalar(), 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(), GYRO_OVERSAMPLE_RATIO, NED, SV_6DOF_GB_BASIC::resetflag, and SENSORFS.
Referenced by Fusion_Run().
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().
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().