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; }