// Xtrinsic Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
void f3DOFTiltNED(float fR[][3], float fGp[])
{
// the NED self-consistency twist occurs at 90 deg pitch
// local variables
int16 i; // counter
float fmodGxyz; // modulus of the x, y, z accelerometer readings
float fmodGyz; // modulus of the y, z accelerometer readings
float frecipmodGxyz; // reciprocal of modulus
float ftmp; // scratch variable
// compute the accelerometer squared magnitudes
fmodGyz = fGp[Y] * fGp[Y] + fGp[Z] * fGp[Z];
fmodGxyz = fmodGyz + fGp[X] * fGp[X];
// check for freefall special case where no solution is possible
if (fmodGxyz == 0.0F)
{
f3x3matrixAeqI(fR);
return;
}
// check for vertical up or down gimbal lock case
if (fmodGyz == 0.0F)
{
f3x3matrixAeqScalar(fR, 0.0F);
fR[Y][Y] = 1.0F;
if (fGp[X] >= 0.0F)
{
fR[X][Z] = 1.0F;
fR[Z][X] = -1.0F;
}
else
{
fR[X][Z] = -1.0F;
fR[Z][X] = 1.0F;
}
return;
}
// compute moduli for the general case
fmodGyz = sqrtf(fmodGyz);
fmodGxyz = sqrtf(fmodGxyz);
frecipmodGxyz = 1.0F / fmodGxyz;
ftmp = fmodGxyz / fmodGyz;
// normalize the accelerometer reading into the z column
for (i = X; i <= Z; i++)
{
fR[i][Z] = fGp[i] * frecipmodGxyz;
}
// construct x column of orientation matrix
fR[X][X] = fmodGyz * frecipmodGxyz;
fR[Y][X] = -fR[X][Z] * fR[Y][Z] * ftmp;
fR[Z][X] = -fR[X][Z] * fR[Z][Z] * ftmp;
// // construct y column of orientation matrix
fR[X][Y] = 0.0F;
fR[Y][Y] = fR[Z][Z] * ftmp;
fR[Z][Y] = -fR[Y][Z] * ftmp;
return;
}
// 7 element calibration using direct eigen-decomposition
void fUpdateCalibration7EIG(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
{
// local variables
float det; // matrix determinant
float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
float ftmp; // scratch variable
int16 iOffset[3]; // offset to remove large DC hard iron bias
int16 iCount; // number of measurements counted
int8 i, j, k, l, m, n; // loop counters
// compute fscaling to reduce multiplications later
fscaling = pthisMag->fuTPerCount / DEFAULTB;
// the offsets are guaranteed to be set from the first element but to avoid compiler error
iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
// zero the on and above diagonal elements of the 7x7 symmetric measurement matrix fmatA
for (m = 0; m < 7; m++)
{
for (n = m; n < 7; n++)
{
pthisMagCal->fmatA[m][n] = 0.0F;
}
}
// add megnetic buffer entries into product matrix fmatA
iCount = 0;
for (j = 0; j < MAGBUFFSIZEX; j++)
{
for (k = 0; k < MAGBUFFSIZEY; k++)
{
if (pthisMagBuffer->index[j][k] != -1)
{
// use first valid magnetic buffer entry as offset estimate (bit counts)
if (iCount == 0)
{
for (l = CHX; l <= CHZ; l++)
{
iOffset[l] = pthisMagBuffer->iBs[l][j][k];
}
}
// apply the offset and scaling and store in fvecA
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->fvecA[l + 3] = (float)((int32)pthisMagBuffer->iBs[l][j][k] - (int32)iOffset[l]) * fscaling;
pthisMagCal->fvecA[l] = pthisMagCal->fvecA[l + 3] * pthisMagCal->fvecA[l + 3];
}
// accumulate the on-and above-diagonal terms of pthisMagCal->fmatA=Sigma{fvecA^T * fvecA}
// with the exception of fmatA[6][6] which will sum to the number of measurements
// and remembering that fvecA[6] equals 1.0F
// update the right hand column [6] of fmatA except for fmatA[6][6]
for (m = 0; m < 6; m++)
{
pthisMagCal->fmatA[m][6] += pthisMagCal->fvecA[m];
}
// update the on and above diagonal terms except for right hand column 6
for (m = 0; m < 6; m++)
{
for (n = m; n < 6; n++)
{
pthisMagCal->fmatA[m][n] += pthisMagCal->fvecA[m] * pthisMagCal->fvecA[n];
}
}
// increment the measurement counter for the next iteration
iCount++;
}
}
}
// finally set the last element fmatA[6][6] to the number of measurements
pthisMagCal->fmatA[6][6] = (float) iCount;
// store the number of measurements accumulated (defensive programming, should never be needed)
pthisMagBuffer->iMagBufferCount = iCount;
// copy the above diagonal elements of fmatA to below the diagonal
for (m = 1; m < 7; m++)
{
for (n = 0; n < m; n++)
{
pthisMagCal->fmatA[m][n] = pthisMagCal->fmatA[n][m];
}
}
// set tmpA7x1 to the unsorted eigenvalues and fmatB to the unsorted eigenvectors of fmatA
eigencompute10(pthisMagCal->fmatA, pthisMagCal->fvecA, pthisMagCal->fmatB, 7);
// find the smallest eigenvalue
j = 0;
for (i = 1; i < 7; i++)
{
if (pthisMagCal->fvecA[i] < pthisMagCal->fvecA[j])
{
j = i;
}
}
// set ellipsoid matrix A to the solution vector with smallest eigenvalue, compute its determinant
// and the hard iron offset (scaled and offset)
f3x3matrixAeqScalar(pthisMagCal->fA, 0.0F);
det = 1.0F;
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->fA[l][l] = pthisMagCal->fmatB[l][j];
det *= pthisMagCal->fA[l][l];
pthisMagCal->ftrV[l] = -0.5F * pthisMagCal->fmatB[l + 3][j] / pthisMagCal->fA[l][l];
}
// negate A if it has negative determinant
if (det < 0.0F)
{
f3x3matrixAeqMinusA(pthisMagCal->fA);
pthisMagCal->fmatB[6][j] = -pthisMagCal->fmatB[6][j];
det = -det;
}
// set ftmp to the square of the trial geomagnetic field strength B (counts times FMATRIXSCALING)
ftmp = -pthisMagCal->fmatB[6][j];
for (l = CHX; l <= CHZ; l++)
{
ftmp += pthisMagCal->fA[l][l] * pthisMagCal->ftrV[l] * pthisMagCal->ftrV[l];
}
// calculate the trial normalized fit error as a percentage
pthisMagCal->ftrFitErrorpc = 50.0F * sqrtf(fabsf(pthisMagCal->fvecA[j]) / (float) pthisMagBuffer->iMagBufferCount) / fabsf(ftmp);
// normalize the ellipsoid matrix A to unit determinant
f3x3matrixAeqAxScalar(pthisMagCal->fA, powf(det, -(ONETHIRD)));
// convert the geomagnetic field strength B into uT for normalized soft iron matrix A and normalize
pthisMagCal->ftrB = sqrtf(fabsf(ftmp)) * DEFAULTB * powf(det, -(ONESIXTH));
// compute trial invW from the square root of A also with normalized determinant and hard iron offset in uT
f3x3matrixAeqI(pthisMagCal->ftrinvW);
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->ftrinvW[l][l] = sqrtf(fabsf(pthisMagCal->fA[l][l]));
pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l] * pthisMag->fuTPerCount;
}
return;
}
// Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR
void f3DOFTiltWin8(float fR[][3], float fGs[])
{
// the Win8 self-consistency twist occurs at 90 deg roll
// local variables
float fmodGxyz; // modulus of the x, y, z accelerometer readings
float fmodGxz; // modulus of the x, z accelerometer readings
float frecipmodGxyz; // reciprocal of modulus
float ftmp; // scratch variable
int8 i; // counter
// compute the accelerometer squared magnitudes
fmodGxz = fGs[CHX] * fGs[CHX] + fGs[CHZ] * fGs[CHZ];
fmodGxyz = fmodGxz + fGs[CHY] * fGs[CHY];
// check for freefall special case where no solution is possible
if (fmodGxyz == 0.0F)
{
f3x3matrixAeqI(fR);
return;
}
// check for vertical up or down gimbal lock case
if (fmodGxz == 0.0F)
{
f3x3matrixAeqScalar(fR, 0.0F);
fR[CHX][CHX] = 1.0F;
if (fGs[CHY] >= 0.0F)
{
fR[CHY][CHZ] = -1.0F;
fR[CHZ][CHY] = 1.0F;
}
else
{
fR[CHY][CHZ] = 1.0F;
fR[CHZ][CHY] = -1.0F;
}
return;
}
// compute moduli for the general case
fmodGxz = sqrtf(fmodGxz);
fmodGxyz = sqrtf(fmodGxyz);
frecipmodGxyz = 1.0F / fmodGxyz;
ftmp = fmodGxyz / fmodGxz;
if (fGs[CHZ] < 0.0F)
{
ftmp = -ftmp;
}
// normalize the negated accelerometer reading into the z column
for (i = CHX; i <= CHZ; i++)
{
fR[i][CHZ] = -fGs[i] * frecipmodGxyz;
}
// construct x column of orientation matrix
fR[CHX][CHX] = -fR[CHZ][CHZ] * ftmp;
fR[CHY][CHX] = 0.0F;
fR[CHZ][CHX] = fR[CHX][CHZ] * ftmp;;
// // construct y column of orientation matrix
fR[CHX][CHY] = fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
fR[CHY][CHY] = -fmodGxz * frecipmodGxyz;
if (fGs[CHZ] < 0.0F)
{
fR[CHY][CHY] = -fR[CHY][CHY];
}
fR[CHZ][CHY] = fR[CHY][CHZ] * fR[CHZ][CHZ] * ftmp;
return;
}
