44 pthisMagCal->
fV[
CHX] = pthisMagCal->
fV[
CHY] = pthisMagCal->
fV[
CHZ] = 0.0F;
57 pthisMagBuffer->
index[j][k] = -1;
65 for (j = 0; j < (MAGBUFFSIZEX - 1); j++)
67 pthisMagBuffer->
tanarray[j] = (
int16) (100.0F * tanf(
PI * (-0.5F + (
float) (j + 1) / MAGBUFFSIZEX)));
101 for (i =
CHX; i <=
CHZ; i++)
103 pthisMagBuffer->
iBs[i][j][k] = pthisMag->
iBs[i];
105 pthisMagBuffer->
index[j][k] = loopcounter;
114 for (i =
CHX; i <=
CHZ; i++)
116 pthisMagBuffer->
iBs[i][j][k] = pthisMag->
iBs[i];
118 pthisMagBuffer->
index[j][k] = loopcounter;
128 if (pthisMagBuffer->
index[j][k] < i)
131 if (pthisMagBuffer->
index[j][k] != -1)
137 i = pthisMagBuffer->
index[l][m];
144 pthisMagBuffer->
index[l][m] = -1;
152 for (i =
CHX; i <=
CHZ; i++)
154 pthisMagBuffer->
iBs[i][j][k] = pthisMag->
iBs[i];
156 pthisMagBuffer->
index[j][k] = loopcounter;
167 for (i =
CHX; i <=
CHZ; i++)
169 idelta += abs((
int32)pthisMag->
iBs[i] - (
int32)pthisMagBuffer->
iBs[i][j][k]);
175 for (i =
CHX; i <=
CHZ; i++)
177 pthisMagBuffer->
iBs[i][j][k] = pthisMag->
iBs[i];
179 pthisMagBuffer->
index[j][k] = loopcounter;
196 if (pthisMagBuffer->
index[j][k] != -1)
200 for (i =
CHX; i <=
CHZ; i++)
202 idelta += abs((
int32)pthisMag->
iBs[i] - (
int32)pthisMagBuffer->
iBs[i][j][k]);
226 for (i =
CHX; i <=
CHZ; i++)
228 pthisMagBuffer->
iBs[i][l][m] = pthisMag->
iBs[i];
230 pthisMagBuffer->
index[l][m] = loopcounter;
253 for (i =
CHX; i <=
CHZ; i++)
255 ftmp[i] = pthisMag->
fBsAvg[i] - pthisMagCal->
fV[i];
258 for (i =
CHX; i <=
CHZ; i++)
267 for (i =
CHX; i <=
CHZ; i++)
332 pthisMagCal->
fB = pthisMagCal->
ftrB;
333 for (i =
CHX; i <=
CHZ; i++)
335 pthisMagCal->
fV[i] = pthisMagCal->
ftrV[i];
336 for (j =
CHX; j <=
CHZ; j++)
377 for (i = 0; i < 4; i++)
379 pthisMagCal->
fvecB[i] = 0.0F;
380 for (j = i; j < 4; j++)
382 pthisMagCal->
fmatA[i][j] = 0.0F;
387 iOffset[
CHX] = iOffset[
CHY] = iOffset[
CHZ] = 0;
395 if (pthisMagBuffer->
index[j][k] != -1)
400 for (l =
CHX; l <=
CHZ; l++)
402 iOffset[l] = pthisMagBuffer->
iBs[l][j][k];
407 for (l =
CHX; l <=
CHZ; l++)
409 pthisMagCal->
fvecA[l] = (float)((
int32)pthisMagBuffer->
iBs[l][j][k] - (
int32)iOffset[l]) * fscaling;
410 pthisMagCal->
fvecA[l + 3] = pthisMagCal->
fvecA[l] * pthisMagCal->
fvecA[l];
414 fBs2 = pthisMagCal->
fvecA[3] + pthisMagCal->
fvecA[4] + pthisMagCal->
fvecA[5];
417 fSumBs4 += fBs2 * fBs2;
420 for (l =
CHX; l <=
CHZ; l++)
422 pthisMagCal->
fvecB[l] += pthisMagCal->
fvecA[l] * fBs2;
426 pthisMagCal->
fvecB[3] += fBs2;
446 pthisMagCal->
fmatA[3][3] = (float) iCount;
452 for (i = 0; i < 4; i++)
454 for (j = i; j < 4; j++)
456 pthisMagCal->
fmatB[i][j] = pthisMagCal->
fmatB[j][i] = pthisMagCal->
fmatA[j][i] = pthisMagCal->
fmatA[i][j];
461 for (i = 0; i < 4; i++)
463 pfRows[i] = pthisMagCal->
fmatB[i];
468 for (i = 0; i < 4; i++)
470 pthisMagCal->
fvecA[i] = 0.0F;
471 for (k = 0; k < 4; k++)
473 pthisMagCal->
fvecA[i] += pthisMagCal->
fmatB[i][k] * pthisMagCal->
fvecB[k];
481 for (i = 0; i < 4; i++)
483 fE += pthisMagCal->
fvecA[i] * pthisMagCal->
fvecB[i];
485 fE = fSumBs4 - 2.0F * fE;
488 for (i = 0; i < 4; i++)
490 pthisMagCal->
fvecB[i] = 0.0F;
491 for (k = 0; k < 4; k++)
493 pthisMagCal->
fvecB[i] += pthisMagCal->
fmatA[i][k] * pthisMagCal->
fvecA[k];
498 for (i = 0; i < 4; i++)
500 fE += pthisMagCal->
fvecB[i] * pthisMagCal->
fvecA[i];
504 for (l =
CHX; l <=
CHZ; l++)
506 pthisMagCal->
ftrV[l] = 0.5F * pthisMagCal->
fvecA[l];
515 (2.0F * pthisMagCal->
ftrB * pthisMagCal->
ftrB);
518 for (l =
CHX; l <=
CHZ; l++)
538 int8 i, j, k, l, m, n;
544 iOffset[
CHX] = iOffset[
CHY] = iOffset[
CHZ] = 0;
547 for (m = 0; m < 7; m++)
549 for (n = m; n < 7; n++)
551 pthisMagCal->
fmatA[m][n] = 0.0F;
561 if (pthisMagBuffer->
index[j][k] != -1)
566 for (l =
CHX; l <=
CHZ; l++)
568 iOffset[l] = pthisMagBuffer->
iBs[l][j][k];
573 for (l =
CHX; l <=
CHZ; l++)
575 pthisMagCal->
fvecA[l + 3] = (float)((
int32)pthisMagBuffer->
iBs[l][j][k] - (
int32)iOffset[l]) * fscaling;
576 pthisMagCal->
fvecA[l] = pthisMagCal->
fvecA[l + 3] * pthisMagCal->
fvecA[l + 3];
583 for (m = 0; m < 6; m++)
585 pthisMagCal->
fmatA[m][6] += pthisMagCal->
fvecA[m];
588 for (m = 0; m < 6; m++)
590 for (n = m; n < 6; n++)
592 pthisMagCal->
fmatA[m][n] += pthisMagCal->
fvecA[m] * pthisMagCal->
fvecA[n];
603 pthisMagCal->
fmatA[6][6] = (float) iCount;
609 for (m = 1; m < 7; m++)
611 for (n = 0; n < m; n++)
613 pthisMagCal->
fmatA[m][n] = pthisMagCal->
fmatA[n][m];
622 for (i = 1; i < 7; i++)
624 if (pthisMagCal->
fvecA[i] < pthisMagCal->
fvecA[j])
634 for (l =
CHX; l <=
CHZ; l++)
636 pthisMagCal->
fA[l][l] = pthisMagCal->
fmatB[l][j];
637 det *= pthisMagCal->
fA[l][l];
638 pthisMagCal->
ftrV[l] = -0.5F * pthisMagCal->
fmatB[l + 3][j] / pthisMagCal->
fA[l][l];
645 pthisMagCal->
fmatB[6][j] = -pthisMagCal->
fmatB[6][j];
650 ftmp = -pthisMagCal->
fmatB[6][j];
651 for (l =
CHX; l <=
CHZ; l++)
653 ftmp += pthisMagCal->
fA[l][l] * pthisMagCal->
ftrV[l] * pthisMagCal->
ftrV[l];
667 for (l =
CHX; l <=
CHZ; l++)
669 pthisMagCal->
ftrinvW[l][l] = sqrtf(fabsf(pthisMagCal->
fA[l][l]));
685 int8 i, j, k, l, m, n;
691 iOffset[
CHX] = iOffset[
CHY] = iOffset[
CHZ] = 0;
694 for (m = 0; m < 10; m++)
696 for (n = m; n < 10; n++)
698 pthisMagCal->
fmatA[m][n] = 0.0F;
708 if (pthisMagBuffer->
index[j][k] != -1)
713 for (l =
CHX; l <=
CHZ; l++)
715 iOffset[l] = pthisMagBuffer->
iBs[l][j][k];
720 for (l =
CHX; l <=
CHZ; l++)
722 pthisMagCal->
fvecA[l + 6] = (float)((
int32)pthisMagBuffer->
iBs[l][j][k] - (
int32)iOffset[l]) * fscaling;
727 pthisMagCal->
fvecA[1] = 2.0F * pthisMagCal->
fvecA[6] * pthisMagCal->
fvecA[7];
728 pthisMagCal->
fvecA[2] = 2.0F * pthisMagCal->
fvecA[6] * pthisMagCal->
fvecA[8];
730 pthisMagCal->
fvecA[4] = 2.0F * pthisMagCal->
fvecA[7] * pthisMagCal->
fvecA[8];
736 for (m = 0; m < 9; m++)
738 pthisMagCal->
fmatA[m][9] += pthisMagCal->
fvecA[m];
741 for (m = 0; m < 9; m++)
743 for (n = m; n < 9; n++)
745 pthisMagCal->
fmatA[m][n] += pthisMagCal->
fvecA[m] * pthisMagCal->
fvecA[n];
756 pthisMagCal->
fmatA[9][9] = (float) iCount;
762 for (m = 1; m < 10; m++)
764 for (n = 0; n < m; n++)
766 pthisMagCal->
fmatA[m][n] = pthisMagCal->
fmatA[n][m];
775 for (i = 1; i < 10; i++)
777 if (pthisMagCal->
fvecA[i] < pthisMagCal->
fvecA[j])
782 pthisMagCal->
fA[0][0] = pthisMagCal->
fmatB[0][j];
783 pthisMagCal->
fA[0][1] = pthisMagCal->
fA[1][0] = pthisMagCal->
fmatB[1][j];
784 pthisMagCal->
fA[0][2] = pthisMagCal->
fA[2][0] = pthisMagCal->
fmatB[2][j];
785 pthisMagCal->
fA[1][1] = pthisMagCal->
fmatB[3][j];
786 pthisMagCal->
fA[1][2] = pthisMagCal->
fA[2][1] = pthisMagCal->
fmatB[4][j];
787 pthisMagCal->
fA[2][2] = pthisMagCal->
fmatB[5][j];
794 pthisMagCal->
fmatB[6][j] = -pthisMagCal->
fmatB[6][j];
795 pthisMagCal->
fmatB[7][j] = -pthisMagCal->
fmatB[7][j];
796 pthisMagCal->
fmatB[8][j] = -pthisMagCal->
fmatB[8][j];
797 pthisMagCal->
fmatB[9][j] = -pthisMagCal->
fmatB[9][j];
805 for (l =
CHX; l <=
CHZ; l++)
807 pthisMagCal->
ftrV[l] = 0.0F;
808 for (m =
CHX; m <=
CHZ; m++)
810 pthisMagCal->
ftrV[l] += pthisMagCal->
finvA[l][m] * pthisMagCal->
fmatB[m + 6][j];
812 pthisMagCal->
ftrV[l] *= -0.5F;
816 pthisMagCal->
ftrB = sqrtf(fabsf(pthisMagCal->
fA[0][0] * pthisMagCal->
ftrV[
CHX] * pthisMagCal->
ftrV[
CHX] +
817 2.0F * pthisMagCal->
fA[0][1] * pthisMagCal->
ftrV[
CHX] * pthisMagCal->
ftrV[
CHY] +
818 2.0F * pthisMagCal->
fA[0][2] * pthisMagCal->
ftrV[
CHX] * pthisMagCal->
ftrV[
CHZ] +
820 2.0F * pthisMagCal->
fA[1][2] * pthisMagCal->
ftrV[
CHY] * pthisMagCal->
ftrV[
CHZ] +
821 pthisMagCal->
fA[2][2] * pthisMagCal->
ftrV[
CHZ] * pthisMagCal->
ftrV[
CHZ] - pthisMagCal->
fmatB[9][j]));
825 (pthisMagCal->
ftrB * pthisMagCal->
ftrB);
828 for (l =
CHX; l <=
CHZ; l++)
843 for (i = 0; i < 3; i++)
845 for (j = 0; j < 3; j++)
847 pthisMagCal->
fmatA[i][j] = pthisMagCal->
fA[i][j];
853 for (j = 0; j < 3; j++)
855 ftmp = sqrtf(sqrtf(fabsf(pthisMagCal->
fvecA[j])));
856 for (i = 0; i < 3; i++)
858 pthisMagCal->
fmatB[i][j] *= ftmp;
865 for (i = 0; i < 3; i++)
868 for (j = i; j < 3; j++)
870 pthisMagCal->
ftrinvW[i][j] = 0.0F;
872 for (k = 0; k < 3; k++)
874 pthisMagCal->
ftrinvW[i][j] += pthisMagCal->
fmatB[i][k] * pthisMagCal->
fmatB[j][k];
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
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
void iUpdateMagnetometerBuffer(struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag, int32 loopcounter)
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
void fUpdateCalibration10EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
void f3x3matrixAeqI(float A[][3])
void fInvertMagCal(struct MagSensor *pthisMag, struct MagCalibration *pthisMagCal)
int16 iBs[3][MAGBUFFSIZEX][MAGBUFFSIZEY]
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)
#define MINMEASUREMENTS10CAL
#define MINMEASUREMENTS7CAL