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