45 #define FQVA_6DOF_GY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
46 #define FQVG_6DOF_GY_KALMAN 0.3F // gyro noise (deg/s)^2
47 #define FQWB_6DOF_GY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
48 #define FQWA_6DOF_GY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
50 #define FQWINITTHTH_6DOF_GY_KALMAN 2000E-5F // th_e * th_e terms
51 #define FQWINITBB_6DOF_GY_KALMAN 250E-3F // for FXAS21000: b_e * b_e terms
52 #define FQWINITTHB_6DOF_GY_KALMAN 0.0F // th_e * b_e terms
53 #define FQWINITAA_6DOF_GY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
55 #define FCA_6DOF_GY_KALMAN 0.5F // linear acceleration decay factor
61 #define FQVA_9DOF_GBY_KALMAN 2E-6F // accelerometer noise g^2 so 1.4mg RMS
62 #define FQVM_9DOF_GBY_KALMAN 0.1F // magnetometer noise uT^2
63 #define FQVG_9DOF_GBY_KALMAN 0.3F // gyro noise (deg/s)^2
64 #define FQWB_9DOF_GBY_KALMAN 1E-9F // gyro offset drift (deg/s)^2: 1E-9 implies 0.09deg/s max at 50Hz
65 #define FQWA_9DOF_GBY_KALMAN 1E-4F // linear acceleration drift g^2 (increase slows convergence to g but reduces sensitivity to shake)
66 #define FQWD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance drift uT^2 (increase slows convergence to B but reduces sensitivity to magnet)
68 #define FQWINITTHTH_9DOF_GBY_KALMAN 2000E-5F // th_e * th_e terms
69 #define FQWINITBB_9DOF_GBY_KALMAN 250E-3F // b_e * b_e terms
70 #define FQWINITTHB_9DOF_GBY_KALMAN 0.0F // th_e * b_e terms
71 #define FQWINITAA_9DOF_GBY_KALMAN 10E-5F // a_e * a_e terms (increase slows convergence to g but reduces sensitivity to shake)
72 #define FQWINITDD_9DOF_GBY_KALMAN 600E-3F // d_e * d_e terms (increase slows convergence to B but reduces sensitivity to magnet)
74 #define FCA_9DOF_GBY_KALMAN 0.5F // linear acceleration decay factor
75 #define FCD_9DOF_GBY_KALMAN 0.5F // magnetic disturbance decay factor
77 #define SINDELTAMAX 0.9063078F // sin of max +ve geomagnetic inclination angle: here 65.0 deg
78 #define COSDELTAMAX 0.4226183F // cos of max +ve geomagnetic inclination angle: here 65.0 deg
84 pthisSV->
fdeltat = (float) iOverSampleRatio / (
float) iSensorFS;
86 if (pthisSV->
flpf > 1.0F)
100 pthisSV->
fdeltat = (float) iOverSampleRatio / (
float) iSensorFS;
103 if (flpftimesecs > pthisSV->
fdeltat)
106 pthisSV->
flpf = 1.0F;
117 pthisSV->
fdeltat = (float) iOverSampleRatio / (
float) iSensorFS;
120 if (flpftimesecs > pthisSV->
fdeltat)
123 pthisSV->
flpf = 1.0F;
151 pthisSV->
fdeltat = (float) iOverSampleRatio / (
float) iSensorFS;
154 if (flpftimesecs > pthisSV->
fdeltat)
157 pthisSV->
flpf = 1.0F;
181 for (i = 0; i < 3; i++)
183 for (j = 0; j < 9; j++)
185 pthisSV->
fC3x9[i][j]= 0.0F;
188 pthisSV->
fC3x9[0][6] = pthisSV->
fC3x9[1][7] = pthisSV->
fC3x9[2][8] = 1.0F;
193 for (i =
X; i <=
Z; i++)
206 for (i = 0; i < 9; i++)
208 for (j = 0; j < 9; j++)
210 pthisSV->
fQw9x9[i][j] = 0.0F;
214 for (i = 0; i < 3; i++)
250 for (i = 0; i < 6; i++)
252 for (j = 0; j < 12; j++)
254 pthisSV->
fC6x12[i][j]= 0.0F;
263 for (i =
X; i <=
Z; i++)
271 if (ithisCoordSystem ==
NED)
275 pthisSV->
fmGl[
Y] = 0.0F;
276 pthisSV->
fmGl[
Z] = 0.0F;
281 pthisSV->
fmGl[
X] = 0.0F;
283 pthisSV->
fmGl[
Z] = 0.0F;
293 for (i = 0; i < 12; i++)
295 for (j = 0; j < 12; j++)
301 for (i = 0; i < 3; i++)
350 if (ithisCoordSystem ==
NED)
355 else if (ithisCoordSystem ==
ANDROID)
377 if (ithisCoordSystem ==
NED)
383 else if (ithisCoordSystem ==
ANDROID)
413 if (ithisCoordSystem ==
NED)
418 else if (ithisCoordSystem ==
ANDROID)
438 if (ithisCoordSystem ==
NED)
444 else if (ithisCoordSystem ==
ANDROID)
474 for (i =
X; i <=
Z; i++)
480 for (j = 0; j < iOverSampleRatio; j++)
483 for (i =
X; i <=
Z; i++)
509 if (ithisCoordSystem ==
NED)
513 &(pthisSV->
fRho), &(pthisSV->
fChi));
515 else if (ithisCoordSystem ==
ANDROID)
519 &(pthisSV->
fRho), &(pthisSV->
fChi));
525 &(pthisSV->
fRho), &(pthisSV->
fChi));
542 if (ithisCoordSystem ==
NED)
547 else if (ithisCoordSystem ==
ANDROID)
567 if (ithisCoordSystem ==
NED)
573 else if (ithisCoordSystem ==
ANDROID)
594 int16 ithisCoordSystem,
int16 iOverSampleRatio)
598 float ftmpA9x3[9][3];
633 if (ithisCoordSystem ==
NED)
638 else if (ithisCoordSystem ==
ANDROID)
664 for (j = 0; j < iOverSampleRatio; j++)
667 for (i =
X; i <=
Z; i++)
690 for (i =
X; i <=
Z; i++)
692 if (ithisCoordSystem ==
NED)
707 if ((ithisCoordSystem ==
NED) || (ithisCoordSystem ==
WIN8))
747 for (i = 0; i < 9; i++)
750 pftmpA9x3ij = ftmpA9x3[i];
752 for (j = 0; j < 3; j++)
758 pfC3x9jk = pthisSV->
fC3x9[j];
761 pfQw9x9ik = pthisSV->
fQw9x9[i];
764 for (k = 0; k < 9; k++)
766 if ((*pfQw9x9ik != 0.0F) && (*pfC3x9jk != 0.0F))
768 if (*pfC3x9jk == 1.0F)
769 *pftmpA9x3ij += *pfQw9x9ik;
770 else if (*pfC3x9jk == -1.0F)
771 *pftmpA9x3ij -= *pfQw9x9ik;
773 *pftmpA9x3ij += *pfQw9x9ik * *pfC3x9jk;
791 for (i = 0; i < 3; i++)
794 pfPPlus9x9ij = pthisSV->
fPPlus9x9[i] + i;
796 for (j = i; j < 3; j++)
799 *pfPPlus9x9ij = 0.0F;
802 pfC3x9ik = pthisSV->
fC3x9[i];
805 pftmpA9x3kj = *ftmpA9x3 + j;
808 for (k = 0; k < 9; k++)
810 if ((*pfC3x9ik != 0.0F) && (*pftmpA9x3kj != 0.0F))
812 if (*pfC3x9ik == 1.0F)
813 *pfPPlus9x9ij += *pftmpA9x3kj;
814 else if (*pfC3x9ik == -1.0F)
815 *pfPPlus9x9ij -= *pftmpA9x3kj;
817 *pfPPlus9x9ij += *pfC3x9ik * *pftmpA9x3kj;
843 for (i = 0; i < 3; i++)
853 for (i = 0; i < 9; i++)
856 pfK9x3ij = pthisSV->
fK9x3[i];
858 for (j = 0; j < 3; j++)
864 pftmpA9x3ik = ftmpA9x3[i];
870 for (k = 0; k < 3; k++)
872 if (*pftmpA9x3ik != 0.0F)
874 *pfK9x3ij += *pftmpA9x3ik * *pfPPlus9x9kj;
894 for (i =
X; i <=
Z; i++)
900 for (k = 0; k < 3; k++)
930 for (i =
X; i <=
Z; i++)
939 for (i =
X; i <=
Z; i++)
948 if (ithisCoordSystem ==
NED)
954 else if (ithisCoordSystem ==
ANDROID)
977 for (i = 0; i < 3; i++)
982 for (j = 0; j < 9; j++)
985 *pfPPlus9x9ij = 0.0F;
988 pfC3x9ik = pthisSV->
fC3x9[i];
991 pfQw9x9kj = &pthisSV->
fQw9x9[0][j];
994 for (k = 0; k < 9; k++)
996 if ((*pfC3x9ik != 0.0F) && (*pfQw9x9kj != 0.0F))
998 if (*pfC3x9ik == 1.0F)
999 *pfPPlus9x9ij += *pfQw9x9kj;
1000 else if (*pfC3x9ik == -1.0F)
1001 *pfPPlus9x9ij -= *pfQw9x9kj;
1003 *pfPPlus9x9ij += *pfC3x9ik * *pfQw9x9kj;
1022 for (i = 0; i < 9; i++)
1025 pfQw9x9ij = pthisSV->
fQw9x9[i] + i;
1027 for (j = i; j < 9; j++)
1030 pfK9x3ik = pthisSV->
fK9x3[i];
1036 for (k = 0; k < 3; k++)
1039 if (*pfPPlus9x9kj != 0.0F)
1041 *pfQw9x9ij -= *pfK9x3ik * *pfPPlus9x9kj;
1059 for (i = 0; i < 9; i++)
1062 pfPPlus9x9ij = pthisSV->
fPPlus9x9[i] + i;
1063 pfQw9x9ij = pthisSV->
fQw9x9[i] + i;
1066 *(pfPPlus9x9ij++) = *(pfQw9x9ij++);
1069 for (j = i + 1; j < 9; j++)
1071 *(pfPPlus9x9ij++)= pthisSV->
fPPlus9x9[j][i] = *(pfQw9x9ij++);
1082 for (i = 0; i < 9; i++)
1084 for (j = 0; j < 9; j++)
1086 pthisSV->
fQw9x9[i][j] = 0.0F;
1091 for (i = 0; i < 3; i++)
1114 float fopp, fadj, fhyp;
1115 float fsindelta, fcosdelta;
1118 float ftmpA12x6[12][6];
1123 float *pfPPlus12x12ij;
1124 float *pfPPlus12x12kj;
1130 float *pftmpA12x6ik;
1131 float *pftmpA12x6kj;
1132 float *pftmpA12x6ij;
1157 if (ithisCoordSystem ==
NED)
1161 else if (ithisCoordSystem ==
ANDROID)
1165 else if (ithisCoordSystem ==
WIN8)
1185 for (j = 0; j < iOverSampleRatio; j++)
1188 for (i =
X; i <=
Z; i++)
1210 for (i =
X; i <=
Z; i++)
1214 if (ithisCoordSystem ==
NED)
1229 if ((ithisCoordSystem ==
NED) || (ithisCoordSystem ==
WIN8))
1242 if (ithisCoordSystem ==
NED)
1297 for (i = 0; i < 12; i++)
1300 pftmpA12x6ij = ftmpA12x6[i];
1302 for (j = 0; j < 6; j++)
1305 *pftmpA12x6ij = 0.0F;
1308 pfC6x12jk = pthisSV->
fC6x12[j];
1311 pfQw12x12ik = pthisSV->
fQw12x12[i];
1314 for (k = 0; k < 12; k++)
1316 if ((*pfQw12x12ik != 0.0F) && (*pfC6x12jk != 0.0F))
1318 if (*pfC6x12jk == 1.0F)
1319 *pftmpA12x6ij += *pfQw12x12ik;
1320 else if (*pfC6x12jk == -1.0F)
1321 *pftmpA12x6ij -= *pfQw12x12ik;
1323 *pftmpA12x6ij += *pfQw12x12ik * *pfC6x12jk;
1341 for (i = 0; i < 6; i++)
1346 for (j = i; j < 6; j++)
1349 *pfPPlus12x12ij = 0.0F;
1352 pfC6x12ik = pthisSV->
fC6x12[i];
1355 pftmpA12x6kj = *ftmpA12x6 + j;
1358 for (k = 0; k < 12; k++)
1360 if ((*pfC6x12ik != 0.0F) && (*pftmpA12x6kj != 0.0F))
1362 if (*pfC6x12ik == 1.0F)
1363 *pfPPlus12x12ij += *pftmpA12x6kj;
1364 else if (*pfC6x12ik == -1.0F)
1365 *pfPPlus12x12ij -= *pftmpA12x6kj;
1367 *pfPPlus12x12ij += *pfC6x12ik * *pftmpA12x6kj;
1391 for (i = 1; i < 6; i++)
1392 for (j = 0; j < i; j++)
1396 for (i = 0; i < 6; i++)
1406 for (i = 0; i < 12; i++)
1409 pfK12x6ij = pthisSV->
fK12x6[i];
1411 for (j = 0; j < 6; j++)
1417 pftmpA12x6ik = ftmpA12x6[i];
1423 for (k = 0; k < 6; k++)
1425 if (*pftmpA12x6ik != 0.0F)
1427 *pfK12x6ij += *pftmpA12x6ik * *pfPPlus12x12kj;
1432 pfPPlus12x12kj += 12;
1448 for (i =
X; i <=
Z; i++)
1475 for (i =
X; i <=
Z; i++)
1512 for (i =
X; i <=
Z; i++)
1521 for (i =
X; i <=
Z; i++)
1536 switch (ithisCoordSystem)
1562 if (ithisCoordSystem ==
NED)
1579 fhyp = sqrtf(fopp * fopp + fadj * fadj);
1586 fsindelta = fopp * ftmp;
1587 fcosdelta = fadj * ftmp;
1603 pthisSV->
fmGl[
X] = pthisMagCal->
fB * fcosdelta;
1604 pthisSV->
fmGl[
Z] = pthisMagCal->
fB * fsindelta;
1624 fhyp = sqrtf(fopp * fopp + fadj * fadj);
1631 fsindelta = fopp * ftmp;
1632 fcosdelta = fadj * ftmp;
1648 pthisSV->
fmGl[
Y] = pthisMagCal->
fB * fcosdelta;
1649 pthisSV->
fmGl[
Z] = -pthisMagCal->
fB * fsindelta;
1658 if (ithisCoordSystem ==
NED)
1664 else if (ithisCoordSystem ==
ANDROID)
1687 for (i = 0; i < 6; i++)
1692 for (j = 0; j < 12; j++)
1695 *pfPPlus12x12ij = 0.0F;
1698 pfC6x12ik = pthisSV->
fC6x12[i];
1701 pfQw12x12kj = &pthisSV->
fQw12x12[0][j];
1704 for (k = 0; k < 12; k++)
1706 if ((*pfC6x12ik != 0.0F) && (*pfQw12x12kj != 0.0F))
1708 if (*pfC6x12ik == 1.0F)
1709 *pfPPlus12x12ij += *pfQw12x12kj;
1710 else if (*pfC6x12ik == -1.0F)
1711 *pfPPlus12x12ij -= *pfQw12x12kj;
1713 *pfPPlus12x12ij += *pfC6x12ik * *pfQw12x12kj;
1732 for (i = 0; i < 12; i++)
1735 pfQw12x12ij = pthisSV->
fQw12x12[i] + i;
1737 for (j = i; j < 12; j++)
1740 pfK12x6ik = pthisSV->
fK12x6[i];
1746 for (k = 0; k < 6; k++)
1749 if (*pfPPlus12x12kj != 0.0F)
1751 *pfQw12x12ij -= *pfK12x6ik * *pfPPlus12x12kj;
1756 pfPPlus12x12kj += 12;
1769 for (i = 0; i < 12; i++)
1773 pfQw12x12ij = pthisSV->
fQw12x12[i] + i;
1776 *(pfPPlus12x12ij++) = *(pfQw12x12ij++);
1779 for (j = i + 1; j < 12; j++)
1781 *(pfPPlus12x12ij++)= pthisSV->
fPPlus12x12[j][i] = *(pfQw12x12ij++);
1792 for (i = 0; i < 12; i++)
1794 for (j = 0; j < 12; j++)
1801 for (i = 0; i < 3; i++)
int16 iFirstOrientationLock
#define FQWINITBB_9DOF_GBY_KALMAN
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
void f3DOFTiltNED(float fR[][3], float fGp[])
#define FCD_9DOF_GBY_KALMAN
#define FQWA_9DOF_GBY_KALMAN
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
#define FQWB_9DOF_GBY_KALMAN
void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
struct fquaternion fDeltaq
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)
void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
int16 iFirstOrientationLock
void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
#define FQWINITDD_9DOF_GBY_KALMAN
struct fquaternion fDeltaq
void fqAeq1(struct fquaternion *pqA)
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, int32 loopcounter, int16 ithisCoordSystem)
#define FQVM_9DOF_GBY_KALMAN
#define FQWD_9DOF_GBY_KALMAN
#define FQWINITTHTH_6DOF_GY_KALMAN
#define GYRO_OVERSAMPLE_RATIO
void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
#define FQWINITTHTH_9DOF_GBY_KALMAN
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
#define MAG_OVERSAMPLE_RATIO
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro, int16 ithisCoordSystem, int16 iOverSampleRatio)
#define FQVA_9DOF_GBY_KALMAN
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGp[])
void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, int16 ithisCoordSystem, int16 iSensorFS, int16 iOverSampleRatio)
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
#define FQVG_9DOF_GBY_KALMAN
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro, int32 loopcounter, int16 ithisCoordSystem, int16 iOverSampleRatio)
#define FQWINITAA_9DOF_GBY_KALMAN
#define FQWA_6DOF_GY_KALMAN
void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, float flpftimesecs, int16 iSensorFS, int16 iOverSampleRatio)
float fYpBuffer[GYRO_OVERSAMPLE_RATIO][3]
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGp[])
#define FCA_6DOF_GY_KALMAN
void fqAeqNormqA(struct fquaternion *pqA)
void f3DOFTiltAndroid(float fR[][3], float fGp[])
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, int16 iSensorFS, int16 iOverSampleRatio)
#define FQWINITTHB_6DOF_GY_KALMAN
#define FQWB_6DOF_GY_KALMAN
#define FQVG_6DOF_GY_KALMAN
long int32
This defines int32 as long.
short int16
This defines int16 as short.
void f3x3matrixAeqI(float A[][3])
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
float fPPlus12x12[12][12]
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[], int32 loopcounter)
#define FQWINITBB_6DOF_GY_KALMAN
#define ACCEL_OVERSAMPLE_RATIO
struct fquaternion fDeltaq
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
#define FQWINITTHB_9DOF_GBY_KALMAN
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, int32 loopcounter)
#define FCA_9DOF_GBY_KALMAN
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
#define FQWINITAA_6DOF_GY_KALMAN
#define FQVA_6DOF_GY_KALMAN
void fLPFScalar(float *pfS, float *pfLPS, float flpf, int32 loopcounter)
void f3DOFTiltWin8(float fR[][3], float fGp[])
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel, int32 loopcounter, int16 ithisCoordSystem)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)