void fCalibrationUpdate()
{
// try to find an improved calibration if update is enabled ///CALIBRATION
if (CALUPDACTIVE)
{
// check for enough data in magnetic buffer for a calibration
if (MagBufferCount >= MINEQUATIONS)
{
// calibrate if this will be the first calibration or every CALINTERVAL iterations
if ((!validmagcal) || (validmagcal && !(loopcounter % CALINTERVAL)))
{
// 7 point eigenpair calibration
if (SOLUTIONSIZE == 7)
fUpdateCalibration7EIG();
// 4 point INV calibration
else if (SOLUTIONSIZE == 4)
fUpdateCalibration4INV();
else
fUpdateCalibration7EIG();
// decide if this new calibration should be accepted
if ((ftrFitErrorpc <= fFitErrorpc) && (ftrB >= MINBFIT) && (ftrB <= MAXBFIT))
{
printf("\n\nAccepting new calibration solution\n\n");
fFitErrorpc = ftrFitErrorpc;
fB = ftrB;
fVx = ftrVx;
fVy = ftrVy;
fVz = ftrVz;
fmatrixAeqB(finvW, ftrinvW, 3, 3);
}
else
{
printf("\n\nRejecting new calibration solution");
}
// age (increase) the calibration fit to avoid a good calibration preventing future updates
// FITERRORAGING is the reciprocal of the time (s) for the fit error to increase by e=2.718
// CALINTERVAL * DELTAT is the interval in seconds between each aging update of fFitErrorpc
// (1 + FITERRORAGING * CALINTERVAL * DELTAT)^n=e defines n, the number of updates for e fold increase
// approx n * CALINTERVAL * DELTAT = 1. / FITERRORAGING
// so as required FITERRORAGING is the reciprocal of the time in secs for e fold increase
fFitErrorpc += fFitErrorpc * FITERRORAGING * (float) CALINTERVAL * DELTAT;
} // end of test whether to call calibration functions
}
else // still too few entries in magnetic buffer for calibration
//printf("\r %d entries in magnetometer buffer is too few for calibration", MagBufferCount); ///**********************************
printf("\r %d move board to calibrate ", MagBufferCount); ///**********************************
CalPercentage ();
} // end of test for active calibration flag
/////////////////////////////////////////////////////////////
}
// function runs the magnetic calibration
void fRunMagCalibration(struct MagCalibration *pthisMagCal, struct MagneticBuffer *pthisMagBuffer, struct MagSensor* pthisMag)
{
int8 i, j; // loop counters
int8 isolver; // magnetic solver used
// 4 element calibration case
if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS7CAL)
{
// age the existing fit error to avoid one good calibration locking out future updates
if (pthisMagCal->iValidMagCal)
{
pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL4CAL * (float) OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
}
// call the 4 element matrix inversion calibration
isolver = 4;
fUpdateCalibration4INV(pthisMagCal, pthisMagBuffer, pthisMag);
}
// 7 element calibration case
else if (pthisMagBuffer->iMagBufferCount < MINMEASUREMENTS10CAL)
{
// age the existing fit error to avoid one good calibration locking out future updates
if (pthisMagCal->iValidMagCal)
{
pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL7CAL * (float) OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
}
// call the 7 element eigenpair calibration
isolver = 7;
fUpdateCalibration7EIG(pthisMagCal, pthisMagBuffer, pthisMag);
}
// 10 element calibration case
else
{
// age the existing fit error to avoid one good calibration locking out future updates
if (pthisMagCal->iValidMagCal)
{
pthisMagCal->fFitErrorpc *= (1.0F + (float) INTERVAL10CAL * (float) OVERSAMPLE_RATIO / ((float) SENSORFS * FITERRORAGINGSECS));
}
// call the 10 element eigenpair calibration
isolver = 10;
fUpdateCalibration10EIG(pthisMagCal, pthisMagBuffer, pthisMag);
}
// the trial geomagnetic field must be in range (earth is 22uT to 67uT)
if ((pthisMagCal->ftrB >= MINBFITUT) && (pthisMagCal->ftrB <= MAXBFITUT))
{
// always accept the calibration if i) no previous calibration exists ii) the calibration fit is reduced or
// an improved solver was used giving a good trial calibration (4% or under)
if ((pthisMagCal->iValidMagCal == 0) ||
(pthisMagCal->ftrFitErrorpc <= pthisMagCal->fFitErrorpc) ||
((isolver > pthisMagCal->iValidMagCal) && (pthisMagCal->ftrFitErrorpc <= 4.0F)))
{
// accept the new calibration solution
pthisMagCal->iValidMagCal = isolver;
pthisMagCal->fFitErrorpc = pthisMagCal->ftrFitErrorpc;
pthisMagCal->fB = pthisMagCal->ftrB;
for (i = CHX; i <= CHZ; i++)
{
pthisMagCal->fV[i] = pthisMagCal->ftrV[i];
for (j = CHX; j <= CHZ; j++)
{
pthisMagCal->finvW[i][j] = pthisMagCal->ftrinvW[i][j];
}
}
} // end of test to accept the new calibration
} // end of test for geomagnetic field strength in range
// reset the calibration in progress flag to allow writing to the magnetic buffer
pthisMagCal->iCalInProgress = 0;
return;
}
