void ControlLoopCompass(void)
{
//printf("\n");
// for (i = 0; i < miterations; i++)
// {
// call sensor driver simulation to get accel and mag data in integer counts
fSixDOFMyNEDSensorDrivers();
// update the magnetometer measurement buffer integer magnetometer data
fUpdateMagnetometerBuffer();
// remove hard and soft iron terms from Bp (uT) to get calibrated data Bc (uT)
fInvertMagCal();
// NED coordinate system
feCompassNED(fBcx, fBcy, fBcz, fGpx, fGpy, fGpz);
Conversion();
if (validmagcal !=0)
{
APhi6DOF = median(iPhi6DOF, &arr_medianas[0]);
AThe6DOF = median(iThe6DOF, &arr_medianas[1]);
APsi6DOF = median(iPsi6DOF, &arr_medianas[2]);
// printf("Yaw =%4d Pitch =%4d Roll =%4d \r", APhi6DOF, AThe6DOF, APsi6DOF);
APsi6DOF = 359 - APsi6DOF;
sprintf(buffer,"%04d",APsi6DOF);
vfnLCD_Write_Msg(buffer); //print when it is calibrated
ecompass_direction = APsi6DOF;
}
// try to find an improved calibration if update is enabled
else
{
fCalibrationUpdate();
}
loopcounter++;
// }
}
// function runs the sensor fusion algorithms
void Fusion_Run(void)
{
int8 initiatemagcal; // flag to initiate a new magnetic calibration
#if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
// magnetic DOF: remove hard and soft iron terms from Bp (uT) to get calibrated data Bc (uT)
fInvertMagCal(&thisMag, &thisMagCal);
// update magnetic buffer checking for i) absence of first all-zero magnetometer output and ii) no calibration in progress
// an all zero magnetometer reading can occur after power-on at rare intervals but it simply won't be used in the buffer
if (!((globals.loopcounter < 100) && (thisMag.iBpFast[X] == 0) && (thisMag.iBpFast[Y] == 0) && (thisMag.iBpFast[Z] == 0)) && !thisMagCal.iCalInProgress)
{
// update the magnetometer measurement buffer integer magnetometer data (typically at 25Hz)
iUpdateMagnetometerBuffer(&thisMagBuffer, &thisAccel, &thisMag, globals.loopcounter);
}
#endif
// 1DOF Pressure low pass filter algorithm
#if defined COMPUTE_1DOF_P_BASIC
thisSV_1DOF_P_BASIC.systick = SYST_CVR & 0x00FFFFFF;
fRun_1DOF_P_BASIC(&thisSV_1DOF_P_BASIC, &thisPressure, globals.loopcounter);
thisSV_1DOF_P_BASIC.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_1DOF_P_BASIC.systick < 0) thisSV_1DOF_P_BASIC.systick += SYST_RVR;
#endif
// 3DOF Accel Basic: call the tilt algorithm, low pass filters and Euler angle calculation
#if defined COMPUTE_3DOF_G_BASIC
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q3))
{
thisSV_3DOF_G_BASIC.systick = SYST_CVR & 0x00FFFFFF;
fRun_3DOF_G_BASIC(&thisSV_3DOF_G_BASIC, &thisAccel, globals.loopcounter, THISCOORDSYSTEM);
thisSV_3DOF_G_BASIC.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_3DOF_G_BASIC.systick < 0) thisSV_3DOF_G_BASIC.systick += SYST_RVR;
}
#endif
// 3DOF Magnetometer Basic: call the 2D vehicle compass algorithm
#if defined COMPUTE_3DOF_B_BASIC
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q3M))
{
thisSV_3DOF_B_BASIC.systick = SYST_CVR & 0x00FFFFFF;
fRun_3DOF_B_BASIC(&thisSV_3DOF_B_BASIC, &thisMag, globals.loopcounter, THISCOORDSYSTEM);
thisSV_3DOF_B_BASIC.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_3DOF_B_BASIC.systick < 0) thisSV_3DOF_B_BASIC.systick += SYST_RVR;
}
#endif
// 3DOF Gyro Basic: call the gyro integration algorithm
#if defined COMPUTE_3DOF_Y_BASIC
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q3G))
{
thisSV_3DOF_Y_BASIC.systick = SYST_CVR & 0x00FFFFFF;
fRun_3DOF_Y_BASIC(&thisSV_3DOF_Y_BASIC, &thisGyro, globals.loopcounter, THISCOORDSYSTEM,
OVERSAMPLE_RATIO);
thisSV_3DOF_Y_BASIC.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_3DOF_Y_BASIC.systick < 0) thisSV_3DOF_Y_BASIC.systick += SYST_RVR;
}
#endif
// 6DOF Accel / Mag: Basic: call the eCompass orientation algorithm, low pass filters and Euler angle calculation
#if defined COMPUTE_6DOF_GB_BASIC
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q6MA))
{
thisSV_6DOF_GB_BASIC.systick = SYST_CVR & 0x00FFFFFF;
fRun_6DOF_GB_BASIC(&thisSV_6DOF_GB_BASIC, &thisMag, &thisAccel, globals.loopcounter, THISCOORDSYSTEM);
thisSV_6DOF_GB_BASIC.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_6DOF_GB_BASIC.systick < 0) thisSV_6DOF_GB_BASIC.systick += SYST_RVR;
}
#endif
// 6DOF Accel / Gyro: call the Kalman orientation algorithm
#if defined COMPUTE_6DOF_GY_KALMAN
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q6AG))
{
thisSV_6DOF_GY_KALMAN.systick = SYST_CVR & 0x00FFFFFF;
fRun_6DOF_GY_KALMAN(&thisSV_6DOF_GY_KALMAN, &thisAccel, &thisGyro, THISCOORDSYSTEM, OVERSAMPLE_RATIO);
thisSV_6DOF_GY_KALMAN.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_6DOF_GY_KALMAN.systick < 0) thisSV_6DOF_GY_KALMAN.systick += SYST_RVR;
}
#endif
// 9DOF Accel / Mag / Gyro: apply the Kalman filter
#if defined COMPUTE_9DOF_GBY_KALMAN
if (PARALLELNOTSEQUENTIAL || (globals.QuaternionPacketType == Q9))
{
thisSV_9DOF_GBY_KALMAN.systick = SYST_CVR & 0x00FFFFFF;
fRun_9DOF_GBY_KALMAN(&thisSV_9DOF_GBY_KALMAN, &thisAccel, &thisMag, &thisGyro, &thisMagCal, THISCOORDSYSTEM, OVERSAMPLE_RATIO);
thisSV_9DOF_GBY_KALMAN.systick -= SYST_CVR & 0x00FFFFFF;
if (thisSV_9DOF_GBY_KALMAN.systick < 0) thisSV_9DOF_GBY_KALMAN.systick += SYST_RVR;
}
#endif // COMPUTE_9DOF_GBY_KALMAN
// 6DOF and 9DOF: decide whether to initiate a magnetic calibration
#if defined COMPUTE_3DOF_B_BASIC || defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN
// check no magnetic calibration is in progress
if (!thisMagCal.iCalInProgress)
{
// do the first 4 element calibration immediately there are a minimum of MINMEASUREMENTS4CAL
initiatemagcal = (!thisMagCal.iMagCalHasRun && (thisMagBuffer.iMagBufferCount >= MINMEASUREMENTS4CAL));
// otherwise initiate a calibration at intervals depending on the number of measurements available
initiatemagcal |= ((thisMagBuffer.iMagBufferCount >= MINMEASUREMENTS4CAL) &&
(thisMagBuffer.iMagBufferCount < MINMEASUREMENTS7CAL) &&
!(globals.loopcounter % INTERVAL4CAL));
initiatemagcal |= ((thisMagBuffer.iMagBufferCount >= MINMEASUREMENTS7CAL) &&
(thisMagBuffer.iMagBufferCount < MINMEASUREMENTS10CAL) &&
!(globals.loopcounter % INTERVAL7CAL));
initiatemagcal |= ((thisMagBuffer.iMagBufferCount >= MINMEASUREMENTS10CAL) &&
!(globals.loopcounter % INTERVAL10CAL));
// initiate the magnetic calibration if any of the conditions are met
if (initiatemagcal)
{
// set the flags denoting that a calibration is in progress
thisMagCal.iCalInProgress = 1;
thisMagCal.iMagCalHasRun = 1;
// enable the magnetic calibration task to run
mqxglobals.MagCal_Event_Flag = 1;
} // end of test whether to call calibration functions
} // end of test that no calibration is already in progress
#endif
// increment the loopcounter (used for time stamping magnetic data)
globals.loopcounter++;
return;
}
