void calibrateIMU(float coeff[12]) {
float th[SAMPLES], pr[SAMPLES], data[3], regPara[2] ;
uint8_t i ;
chprintf((BaseSequentialStream *)&OUTPUT, "Calibrating Gyroscope....Keep the board stable..\r\n") ;
chThdSleepMilliseconds(1000) ;
for(i = 0 ; i < SAMPLES ; i++) {
readIMUData(GYRO_RAW_DATA, data) ;
pr[i] = data[0] ;
chThdSleepMilliseconds(2) ;
}
coeff[6] = (-1.0f * average(pr, SAMPLES)) ;
coeff[7] = 1.0f ;
for(i = 0 ; i < SAMPLES ; i++) {
readIMUData(GYRO_RAW_DATA, data) ;
pr[i] = data[1] ;
chThdSleepMilliseconds(2) ;
}
coeff[8] = (-1.0f * average(pr, SAMPLES)) ;
coeff[9] = 1.0f ;
for(i = 0 ; i < SAMPLES ; i++) {
readIMUData(GYRO_RAW_DATA, data) ;
pr[i] = data[2] ;
chThdSleepMilliseconds(2) ;
}
coeff[10] = (-1.0f * average(pr, SAMPLES)) ;
coeff[11] = 1.0f ;
coeff[0] = 0.0f ;
coeff[1] = 1.0f ;
coeff[2] = 0.0f ;
coeff[3] = 1.0f ;
coeff[4] = 0.0f ;
coeff[5] = 1.0f ;
}
int main()
{
// open data files
pImuFile = fopen ("IMU.txt","r");
pMagFile = fopen ("MAG.txt","r");
pGpsFile = fopen ("GPS.txt","r");
pAhrsFile = fopen ("ATT.txt","r");
pAdsFile = fopen ("NTUN.txt","r");
pTimeFile = fopen ("timing.txt","r");
pStateOutFile = fopen ("StateDataOut.txt","w");
pEulOutFile = fopen ("EulDataOut.txt","w");
pCovOutFile = fopen ("CovDataOut.txt","w");
pRefPosVelOutFile = fopen ("RefVelPosDataOut.txt","w");
pVelPosFuseFile = fopen ("VelPosFuse.txt","w");
pMagFuseFile = fopen ("MagFuse.txt","w");
pTasFuseFile = fopen ("TasFuse.txt","w");
pGpsRawINFile = fopen ("GPSraw.txt","r");
pGpsRawOUTFile = fopen ("GPSrawOut.txt","w");
memset(gpsRaw, 0, sizeof(gpsRaw));
// read test data from files for first timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
readTimingData();
while (!endOfData)
{
if ((IMUmsec >= msecStartTime) && (IMUmsec <= msecEndTime))
{
// Initialise states, covariance and other data
if ((IMUmsec > msecAlignTime) && !statesInitialised && (GPSstatus == 3))
{
InitialiseFilter();
}
// If valid IMU data and states initialised, predict states and covariances
if (statesInitialised)
{
// Run the strapdown INS equations every IMU update
UpdateStrapdownEquationsNED();
// debug code - could be tunred into a filter mnitoring/watchdog function
float tempQuat[4];
for (uint8_t j=0; j<=3; j++) tempQuat[j] = states[j];
quat2eul(eulerEst, tempQuat);
for (uint8_t j=0; j<=2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];
if (eulerDif[2] > pi) eulerDif[2] -= 2*pi;
if (eulerDif[2] < -pi) eulerDif[2] += 2*pi;
// store the predicted states for subsequent use by measurement fusion
StoreStates();
// Check if on ground - status is used by covariance prediction
OnGroundCheck();
// sum delta angles and time used by covariance prediction
summedDelAng = summedDelAng + correctedDelAng;
summedDelVel = summedDelVel + correctedDelVel;
dt += dtIMU;
// perform a covariance prediction if the total delta angle has exceeded the limit
// or the time limit will be exceeded at the next IMU update
if ((dt >= (covTimeStepMax - dtIMU)) || (summedDelAng.length() > covDelAngMax))
{
CovariancePrediction();
summedDelAng = summedDelAng.zero();
summedDelVel = summedDelVel.zero();
dt = 0.0f;
}
}
// Fuse GPS Measurements
if (newDataGps && statesInitialised)
{
// Convert GPS measurements to Pos NE, hgt and Vel NED
calcvelNED(velNED, gpsCourse, gpsGndSpd, gpsVelD);
calcposNED(posNED, gpsLat, gpsLon, gpsHgt, latRef, lonRef, hgtRef);
posNE[0] = posNED[0];
posNE[1] = posNED[1];
hgtMea = -posNED[2];
// set fusion flags
fuseVelData = true;
fusePosData = true;
fuseHgtData = true;
// recall states stored at time of measurement after adjusting for delays
RecallStates(statesAtVelTime, (IMUmsec - msecVelDelay));
RecallStates(statesAtPosTime, (IMUmsec - msecPosDelay));
RecallStates(statesAtHgtTime, (IMUmsec - msecHgtDelay));
// run the fusion step
FuseVelposNED();
}
else
{
fuseVelData = false;
fusePosData = false;
fuseHgtData = false;
}
// Fuse Magnetometer Measurements
if (newDataMag && statesInitialised)
{
fuseMagData = true;
RecallStates(statesAtMagMeasTime, (IMUmsec - msecMagDelay)); // Assume 50 msec avg delay for magnetometer data
}
else
{
fuseMagData = false;
}
if (statesInitialised) FuseMagnetometer();
// Fuse Airspeed Measurements
if (newAdsData && statesInitialised && VtasMeas > 8.0f)
{
fuseVtasData = true;
RecallStates(statesAtVtasMeasTime, (IMUmsec - msecTasDelay)); // assume 100 msec avg delay for airspeed data
FuseAirspeed();
}
else
{
fuseVtasData = false;
}
// debug output
//printf("Euler Angle Difference = %3.1f , %3.1f , %3.1f deg\n", rad2deg*eulerDif[0],rad2deg*eulerDif[1],rad2deg*eulerDif[2]);
WriteFilterOutput();
}
// read test data from files for next timestamp
readIMUData();
readGpsData();
readMagData();
readAirSpdData();
readAhrsData();
if (IMUmsec > msecEndTime)
{
CloseFiles();
endOfData = true;
}
}
}
