/*
Automatic 3 axis calibration.
This method of calibration is an extension of the 2D method described at
http://www.fatquarterssoftware.com/downloads/AUTOCAL.pdf
While that paper says that extension to 3D is obvious, it doesn't mention
that is isn't simple. The math is really pretty simple but keeping track
of the details is tedious.
Using this method you do not have to carefully find the minimum and maximum
responses from the sensor. Just pick 6 reasonably distinct measurement sets
and go.
Oct. 2009 David W. Schultz
*/
#include
#include
/* Matrix code snarfed from: http://www.hlevkin.com/NumAlg/LinearEquations.c
*/
//==============================================================================
// return 1 if system not solving
// nDim - system dimension
// pfMatr - matrix with coefficients
// pfVect - vector with free members
// pfSolution - vector with system solution
// pfMatr becames trianglular after function call
// pfVect changes after function call
//
// Developer: Henry Guennadi Levkin
//
//==============================================================================
int LinearEquationsSolving(int nDim, float* pfMatr, float* pfVect, float* pfSolution)
{
float fMaxElem;
float fAcc;
int i , j, k, m;
for(k=0; k<(nDim-1); k++) // base row of matrix
{
// search of line with max element
fMaxElem = fabs( pfMatr[k*nDim + k] );
m = k;
for(i=k+1; i=0; k--)
{
pfSolution[k] = pfVect[k];
for(i=(k+1); i