41 extern struct ProjectGlobals
globals;
56 #if defined COMPUTE_1DOF_P_BASIC
59 #if defined COMPUTE_3DOF_G_BASIC
62 #if defined COMPUTE_3DOF_B_BASIC
65 #if defined COMPUTE_3DOF_Y_BASIC
68 #if defined COMPUTE_6DOF_GB_BASIC
71 #if defined COMPUTE_6DOF_GY_KALMAN
74 #if defined COMPUTE_9DOF_GBY_KALMAN
91 if (pthisSV->
flpf > 1.0F)
97 pthisSV->
fLPH = pthisPressure->
fH;
98 pthisSV->
fLPT = pthisPressure->
fT;
112 if (flpftimesecs > pthisSV->
fdeltat)
115 pthisSV->
flpf = 1.0F;
134 if (
globals.DefaultQuaternionPacketType <
Q3)
149 if (flpftimesecs > pthisSV->
fdeltat)
152 pthisSV->
flpf = 1.0F;
171 if (
globals.DefaultQuaternionPacketType <
Q3M)
191 if (
globals.DefaultQuaternionPacketType <
Q3G)
206 if (flpftimesecs > pthisSV->
fdeltat)
209 pthisSV->
flpf = 1.0F;
250 for (i =
CHX; i <=
CHZ; i++)
252 pthisSV->
fbPl[i] = 0.0F;
302 for (i =
CHX; i <=
CHZ; i++)
304 pthisSV->
fbPl[i] = 0.0F;
313 for (i =
CHX; i <=
CHZ; i++)
315 pthisSV->
fVelGl[i] = 0.0F;
316 pthisSV->
fDisGl[i] = 0.0F;
347 if (
globals.DefaultQuaternionPacketType <
Q9)
373 pthisSV->
fLPH += pthisSV->
flpf * (pthisPressure->
fH - pthisSV->
fLPH);
374 pthisSV->
fLPT += pthisSV->
flpf * (pthisPressure->
fT - pthisSV->
fLPT);
506 for (i =
CHX; i <=
CHZ; i++)
534 &(pthisSV->
fRho), &(pthisSV->
fChi));
538 &(pthisSV->
fRho), &(pthisSV->
fChi));
543 &(pthisSV->
fRho), &(pthisSV->
fChi));
610 float ftmp3DOF3x1[3];
611 float ftmpA3x3[3][3];
635 fqMi = pthisSV->
fqPl;
640 for (i =
CHX; i <=
CHZ; i++)
654 if (fmodSqGs != 0.0F)
657 ftmp = 1.0F / sqrt(fabsf(fmodSqGs));
665 ftmp3DOF3x1[
CHX] = 0.0F;
666 ftmp3DOF3x1[
CHY] = 0.0F;
667 ftmp3DOF3x1[
CHZ] = 1.0F;
678 ftmp3DOF3x1[
CHX] = -ftmp3DOF3x1[
CHX];
679 ftmp3DOF3x1[
CHY] = -ftmp3DOF3x1[
CHY];
680 ftmp3DOF3x1[
CHZ] = -ftmp3DOF3x1[
CHZ];
689 ftmpMi3x1[
CHX] = 2.0F * (fqMi.
q1 * fqMi.
q3 - fqMi.
q0 * fqMi.
q2);
690 ftmpMi3x1[
CHY] = 2.0F * (fqMi.
q2 * fqMi.
q3 + fqMi.
q0 * fqMi.
q1);
691 ftmpMi3x1[
CHZ] = 2.0F * (fqMi.
q0 * fqMi.
q0 + fqMi.
q3 * fqMi.
q3) - 1.0F;
703 ftmpMi3x1[
CHX] = -ftmpMi3x1[
CHX];
704 ftmpMi3x1[
CHY] = -ftmpMi3x1[
CHY];
705 ftmpMi3x1[
CHZ] = -ftmpMi3x1[
CHZ];
719 for (i = 0; i < 6; i++)
720 for (j = 0; j < 6; j++)
721 pthisSV->
fQw6x6[i][j] = 0.0F;
747 for (i = 0; i < 6; i++)
749 for (j = 0; j < 3; j++)
753 for (k = 0; k < 6; k++)
757 if (k == j) fC3x6jk = 1.0F;
760 if ((pthisSV->
fQw6x6[i][k] != 0.0F) && (fC3x6jk != 0.0F))
772 for (i = 0; i < 3; i++)
774 for (j = i; j < 3; j++)
778 ftmpA3x3[i][j] = pthisSV->
fQv;
780 ftmpA3x3[i][j] = 0.0F;
782 for (k = 0; k < 6; k++)
786 if (k == i) fC3x6ik = 1.0F;
790 if ((fC3x6ik != 0.0F) && (pthisSV->
fQwCT6x3[k][j] != 0.0F))
793 ftmpA3x3[i][j] += pthisSV->
fQwCT6x3[k][j];
795 ftmpA3x3[i][j] += fC3x6ik * pthisSV->
fQwCT6x3[k][j];
801 ftmpA3x3[1][0] = ftmpA3x3[0][1];
802 ftmpA3x3[2][0] = ftmpA3x3[0][2];
803 ftmpA3x3[2][1] = ftmpA3x3[1][2];
806 for (i = 0; i < 3; i++)
807 pfRows[i] = ftmpA3x3[i];
814 for (i = 0; i < 6; i++)
816 for (j = 0; j < 3; j++)
818 pthisSV->
fK6x3[i][j] = 0.0F;
819 for (k = 0; k < 3; k++)
821 if ((pthisSV->
fQwCT6x3[i][k] != 0.0F) && (ftmpA3x3[k][j] != 0.0F))
823 pthisSV->
fK6x3[i][j] += pthisSV->
fQwCT6x3[i][k] * ftmpA3x3[k][j];
832 for (i = 0; i < 6; i++)
834 for (j = 0; j < 3; j++)
836 pthisSV->
fK6x3[i][j] = 0.0F;
843 for (i =
CHX; i <=
CHZ; i++)
854 if (pthisSV->
fK6x3[i + 3][
CHX] != 0.0F)
866 ftmp = ftmpq.
q1 * ftmpq.
q1 + ftmpq.
q2 * ftmpq.
q2 + ftmpq.
q3 * ftmpq.
q3;
871 ftmpq.
q0 = sqrtf(fabsf(1.0F - ftmp));
888 for (i =
CHX; i <=
CHZ; i++)
944 float ftmpA7x7[7][7];
948 float ftmp6DOF3x1[3];
976 fqMi = pthisSV->
fqPl;
981 for (i =
CHX; i <=
CHZ; i++)
1000 &fDelta6DOF, pthisMag->
fBcAvg, pthisAccel->
fGsAvg, &fQvBQd, &fQvGQa);
1004 &fDelta6DOF, pthisMag->
fBcAvg, pthisAccel->
fGsAvg, &fQvBQd, &fQvGQa);
1009 &fDelta6DOF, pthisMag->
fBcAvg, pthisAccel->
fGsAvg, &fQvBQd, &fQvGQa);
1021 fqMi = pthisSV->
fqPl = fq6DOF;
1085 for (i = 0; i < 10; i++)
1086 for (j = i; j < 10; j++)
1119 for (i = 1; i < 10; i++)
1120 for (j = 0; j < i; j++)
1125 ftmp = fQvBQd / (pthisMagCal->
fB * pthisMagCal->
fB);
1132 for (i = 0; i < 10; i++)
1134 for (j = 0; j < 7; j++)
1138 for (k = 0; k < 10; k++)
1145 if (k == j) fC7x10jk = 1.0F;
1146 if (k == (j + 6)) fC7x10jk = -pthisSV->
fAlphaOver2;
1151 if (k == j) fC7x10jk = 1.0F;
1152 if (k == (j + 3)) fC7x10jk = -pthisSV->
fAlphaOver2;
1155 else if (k == 9) fC7x10jk = 1.0F;
1158 if ((pthisSV->
fQw10x10[i][k] != 0.0F) && (fC7x10jk != 0.0F))
1160 if (fC7x10jk == 1.0F)
1170 for (i = 0; i < 7; i++)
1172 for (j = i; j < 7; j++)
1176 ftmpA7x7[i][j] = pthisSV->
fQv7x1[i];
1178 ftmpA7x7[i][j] = 0.0F;
1180 for (k = 0; k < 10; k++)
1187 if (k == i) fC7x10ik = 1.0F;
1188 if (k == (i + 6)) fC7x10ik = -pthisSV->
fAlphaOver2;
1193 if (k == i) fC7x10ik = 1.0F;
1194 if (k == (i + 3)) fC7x10ik = -pthisSV->
fAlphaOver2;
1197 else if (k == 9) fC7x10ik = 1.0F;
1200 if ((fC7x10ik != 0.0F) && (pthisSV->
fQwCT10x7[k][j] != 0.0F))
1202 if (fC7x10ik == 1.0F)
1203 ftmpA7x7[i][j] += pthisSV->
fQwCT10x7[k][j];
1205 ftmpA7x7[i][j] += fC7x10ik * pthisSV->
fQwCT10x7[k][j];
1211 for (i = 1; i < 7; i++)
1213 for (j = 0; j < i; j++)
1215 ftmpA7x7[i][j] = ftmpA7x7[j][i];
1220 for (i = 0; i < 7; i++)
1221 pfRows[i] = ftmpA7x7[i];
1228 for (i = 0; i < 10; i++)
1230 for (j = 0; j < 7; j++)
1232 pthisSV->
fK10x7[i][j] = 0.0F;
1233 for (k = 0; k < 7; k++)
1235 if ((pthisSV->
fQwCT10x7[i][k] != 0.0F) && (ftmpA7x7[k][j] != 0.0F))
1246 for (i = 0; i < 10; i++)
1248 for (j = 0; j < 7; j++)
1250 pthisSV->
fK10x7[i][j] = 0.0F;
1257 for (i =
CHX; i <=
CHZ; i++)
1260 for (j = 0; j < 7; j++)
1263 if (pthisSV->
fK10x7[i][j] != 0.0F)
1267 if (pthisSV->
fK10x7[i + 3][j] != 0.0F)
1271 if (pthisSV->
fK10x7[i + 6][j] != 0.0F)
1278 for (j = 0; j < 7; j++)
1280 if (pthisSV->
fK10x7[9][j] != 0.0F)
1290 ftmpq.
q0 = sqrtf(fabsf(1.0F - ftmp));
1316 ftmpq.
q0 = sqrtf(fabsf(1.0F - ftmp));
1343 for (i =
CHX; i <=
CHZ; i++)
1390 for (i =
CHX; i <=
CHZ; i++)
void fRun_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure)
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
void fLeastSquareseCompassWin8(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
void fRun_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct MagSensor *pthisMag, struct AccelSensor *pthisAccel)
void f3x3matrixAeqB(float A[][3], float B[][3])
#define FQWB_9DOF_GBY_KALMAN
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fveqconjgquq(struct fquaternion *pfq, float fu[], float fv[])
struct SV_3DOF_B_BASIC thisSV_3DOF_B_BASIC
#define FGYRO_OFFSET_MIN_9DOF_GBY_KALMAN
#define FQVY_6DOF_GY_KALMAN
void fRun_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag)
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)
void fqAeq1(struct fquaternion *pqA)
#define FQWDLT_9DOF_GBY_KALMAN
float fAlphaOver2SqQvYQwb
struct SV_6DOF_GY_KALMAN thisSV_6DOF_GY_KALMAN
void feCompassNED(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void f3DOFTiltNED(float fR[][3], float fGs[])
#define FGYRO_OFFSET_MAX_9DOF_GBY_KALMAN
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
void fRotationVectorDegFromQuaternion(struct fquaternion *pq, float rvecdeg[])
int16 iYsBuffer[GYRO_OVERSAMPLE_RATIO][3]
void fLPFOrientationQuaternion(struct fquaternion *pq, struct fquaternion *pLPq, float flpf, float fdeltat, float fOmega[])
void fInit_3DOF_B_BASIC(struct SV_3DOF_B_BASIC *pthisSV, struct MagSensor *pthisMag, float flpftimesecs)
void fInit_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel)
void fRun_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV, struct GyroSensor *pthisGyro)
void fLeastSquareseCompassAndroid(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
void fInit_6DOF_GB_BASIC(struct SV_6DOF_GB_BASIC *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, float flpftimesecs)
void qAeqBxC(struct fquaternion *pqA, const struct fquaternion *pqB, const struct fquaternion *pqC)
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void f3DOFTiltAndroid(float fR[][3], float fGs[])
void fInit_1DOF_P_BASIC(struct SV_1DOF_P_BASIC *pthisSV, struct PressureSensor *pthisPressure, float flpftimesecs)
struct ProjectGlobals globals
struct SV_3DOF_Y_BASIC thisSV_3DOF_Y_BASIC
#define FQVBQD_MIN_9DOF_GBY_KALMAN
void fqAeqNormqA(struct fquaternion *pqA)
#define FQVY_9DOF_GBY_KALMAN
The fusion_config.h file contains additional static configuration for the Sensor Fusion based Virtual...
#define FQWB_6DOF_GY_KALMAN
void fQuaternionFromRotationVectorDeg(struct fquaternion *pq, const float rvecdeg[], float fscaling)
void fInit_3DOF_Y_BASIC(struct SV_3DOF_Y_BASIC *pthisSV)
void qAeqAxB(struct fquaternion *pqA, const struct fquaternion *pqB)
struct SV_1DOF_P_BASIC thisSV_1DOF_P_BASIC
void fRun_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct GyroSensor *pthisGyro, struct MagCalibration *pthisMagCal)
void fRun_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel)
float fAlphaOver2SqQvYQwb
void f3DOFTiltWin8(float fR[][3], float fGs[])
struct SV_3DOF_G_BASIC thisSV_3DOF_G_BASIC
void f3x3matrixAeqI(float A[][3])
void fInit_9DOF_GBY_KALMAN(struct SV_9DOF_GBY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
void fInit_3DOF_G_BASIC(struct SV_3DOF_G_BASIC *pthisSV, struct AccelSensor *pthisAccel, float flpftimesecs)
struct SV_6DOF_GB_BASIC thisSV_6DOF_GB_BASIC
struct SV_9DOF_GBY_KALMAN thisSV_9DOF_GBY_KALMAN
void feCompassAndroid(float fR[][3], float *pfDelta, float fBc[], float fGs[])
void fRun_6DOF_GY_KALMAN(struct SV_6DOF_GY_KALMAN *pthisSV, struct AccelSensor *pthisAccel, struct GyroSensor *pthisGyro)
void fRotationMatrixFromQuaternion(float R[][3], const struct fquaternion *pq)
void fQuaternionFromRotationMatrix(float R[][3], struct fquaternion *pq)
void fLeastSquareseCompassNED(struct fquaternion *pfq, float fB, float fDelta, float fsinDelta, float fcosDelta, float *pfDelta6DOF, float fBc[], float fGs[], float *pfQvBQd, float *pfQvGQa)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg, float *pfRhoDeg, float *pfChiDeg)