// function directly calculates the symmetric inverse of a symmetric 3x3 matrix
// only the on and above diagonal terms in B are used and need to be specified
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
{
float fB11B22mB12B12; // B[1][1] * B[2][2] - B[1][2] * B[1][2]
float fB12B02mB01B22; // B[1][2] * B[0][2] - B[0][1] * B[2][2]
float fB01B12mB11B02; // B[0][1] * B[1][2] - B[1][1] * B[0][2]
float ftmp; // determinant and then reciprocal
// calculate useful products
fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
// set ftmp to the determinant of the input matrix B
ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
// set A to the inverse of B for any determinant except zero
if (ftmp != 0.0F)
{
ftmp = 1.0F / ftmp;
A[0][0] = fB11B22mB12B12 * ftmp;
A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
}
else
{
// provide the identity matrix if the determinant is zero
f3x3matrixAeqI(A);
}
return;
}
// function resets the magnetometer buffer and magnetic calibration
void fInitMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer)
{
int8 j, k; // loop counters
// initialize the calibration hard and soft iron estimate to null
f3x3matrixAeqI(pthisMagCal->finvW);
pthisMagCal->fV[CHX] = pthisMagCal->fV[CHY] = pthisMagCal->fV[CHZ] = 0.0F;
pthisMagCal->fB = DEFAULTB;
pthisMagCal->fFitErrorpc = 1000.0F;
pthisMagCal->iValidMagCal = 0;
pthisMagCal->iCalInProgress = 0;
pthisMagCal->iMagCalHasRun = 0;
// set magnetic buffer index to invalid value -1 to denote invalid
pthisMagBuffer->iMagBufferCount = 0;
for (j = 0; j < MAGBUFFSIZEX; j++)
{
for (k = 0; k < MAGBUFFSIZEY; k++)
{
pthisMagBuffer->index[j][k] = -1;
}
}
// initialize the array of (MAGBUFFSIZEX - 1) elements of 100 * tangents used for buffer indexing
// entries cover the range 100 * tan(-PI/2 + PI/MAGBUFFSIZEX), 100 * tan(-PI/2 + 2*PI/MAGBUFFSIZEX) to
// 100 * tan(-PI/2 + (MAGBUFFSIZEX - 1) * PI/MAGBUFFSIZEX).
// for MAGBUFFSIZEX=12, the entries range in value from -373 to +373
for (j = 0; j < (MAGBUFFSIZEX - 1); j++)
{
pthisMagBuffer->tanarray[j] = (int16) (100.0F * tanf(PI * (-0.5F + (float) (j + 1) / MAGBUFFSIZEX)));
}
return;
}
// Android magnetometer 3DOF flat eCompass function computing rotation matrix fR
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
{
// local variables
float fmodBxy; // modulus of the x, y magnetometer readings
// compute the magnitude of the horizontal (x and y) magnetometer reading
fmodBxy = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y]);
// check for zero field special case where no solution is possible
if (fmodBxy == 0.0F)
{
f3x3matrixAeqI(fR);
return;
}
// define the fixed entries in the z row and column
fR[Z][X] = fR[Z][Y] = fR[X][Z] = fR[Y][Z] = 0.0F;
fR[Z][Z] = 1.0F;
// define the remaining entries
fR[X][X] = fR[Y][Y] = fBc[Y] / fmodBxy;
fR[X][Y] = fBc[X] / fmodBxy;
fR[Y][X] = -fR[X][Y];
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;
}
// 4 element calibration using 4x4 matrix inverse
void fUpdateCalibration4INV(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor *pthisMag)
{
// local variables
float fBs2; // fBs[CHX]^2+fBs[CHY]^2+fBs[CHZ]^2
float fSumBs4; // sum of fBs2
float fscaling; // set to FUTPERCOUNT * FMATRIXSCALING
float fE; // error function = r^T.r
int16 iOffset[3]; // offset to remove large DC hard iron bias in matrix
int16 iCount; // number of measurements counted
int8 ierror; // matrix inversion error flag
int8 i, j, k, l; // loop counters
// working arrays for 4x4 matrix inversion
float *pfRows[4];
int8 iColInd[4];
int8 iRowInd[4];
int8 iPivot[4];
// compute fscaling to reduce multiplications later
fscaling = pthisMag->fuTPerCount / DEFAULTB;
// the trial inverse soft iron matrix invW always equals the identity matrix for 4 element calibration
f3x3matrixAeqI(pthisMagCal->ftrinvW);
// zero fSumBs4=Y^T.Y, fvecB=X^T.Y (4x1) and on and above diagonal elements of fmatA=X^T*X (4x4)
fSumBs4 = 0.0F;
for (i = 0; i < 4; i++)
{
pthisMagCal->fvecB[i] = 0.0F;
for (j = i; j < 4; j++)
{
pthisMagCal->fmatA[i][j] = 0.0F;
}
}
// the offsets are guaranteed to be set from the first element but to avoid compiler error
iOffset[CHX] = iOffset[CHY] = iOffset[CHZ] = 0;
// use entries from magnetic buffer to compute matrices
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 estimate (in counts) for offset
if (iCount == 0)
{
for (l = CHX; l <= CHZ; l++)
{
iOffset[l] = pthisMagBuffer->iBs[l][j][k];
}
}
// store scaled and offset fBs[XYZ] in fvecA[0-2] and fBs[XYZ]^2 in fvecA[3-5]
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->fvecA[l] = (float)((int32)pthisMagBuffer->iBs[l][j][k] - (int32)iOffset[l]) * fscaling;
pthisMagCal->fvecA[l + 3] = pthisMagCal->fvecA[l] * pthisMagCal->fvecA[l];
}
// calculate fBs2 = fBs[CHX]^2 + fBs[CHY]^2 + fBs[CHZ]^2 (scaled uT^2)
fBs2 = pthisMagCal->fvecA[3] + pthisMagCal->fvecA[4] + pthisMagCal->fvecA[5];
// accumulate fBs^4 over all measurements into fSumBs4=Y^T.Y
fSumBs4 += fBs2 * fBs2;
// now we have fBs2, accumulate fvecB[0-2] = X^T.Y =sum(fBs2.fBs[XYZ])
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->fvecB[l] += pthisMagCal->fvecA[l] * fBs2;
}
//accumulate fvecB[3] = X^T.Y =sum(fBs2)
pthisMagCal->fvecB[3] += fBs2;
// accumulate on and above-diagonal terms of fmatA = X^T.X ignoring fmatA[3][3]
pthisMagCal->fmatA[0][0] += pthisMagCal->fvecA[CHX + 3];
pthisMagCal->fmatA[0][1] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHY];
pthisMagCal->fmatA[0][2] += pthisMagCal->fvecA[CHX] * pthisMagCal->fvecA[CHZ];
pthisMagCal->fmatA[0][3] += pthisMagCal->fvecA[CHX];
pthisMagCal->fmatA[1][1] += pthisMagCal->fvecA[CHY + 3];
pthisMagCal->fmatA[1][2] += pthisMagCal->fvecA[CHY] * pthisMagCal->fvecA[CHZ];
pthisMagCal->fmatA[1][3] += pthisMagCal->fvecA[CHY];
pthisMagCal->fmatA[2][2] += pthisMagCal->fvecA[CHZ + 3];
pthisMagCal->fmatA[2][3] += pthisMagCal->fvecA[CHZ];
// increment the counter for next iteration
iCount++;
}
}
}
// set the last element of the measurement matrix to the number of buffer elements used
pthisMagCal->fmatA[3][3] = (float) iCount;
// store the number of measurements accumulated (defensive programming, should never be needed)
pthisMagBuffer->iMagBufferCount = iCount;
// use above diagonal elements of symmetric fmatA to set both fmatB and fmatA to X^T.X
for (i = 0; i < 4; i++)
{
for (j = i; j < 4; j++)
{
pthisMagCal->fmatB[i][j] = pthisMagCal->fmatB[j][i] = pthisMagCal->fmatA[j][i] = pthisMagCal->fmatA[i][j];
}
}
// calculate in situ inverse of fmatB = inv(X^T.X) (4x4) while fmatA still holds X^T.X
for (i = 0; i < 4; i++)
{
pfRows[i] = pthisMagCal->fmatB[i];
}
fmatrixAeqInvA(pfRows, iColInd, iRowInd, iPivot, 4, &ierror);
// calculate fvecA = solution beta (4x1) = inv(X^T.X).X^T.Y = fmatB * fvecB
for (i = 0; i < 4; i++)
{
pthisMagCal->fvecA[i] = 0.0F;
for (k = 0; k < 4; k++)
{
pthisMagCal->fvecA[i] += pthisMagCal->fmatB[i][k] * pthisMagCal->fvecB[k];
}
}
// calculate P = r^T.r = Y^T.Y - 2 * beta^T.(X^T.Y) + beta^T.(X^T.X).beta
// = fSumBs4 - 2 * fvecA^T.fvecB + fvecA^T.fmatA.fvecA
// first set P = Y^T.Y - 2 * beta^T.(X^T.Y) = fSumBs4 - 2 * fvecA^T.fvecB
fE = 0.0F;
for (i = 0; i < 4; i++)
{
fE += pthisMagCal->fvecA[i] * pthisMagCal->fvecB[i];
}
fE = fSumBs4 - 2.0F * fE;
// set fvecB = (X^T.X).beta = fmatA.fvecA
for (i = 0; i < 4; i++)
{
pthisMagCal->fvecB[i] = 0.0F;
for (k = 0; k < 4; k++)
{
pthisMagCal->fvecB[i] += pthisMagCal->fmatA[i][k] * pthisMagCal->fvecA[k];
}
}
// complete calculation of P by adding beta^T.(X^T.X).beta = fvecA^T * fvecB
for (i = 0; i < 4; i++)
{
fE += pthisMagCal->fvecB[i] * pthisMagCal->fvecA[i];
}
// compute the hard iron vector (in uT but offset and scaled by FMATRIXSCALING)
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->ftrV[l] = 0.5F * pthisMagCal->fvecA[l];
}
// compute the scaled geomagnetic field strength B (in uT but scaled by FMATRIXSCALING)
pthisMagCal->ftrB = sqrtf(pthisMagCal->fvecA[3] + pthisMagCal->ftrV[CHX] * pthisMagCal->ftrV[CHX] +
pthisMagCal->ftrV[CHY] * pthisMagCal->ftrV[CHY] + pthisMagCal->ftrV[CHZ] * pthisMagCal->ftrV[CHZ]);
// calculate the trial fit error (percent) normalized to number of measurements and scaled geomagnetic field strength
pthisMagCal->ftrFitErrorpc = sqrtf(fE / (float) pthisMagBuffer->iMagBufferCount) * 100.0F /
(2.0F * pthisMagCal->ftrB * pthisMagCal->ftrB);
// correct the hard iron estimate for FMATRIXSCALING and the offsets applied (result in uT)
for (l = CHX; l <= CHZ; l++)
{
pthisMagCal->ftrV[l] = pthisMagCal->ftrV[l] * DEFAULTB + (float)iOffset[l] * pthisMag->fuTPerCount;
}
// correct the geomagnetic field strength B to correct scaling (result in uT)
pthisMagCal->ftrB *= DEFAULTB;
return;
}
// 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;
}
// Win8: 6DOF e-Compass function computing rotation matrix fR
void feCompassWin8(float fR[][3], float *pfDelta, float fBc[], float fGp[])
{
// local variables
float fmod[3]; // column moduli
float fmodBc; // modulus of Bc
float fGdotBc; // dot product of vectors G.Bc
float ftmp; // scratch variable
int8 i, j; // loop counters
// set the inclination angle to zero in case it is not computed later
*pfDelta = 0.0F;
// place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
for (i = X; i <= Z; i++)
{
fR[i][Z] = -fGp[i];
fR[i][Y] = fBc[i];
}
// set x vector to vector product of y and z vectors
fR[X][X] = fR[Y][Y] * fR[Z][Z] - fR[Z][Y] * fR[Y][Z];
fR[Y][X] = fR[Z][Y] * fR[X][Z] - fR[X][Y] * fR[Z][Z];
fR[Z][X] = fR[X][Y] * fR[Y][Z] - fR[Y][Y] * fR[X][Z];
// set y vector to vector product of z and x vectors
fR[X][Y] = fR[Y][Z] * fR[Z][X] - fR[Z][Z] * fR[Y][X];
fR[Y][Y] = fR[Z][Z] * fR[X][X] - fR[X][Z] * fR[Z][X];
fR[Z][Y] = fR[X][Z] * fR[Y][X] - fR[Y][Z] * fR[X][X];
// calculate the rotation matrix column moduli
fmod[X] = sqrtf(fR[X][X] * fR[X][X] + fR[Y][X] * fR[Y][X] + fR[Z][X] * fR[Z][X]);
fmod[Y] = sqrtf(fR[X][Y] * fR[X][Y] + fR[Y][Y] * fR[Y][Y] + fR[Z][Y] * fR[Z][Y]);
fmod[Z] = sqrtf(fR[X][Z] * fR[X][Z] + fR[Y][Z] * fR[Y][Z] + fR[Z][Z] * fR[Z][Z]);
// normalize the rotation matrix columns
if (!((fmod[X] == 0.0F) || (fmod[Y] == 0.0F) || (fmod[Z] == 0.0F)))
{
// loop over columns j
for (j = X; j <= Z; j++)
{
ftmp = 1.0F / fmod[j];
// loop over rows i
for (i = X; i <= Z; i++)
{
// normalize by the column modulus
fR[i][j] *= ftmp;
}
}
}
else
{
// no solution is possible to set rotation to identity matrix
f3x3matrixAeqI(fR);
return;
}
// compute the geomagnetic inclination angle
fmodBc = sqrtf(fBc[X] * fBc[X] + fBc[Y] * fBc[Y] + fBc[Z] * fBc[Z]);
fGdotBc = fGp[X] * fBc[X] + fGp[Y] * fBc[Y] + fGp[Z] * fBc[Z];
if (!((fmod[Z] == 0.0F) || (fmodBc == 0.0F)))
{
*pfDelta = fasin_deg(fGdotBc / (fmod[Z] * fmodBc));
}
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;
}
