38 #define ONETHIRD 0.33333333F // one third
39 #define ONESIXTH 0.166666667F // one sixth
48 pthisMagCal->
fV[
X] = pthisMagCal->
fV[
Y] = pthisMagCal->
fV[
Z] = 0.0F;
50 pthisMagCal->
fFourBsq = 4.0F * pthisMagCal->
fB * pthisMagCal->
fB;
62 pthisMagBuffer->
index[j][k] = -1;
70 for (j = 0; j < (MAGBUFFSIZEX - 1); j++)
72 pthisMagBuffer->
tanarray[j] = (
int16) (100.0F * tanf(
PI * (-0.5F + (
float) (j + 1) / MAGBUFFSIZEX)));
90 if (pthisAccel->
iGp[
X] == 0)
return;
106 for (i = X; i <=
Z; i++)
108 pthisMagBuffer->
iBp[i][j][k] = pthisMag->
iBp[i];
110 pthisMagBuffer->
index[j][k] = loopcounter;
119 for (i = X; i <=
Z; i++)
121 pthisMagBuffer->
iBp[i][j][k] = pthisMag->
iBp[i];
123 pthisMagBuffer->
index[j][k] = loopcounter;
133 if (pthisMagBuffer->
index[j][k] < i)
136 if (pthisMagBuffer->
index[j][k] != -1)
142 i = pthisMagBuffer->
index[l][m];
149 pthisMagBuffer->
index[l][m] = -1;
157 for (i = X; i <=
Z; i++)
159 pthisMagBuffer->
iBp[i][j][k] = pthisMag->
iBp[i];
161 pthisMagBuffer->
index[j][k] = loopcounter;
172 for (i = X; i <=
Z; i++)
174 idelta += abs((
int32)pthisMag->
iBp[i] - (
int32)pthisMagBuffer->
iBp[i][j][k]);
180 for (i = X; i <=
Z; i++)
182 pthisMagBuffer->
iBp[i][j][k] = pthisMag->
iBp[i];
184 pthisMagBuffer->
index[j][k] = loopcounter;
201 if (pthisMagBuffer->
index[j][k] != -1)
205 for (i = X; i <=
Z; i++)
207 idelta += abs((
int32)pthisMag->
iBp[i] - (
int32)pthisMagBuffer->
iBp[i][j][k]);
231 for (i = X; i <=
Z; i++)
233 pthisMagBuffer->
iBp[i][l][m] = pthisMag->
iBp[i];
235 pthisMagBuffer->
index[l][m] = loopcounter;
255 for (i =
X; i <=
Z; i++)
257 ftmp[i] = pthisMag->
fBp[i] - pthisMagCal->
fV[i];
260 for (i =
X; i <=
Z; i++)
262 pthisMag->
fBc[i] = pthisMagCal->
finvW[i][
X] * ftmp[
X] + pthisMagCal->
finvW[i][
Y] * ftmp[
Y] + pthisMagCal->
finvW[i][
Z] * ftmp[
Z];
324 pthisMagCal->
fB = pthisMagCal->
ftrB;
326 for (i =
X; i <=
Z; i++)
328 pthisMagCal->
fV[i] = pthisMagCal->
ftrV[i];
329 for (j =
X; j <=
Z; j++)
369 for (i = 0; i < 4; i++)
371 pthisMagCal->
fvecB[i] = 0.0F;
372 for (j = i; j < 4; j++)
374 pthisMagCal->
fmatA[i][j] = 0.0F;
379 iOffset[
X] = iOffset[
Y] = iOffset[
Z] = 0;
387 if (pthisMagBuffer->
index[j][k] != -1)
392 for (l =
X; l <=
Z; l++)
394 iOffset[l] = pthisMagBuffer->
iBp[l][j][k];
399 for (l =
X; l <=
Z; l++)
401 pthisMagCal->
fvecA[l] = (float)((
int32)pthisMagBuffer->
iBp[l][j][k] - (
int32)iOffset[l]) * fscaling;
402 pthisMagCal->
fvecA[l + 3] = pthisMagCal->
fvecA[l] * pthisMagCal->
fvecA[l];
406 fBp2 = pthisMagCal->
fvecA[3] + pthisMagCal->
fvecA[4] + pthisMagCal->
fvecA[5];
409 fSumBp4 += fBp2 * fBp2;
412 for (l =
X; l <=
Z; l++)
414 pthisMagCal->
fvecB[l] += pthisMagCal->
fvecA[l] * fBp2;
418 pthisMagCal->
fvecB[3] += fBp2;
421 pthisMagCal->
fmatA[0][0] += pthisMagCal->
fvecA[
X + 3];
425 pthisMagCal->
fmatA[1][1] += pthisMagCal->
fvecA[
Y + 3];
428 pthisMagCal->
fmatA[2][2] += pthisMagCal->
fvecA[
Z + 3];
438 pthisMagCal->
fmatA[3][3] = (float) iCount;
444 for (i = 0; i < 4; i++)
446 for (j = i; j < 4; j++)
448 pthisMagCal->
fmatB[i][j] = pthisMagCal->
fmatB[j][i] = pthisMagCal->
fmatA[j][i] = pthisMagCal->
fmatA[i][j];
453 for (i = 0; i < 4; i++)
455 pfRows[i] = pthisMagCal->
fmatB[i];
460 for (i = 0; i < 4; i++)
462 pthisMagCal->
fvecA[i] = 0.0F;
463 for (k = 0; k < 4; k++)
465 pthisMagCal->
fvecA[i] += pthisMagCal->
fmatB[i][k] * pthisMagCal->
fvecB[k];
473 for (i = 0; i < 4; i++)
475 fE += pthisMagCal->
fvecA[i] * pthisMagCal->
fvecB[i];
477 fE = fSumBp4 - 2.0F * fE;
480 for (i = 0; i < 4; i++)
482 pthisMagCal->
fvecB[i] = 0.0F;
483 for (k = 0; k < 4; k++)
485 pthisMagCal->
fvecB[i] += pthisMagCal->
fmatA[i][k] * pthisMagCal->
fvecA[k];
490 for (i = 0; i < 4; i++)
492 fE += pthisMagCal->
fvecB[i] * pthisMagCal->
fvecA[i];
496 for (l =
X; l <=
Z; l++)
498 pthisMagCal->
ftrV[l] = 0.5F * pthisMagCal->
fvecA[l];
502 pthisMagCal->
ftrB = sqrtf(pthisMagCal->
fvecA[3] + pthisMagCal->
ftrV[
X] * pthisMagCal->
ftrV[
X] +
507 (2.0F * pthisMagCal->
ftrB * pthisMagCal->
ftrB);
510 for (l =
X; l <=
Z; l++)
530 int8 i, j, k, l, m, n;
536 iOffset[
X] = iOffset[
Y] = iOffset[
Z] = 0;
539 for (m = 0; m < 7; m++)
541 for (n = m; n < 7; n++)
543 pthisMagCal->
fmatA[m][n] = 0.0F;
553 if (pthisMagBuffer->
index[j][k] != -1)
558 for (l =
X; l <=
Z; l++)
560 iOffset[l] = pthisMagBuffer->
iBp[l][j][k];
565 for (l =
X; l <=
Z; l++)
567 pthisMagCal->
fvecA[l + 3] = (float)((
int32)pthisMagBuffer->
iBp[l][j][k] - (
int32)iOffset[l]) * fscaling;
568 pthisMagCal->
fvecA[l] = pthisMagCal->
fvecA[l + 3] * pthisMagCal->
fvecA[l + 3];
575 for (m = 0; m < 6; m++)
577 pthisMagCal->
fmatA[m][6] += pthisMagCal->
fvecA[m];
580 for (m = 0; m < 6; m++)
582 for (n = m; n < 6; n++)
584 pthisMagCal->
fmatA[m][n] += pthisMagCal->
fvecA[m] * pthisMagCal->
fvecA[n];
595 pthisMagCal->
fmatA[6][6] = (float) iCount;
601 for (m = 1; m < 7; m++)
603 for (n = 0; n < m; n++)
605 pthisMagCal->
fmatA[m][n] = pthisMagCal->
fmatA[n][m];
614 for (i = 1; i < 7; i++)
616 if (pthisMagCal->
fvecA[i] < pthisMagCal->
fvecA[j])
626 for (l =
X; l <=
Z; l++)
628 pthisMagCal->
fA[l][l] = pthisMagCal->
fmatB[l][j];
629 det *= pthisMagCal->
fA[l][l];
630 pthisMagCal->
ftrV[l] = -0.5F * pthisMagCal->
fmatB[l + 3][j] / pthisMagCal->
fA[l][l];
637 pthisMagCal->
fmatB[6][j] = -pthisMagCal->
fmatB[6][j];
642 ftmp = -pthisMagCal->
fmatB[6][j];
643 for (l =
X; l <=
Z; l++)
645 ftmp += pthisMagCal->
fA[l][l] * pthisMagCal->
ftrV[l] * pthisMagCal->
ftrV[l];
659 for (l =
X; l <=
Z; l++)
661 pthisMagCal->
ftrinvW[l][l] = sqrtf(fabs(pthisMagCal->
fA[l][l]));
677 int8 i, j, k, l, m, n;
683 iOffset[
X] = iOffset[
Y] = iOffset[
Z] = 0;
686 for (m = 0; m < 10; m++)
688 for (n = m; n < 10; n++)
690 pthisMagCal->
fmatA[m][n] = 0.0F;
700 if (pthisMagBuffer->
index[j][k] != -1)
705 for (l =
X; l <=
Z; l++)
707 iOffset[l] = pthisMagBuffer->
iBp[l][j][k];
712 for (l =
X; l <=
Z; l++)
714 pthisMagCal->
fvecA[l + 6] = (float)((
int32)pthisMagBuffer->
iBp[l][j][k] - (
int32)iOffset[l]) * fscaling;
719 pthisMagCal->
fvecA[1] = 2.0F * pthisMagCal->
fvecA[6] * pthisMagCal->
fvecA[7];
720 pthisMagCal->
fvecA[2] = 2.0F * pthisMagCal->
fvecA[6] * pthisMagCal->
fvecA[8];
722 pthisMagCal->
fvecA[4] = 2.0F * pthisMagCal->
fvecA[7] * pthisMagCal->
fvecA[8];
728 for (m = 0; m < 9; m++)
730 pthisMagCal->
fmatA[m][9] += pthisMagCal->
fvecA[m];
733 for (m = 0; m < 9; m++)
735 for (n = m; n < 9; n++)
737 pthisMagCal->
fmatA[m][n] += pthisMagCal->
fvecA[m] * pthisMagCal->
fvecA[n];
748 pthisMagCal->
fmatA[9][9] = (float) iCount;
754 for (m = 1; m < 10; m++)
756 for (n = 0; n < m; n++)
758 pthisMagCal->
fmatA[m][n] = pthisMagCal->
fmatA[n][m];
767 for (i = 1; i < 10; i++)
769 if (pthisMagCal->
fvecA[i] < pthisMagCal->
fvecA[j])
774 pthisMagCal->
fA[0][0] = pthisMagCal->
fmatB[0][j];
775 pthisMagCal->
fA[0][1] = pthisMagCal->
fA[1][0] = pthisMagCal->
fmatB[1][j];
776 pthisMagCal->
fA[0][2] = pthisMagCal->
fA[2][0] = pthisMagCal->
fmatB[2][j];
777 pthisMagCal->
fA[1][1] = pthisMagCal->
fmatB[3][j];
778 pthisMagCal->
fA[1][2] = pthisMagCal->
fA[2][1] = pthisMagCal->
fmatB[4][j];
779 pthisMagCal->
fA[2][2] = pthisMagCal->
fmatB[5][j];
786 pthisMagCal->
fmatB[6][j] = -pthisMagCal->
fmatB[6][j];
787 pthisMagCal->
fmatB[7][j] = -pthisMagCal->
fmatB[7][j];
788 pthisMagCal->
fmatB[8][j] = -pthisMagCal->
fmatB[8][j];
789 pthisMagCal->
fmatB[9][j] = -pthisMagCal->
fmatB[9][j];
797 for (l =
X; l <=
Z; l++)
799 pthisMagCal->
ftrV[l] = 0.0F;
800 for (m =
X; m <=
Z; m++)
802 pthisMagCal->
ftrV[l] += pthisMagCal->
finvA[l][m] * pthisMagCal->
fmatB[m + 6][j];
804 pthisMagCal->
ftrV[l] *= -0.5F;
808 pthisMagCal->
ftrB = sqrtf(fabs(pthisMagCal->
fA[0][0] * pthisMagCal->
ftrV[
X] * pthisMagCal->
ftrV[
X] +
809 2.0F * pthisMagCal->
fA[0][1] * pthisMagCal->
ftrV[
X] * pthisMagCal->
ftrV[
Y] +
810 2.0F * pthisMagCal->
fA[0][2] * pthisMagCal->
ftrV[
X] * pthisMagCal->
ftrV[
Z] +
811 pthisMagCal->
fA[1][1] * pthisMagCal->
ftrV[
Y] * pthisMagCal->
ftrV[
Y] +
812 2.0F * pthisMagCal->
fA[1][2] * pthisMagCal->
ftrV[
Y] * pthisMagCal->
ftrV[
Z] +
813 pthisMagCal->
fA[2][2] * pthisMagCal->
ftrV[
Z] * pthisMagCal->
ftrV[
Z] - pthisMagCal->
fmatB[9][j]));
817 (pthisMagCal->
ftrB * pthisMagCal->
ftrB);
820 for (l =
X; l <=
Z; l++)
835 for (i = 0; i < 3; i++)
837 for (j = 0; j < 3; j++)
839 pthisMagCal->
fmatA[i][j] = pthisMagCal->
fA[i][j];
845 for (j = 0; j < 3; j++)
847 ftmp = sqrtf(sqrtf(fabs(pthisMagCal->
fvecA[j])));
848 for (i = 0; i < 3; i++)
850 pthisMagCal->
fmatB[i][j] *= ftmp;
857 for (i = 0; i < 3; i++)
860 for (j = i; j < 3; j++)
862 pthisMagCal->
ftrinvW[i][j] = 0.0F;
864 for (k = 0; k < 3; k++)
866 pthisMagCal->
ftrinvW[i][j] += pthisMagCal->
fmatB[i][k] * pthisMagCal->
fmatB[j][k];
int16 iBp[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
int16 tanarray[MAGBUFFSIZEX-1]
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
void f3x3matrixAeqMinusA(float A[][3])
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
float f3x3matrixDetA(float A[][3])
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
int32 index[MAGBUFFSIZEX][MAGBUFFSIZEY]
#define FITERRORAGINGSECS
#define MAG_OVERSAMPLE_RATIO
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct AccelSensor *pthisAccel, struct MagSensor *pthisMag, int32 loopcounter)
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
long int32
This defines int32 as long.
short int16
This defines int16 as short.
void f3x3matrixAeqI(float A[][3])
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)
#define MINMEASUREMENTS10CAL
#define MINMEASUREMENTS7CAL