64 #define FCOUNTSPERUT 10.0F // sensor specific: MAG3110 and FXOS8700 provide 10 counts / uT
65 #define FCOUNTSPERG 2048.0F // sensor specific: MMA8451 and FXOS8700 provide 2048 counts / g in 4g mode
66 #define FUTPERCOUNT 0.1F // MAG3110 and FXOS8700 provide 0.1uT per count resolution
71 #define ANGLE_LPF_FPU 1.0F
76 #define NED 0 // identifier for NED angle output
77 #define ANDROID 1 // identifier for Android angle output
78 #define WIN8 2 // identifier for Windows 8 angle output
81 #define PI 3.141592654F // Pi
82 #define FDEGTORAD 0.01745329251994F // degrees to radians conversion = pi / 180
83 #define FRADTODEG 57.2957795130823F // radians to degrees conversion = 180 / pi
84 #define FRECIP180 0.0055555555555F // multiplicative factor 1/180
85 #define FMATRIXSCALING 0.02F // approx normalizes geomagnetic field 50uT
86 #define FINVMATRIXSCALING 50.0F // inverse of FMATRIXSCALING
89 #define MAGBUFFSIZE 6 // magnetic buffer size: 6 implies 6^3 = 216 entries
90 #define INITIALCALINTERVAL 50 // 1s at 50Hz: initial (limited measurements) calibration interval
91 #define FINALCALINTERVAL 1500 // 30s at 50Hz: normal (sufficient measurements) calibration interval
92 #define MINEQUATIONS 24 // minimum number of measurements used for calibration
93 #define MEDEQUATIONS 80 // new solution always accepted up to this point
94 #define MAXEQUATIONS 120 // maximum number of measurements used for calibration
95 #define MINBFIT 5.0F // minimum acceptable geomagnetic field B (uT) for valid calibration
96 #define MAXBFIT 100.0F // maximum acceptable geomagnetic field B (uT) for valid calibration
97 #define FITERRORAGING 0.0033333F// reciprocal of time (s) for fit error to increase by e=2.718 (so here 300s=5 mins)
100 #define MAXMATINV 6 // maximum size supported in matrix inverse function
103 #define BUFFSIZE 2048
252 float fb0,
float fa1,
float fa2);
258 float **ftmpA10x10,
float **ftmpB10x10,
float **ftmpA10x1,
float **ftmpA3x3,
float **ftmpA3x1);
260 float **ftmpA7x7,
float **ftmpB7x7,
float **ftmpA7x1);
262 float **ftmpA4x4,
float **ftmpB4x4,
float **ftmpA4x1,
float **ftmpB4x1,
float xfR6DOFnm1[3][3]
Definition: include.h:215
float * finvW[3]
Definition: include.h:185
int16 iGpy
Definition: include.h:140
float fB
Definition: include.h:176
int16 iBfx
Definition: include.h:156
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer, float **ftmpA7x7, float **ftmpB7x7, float **ftmpA7x1)
Definition: magnetic.c:180
int16 iGpx
Definition: include.h:139
int32 iMagBufferCount
Definition: include.h:236
float q1
Definition: include.h:109
float ftrVy
Definition: include.h:179
float xW[3][3]
Definition: include.h:124
int16 iBfz
Definition: include.h:158
float fRho6DOF
Definition: include.h:201
float fPsi6DOF
Definition: include.h:200
float fDeltaDeg
Definition: include.h:128
float ftrVz
Definition: include.h:180
int16 iBcz
Definition: include.h:155
Definition: include.h:230
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
Definition: magnetic.c:68
float fBcy
Definition: include.h:163
int16 iBpz
Definition: include.h:152
float fLPDelta6DOFn
Definition: include.h:209
void fmatrixAeqB(float **A, float **B, int32 r, int32 c)
Definition: matrix.c:226
short int int16
16 bits
Definition: arm_cm0.h:101
float ftrFitErrorpc
Definition: include.h:182
int16 iBpy
Definition: include.h:151
float q2
Definition: include.h:110
float xfR6DOFn[3][3]
Definition: include.h:213
float fLPPhi6DOF
Definition: include.h:202
float xfinvW[3][3]
Definition: include.h:184
Definition: include.h:137
float * finvW[3]
Definition: include.h:123
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer, float **ftmpA10x10, float **ftmpB10x10, float **ftmpA10x1, float **ftmpA3x3, float **ftmpA3x1)
Definition: magnetic.c:165
void fLPFOrientationMatrix(struct SV6DOF *pthisSV6DOF, int32 iCoordSystem, int32 loopcounter, float fb0, float fa1, float fa2)
Definition: orientation.c:307
void fQuaternionFromRotationMatrix(float **R, struct fquaternion *pq)
Definition: orientation.c:526
float fPhi6DOF
Definition: include.h:198
float fLPPsi6DOF
Definition: include.h:204
void fmatrixAeqInvA(float **A, int32 isize, int32 **icolind, int32 **irowind, int32 **ipivot)
Definition: matrix.c:439
void fmatrixAeqRootSymB(float **A, float **B, float **ftmpmat, float **ftmpvec, int32 rc)
Definition: matrix.c:244
float fVy
Definition: include.h:174
float fPsiDeg
Definition: include.h:132
float * fW[3]
Definition: include.h:125
void fInitLPFOrientationMatrix(float *pfb0, float *pfa1, float *pfa2)
Definition: orientation.c:283
struct fquaternion fq6DOFn
Definition: include.h:225
float fLPDelta6DOFnm2
Definition: include.h:211
float xftrinvW[3][3]
Definition: include.h:190
float fBcx
Definition: include.h:162
float fBfy
Definition: include.h:166
float fBfx
Definition: include.h:165
void feCompassDirectAndroid(struct SV6DOF *pthisSV6DOF, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: orientation.c:121
void fmatrixAeqAxScalar(float **A, float Scalar, int32 r, int32 c)
Definition: matrix.c:127
float fVy
Definition: include.h:119
void fmatrixAeqBxTrB(float **A, float **B, int32 rB, int32 cB)
Definition: matrix.c:85
int16 iBz[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]
Definition: include.h:234
float f3x3matrixDetA(float **inp)
Definition: matrix.c:177
void fmatrixAeqBxC(float **A, float **B, float **C, int32 rB, int32 cBrC, int32 cC)
Definition: matrix.c:40
void fmatrixAeqRenormRotA(float **A)
Definition: matrix.c:569
float xinvW[3][3]
Definition: include.h:122
float xfLPR6DOFn[3][3]
Definition: include.h:219
float fBcz
Definition: include.h:164
int16 iBpx
Definition: include.h:150
float fFitErrorpc
Definition: include.h:177
float fRhoDeg
Definition: include.h:133
float q3
Definition: include.h:111
float fLPThe6DOF
Definition: include.h:203
float * fLPR6DOFn[3]
Definition: include.h:220
Definition: include.h:148
float fBpy
Definition: include.h:160
float fVx
Definition: include.h:118
float fGpz
Definition: include.h:144
float * fLPR6DOFnm1[3]
Definition: include.h:222
float fB
Definition: include.h:127
float fPhiDeg
Definition: include.h:130
float * fR6DOFn[3]
Definition: include.h:214
float xfLPR6DOFnm1[3][3]
Definition: include.h:221
#define MAGBUFFSIZE
Definition: include.h:89
float xfLPR6DOFnm2[3][3]
Definition: include.h:223
float xfR6DOFnm2[3][3]
Definition: include.h:217
float ftrVx
Definition: include.h:178
Definition: include.h:106
void ResetMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer)
Definition: magnetic.c:40
int16 iBx[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]
Definition: include.h:232
float * fR6DOFnm1[3]
Definition: include.h:216
float fBfz
Definition: include.h:167
int16 iBcy
Definition: include.h:154
float fVz
Definition: include.h:120
Definition: include.h:195
int16 iBcx
Definition: include.h:153
float * fLPR6DOFnm2[3]
Definition: include.h:224
int32 iValidMagCal
Definition: include.h:183
float * fA[3]
Definition: include.h:187
void f3x3matrixAeqInvSymB(float **A, float **B)
Definition: matrix.c:190
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagneticBuffer, float **ftmpA4x4, float **ftmpB4x4, float **ftmpA4x1, float **ftmpB4x1, int32 **icolind, int32 **irowind, int32 **ipivot)
Definition: magnetic.c:342
float * fR6DOFnm2[3]
Definition: include.h:218
void fmatrixPrintA(float **A, int32 r1, int32 r2, int32 c1, int32 c2)
Definition: matrix.c:161
int int32
32 bits
Definition: arm_cm0.h:102
void fUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagneticBuffer, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter)
Definition: magnetic.c:93
float fVz
Definition: include.h:175
float * ftrinvW[3]
Definition: include.h:191
float fBpx
Definition: include.h:159
float fThe6DOF
Definition: include.h:199
Definition: include.h:115
int32 index[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]
Definition: include.h:235
void eigencompute(float **mat, int32 n, float **eigval, float **eigvec)
Definition: matrix.c:280
void fmatrixAeqI(float **A, int32 rc)
Definition: matrix.c:109
float fVx
Definition: include.h:173
float fGpx
Definition: include.h:142
float fBpz
Definition: include.h:161
float fDelta6DOFnm2
Definition: include.h:208
void fAndroidAnglesDegFromRotationMatrix(float **R, float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg)
Definition: orientation.c:415
char int8
8 bits
Definition: arm_cm0.h:100
float q0
Definition: include.h:108
float fLPDelta6DOFnm1
Definition: include.h:210
float xinvA[3][3]
Definition: include.h:188
int16 iBfy
Definition: include.h:157
uint8 SensorDrivers_Init()
Definition: driverwrapper.c:42
struct fquaternion fLPq6DOFn
Definition: include.h:226
unsigned char uint8
The basic data types.
Definition: arm_cm0.h:96
void feCompassDirectWin8(struct SV6DOF *pthisSV6DOF, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: orientation.c:202
int16 iBy[MAGBUFFSIZE][MAGBUFFSIZE][MAGBUFFSIZE]
Definition: include.h:233
void fmatrixAeqMinusA(float **A, int32 r, int32 c)
Definition: matrix.c:144
unsigned short int uint16
16 bits
Definition: arm_cm0.h:97
float fTheDeg
Definition: include.h:131
float * finvA[3]
Definition: include.h:189
void fNEDAnglesDegFromRotationMatrix(float **R, float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg)
Definition: orientation.c:385
unsigned long int uint32
32 bits
Definition: arm_cm0.h:98
float fLPRho6DOF
Definition: include.h:205
void fmatrixAeqTrBxC(float **A, float **B, float **C, int32 rBrC, int32 cB, int32 cC)
Definition: matrix.c:62
uint8 fSixDOFSensorDrivers(struct AccelSensor *pthisAccel, struct MagSensor *pthisMag)
Definition: driverwrapper.c:56
Definition: include.h:171
float fGpy
Definition: include.h:143
float xfA[3][3]
Definition: include.h:186
void fWin8AnglesDegFromRotationMatrix(float **R, float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg)
Definition: orientation.c:446
float fDelta6DOFnm1
Definition: include.h:207
int16 iGpz
Definition: include.h:141
void feCompassDirectNED(struct SV6DOF *pthisSV6DOF, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
Definition: orientation.c:40
float ftrB
Definition: include.h:181
float fDelta6DOFn
Definition: include.h:206