Ejemplo n.º 1
0
// 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;
}
Ejemplo n.º 2
0
// 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;
}
Ejemplo n.º 3
0
// 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;
}
Ejemplo n.º 4
0
// 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;
}
Ejemplo n.º 5
0
// 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;
}
Ejemplo n.º 6
0
// 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;
}
Ejemplo n.º 7
0
// 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;
}
Ejemplo n.º 8
0
// 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;
}