// select fusion of velocity, position and height measurements
void NavEKF2_core::SelectVelPosFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
posVelFusionDelayed = true;
return;
} else {
posVelFusionDelayed = false;
}
// read GPS data from the sensor and check for new data in the buffer
readGpsData();
gpsDataToFuse = RecallGPS();
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
} else {
fuseVelData = false;
fusePosData = false;
}
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// If we are operating without any aiding, fuse in the last known position and zero velocity
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
if (fuseHgtData && PV_AidingMode == AID_NONE) {
gpsDataDelayed.vel.zero();
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
// only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration
if (accNavMag < 9.8f) {
fusePosData = true;
fuseVelData = true;
} else {
fusePosData = false;
fuseVelData = false;
}
}
// perform fusion
if (fuseVelData || fusePosData || fuseHgtData) {
FuseVelPosNED();
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false;
fuseHgtData = false;
fusePosData = false;
}
}
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;
}
}
}
// Processes incoming read events. They can come from the server or
// from the GPS device.
void GpsClient::processEvent( fd_set *fdMask )
{
if( ipcPort )
{
int sfd = clientData.getSock();
if( sfd != -1 && FD_ISSET( sfd, fdMask ) )
{
int loops = 0;
// Try to process several messages from the client in order. That
// is more effective as to wait for a new select call.
while( loops++ < 32 )
{
readServerMsg();
if( shutdown == true )
{
break;
}
// Check, if more bytes are available in the receiver buffer because we
// use blocking IO.
int bytes = 0;
// Number of bytes currently in the socket receiver buffer.
if( ioctl( sfd, FIONREAD, &bytes) == -1 )
{
qWarning() << "GpsClient::processEvent():"
<< "ioctl() returns with Errno="
<< errno
<< "," << strerror(errno);
break;
}
if( bytes <= 0 )
{
break;
}
}
}
}
if( fd != -1 && FD_ISSET( fd, fdMask ) )
{
if( readGpsData() == false )
{
// problem occurred, likely buffer overrun. we do restart the GPS
// receiving.
int error = errno; // Save errno
closeGps();
if( error == ECONNREFUSED )
{
// BT devices can reject a connection try. If we don't return here
// we run in an endless loop.
setShutdownFlag(true);
}
else
{
sleep(3);
// reopen connection
openGps( device.data(), ioSpeedDevice );
}
}
}
}
// select fusion of velocity, position and height measurements
void NavEKF2_core::SelectVelPosFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
posVelFusionDelayed = true;
return;
} else {
posVelFusionDelayed = false;
}
// read GPS data from the sensor and check for new data in the buffer
readGpsData();
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// Don't fuse velocity data if GPS doesn't support it
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = _ahrs->get_gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) {
if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gpsDataDelayed.vel -= velOffsetEarth;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gpsDataDelayed.pos.x -= posOffsetEarth.x;
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
}
} else {
fuseVelData = false;
fusePosData = false;
}
// we have GPS data to fuse and a request to align the yaw using the GPS course
if (gpsYawResetRequest) {
realignYawGPS();
}
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// If we are operating without any aiding, fuse in the last known position
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
if (fuseHgtData && PV_AidingMode == AID_NONE) {
gpsDataDelayed.vel.zero();
gpsDataDelayed.pos.x = lastKnownPositionNE.x;
gpsDataDelayed.pos.y = lastKnownPositionNE.y;
fusePosData = true;
fuseVelData = false;
}
// perform fusion
if (fuseVelData || fusePosData || fuseHgtData) {
FuseVelPosNED();
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false;
fuseHgtData = false;
fusePosData = false;
}
}
// select fusion of velocity, position and height measurements
void NavEKF2_core::SelectVelPosFusion()
{
// Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
// If so, don't fuse measurements on this time step to reduce frame over-runs
// Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
posVelFusionDelayed = true;
return;
} else {
posVelFusionDelayed = false;
}
// Check for data at the fusion time horizon
extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
// read GPS data from the sensor and check for new data in the buffer
readGpsData();
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// set fusion request flags
if (frontend->_fusionModeGPS <= 1) {
fuseVelData = true;
} else {
fuseVelData = false;
}
fusePosData = true;
extNavUsedForPos = false;
// correct GPS data for position offset of antenna phase centre relative to the IMU
Vector3f posOffsetBody = AP::gps().get_antenna_offset(gpsDataDelayed.sensor_idx) - accelPosOffset;
if (!posOffsetBody.is_zero()) {
// Don't fuse velocity data if GPS doesn't support it
if (fuseVelData) {
// TODO use a filtered angular rate with a group delay that matches the GPS delay
Vector3f angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
Vector3f velOffsetBody = angRate % posOffsetBody;
Vector3f velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
gpsDataDelayed.vel.x -= velOffsetEarth.x;
gpsDataDelayed.vel.y -= velOffsetEarth.y;
gpsDataDelayed.vel.z -= velOffsetEarth.z;
}
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
gpsDataDelayed.pos.x -= posOffsetEarth.x;
gpsDataDelayed.pos.y -= posOffsetEarth.y;
gpsDataDelayed.hgt += posOffsetEarth.z;
}
// copy corrected GPS data to observation vector
if (fuseVelData) {
velPosObs[0] = gpsDataDelayed.vel.x;
velPosObs[1] = gpsDataDelayed.vel.y;
velPosObs[2] = gpsDataDelayed.vel.z;
}
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
} else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// This is a special case that uses and external nav system for position
extNavUsedForPos = true;
activeHgtSource = HGT_SOURCE_EV;
fuseVelData = false;
fuseHgtData = true;
fusePosData = true;
velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y;
velPosObs[5] = extNavDataDelayed.pos.z;
// if compass is disabled, also use it for yaw
if (!use_compass()) {
extNavUsedForYaw = true;
if (!yawAlignComplete) {
extNavYawResetRequest = true;
magYawResetRequest = false;
gpsYawResetRequest = false;
controlMagYawReset();
finalInflightYawInit = true;
} else {
fuseEulerYaw();
}
} else {
extNavUsedForYaw = false;
}
} else {
fuseVelData = false;
fusePosData = false;
}
// we have GPS data to fuse and a request to align the yaw using the GPS course
if (gpsYawResetRequest) {
realignYawGPS();
}
// Select height data to be fused from the available baro, range finder and GPS sources
selectHeightForFusion();
// if we are using GPS, check for a change in receiver and reset position and height
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
// record the ID of the GPS that we are using for the reset
last_gps_idx = gpsDataDelayed.sensor_idx;
// Store the position before the reset so that we can record the reset delta
posResetNE.x = stateStruct.position.x;
posResetNE.y = stateStruct.position.y;
// Set the position states to the position from the new GPS
stateStruct.position.x = gpsDataNew.pos.x;
stateStruct.position.y = gpsDataNew.pos.y;
// Calculate the position offset due to the reset
posResetNE.x = stateStruct.position.x - posResetNE.x;
posResetNE.y = stateStruct.position.y - posResetNE.y;
// Add the offset to the output observer states
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.x += posResetNE.x;
storedOutput[i].position.y += posResetNE.y;
}
outputDataNew.position.x += posResetNE.x;
outputDataNew.position.y += posResetNE.y;
outputDataDelayed.position.x += posResetNE.x;
outputDataDelayed.position.y += posResetNE.y;
// store the time of the reset
lastPosReset_ms = imuSampleTime_ms;
// If we are alseo using GPS as the height reference, reset the height
if (activeHgtSource == HGT_SOURCE_GPS) {
// Store the position before the reset so that we can record the reset delta
posResetD = stateStruct.position.z;
// write to the state vector
stateStruct.position.z = -hgtMea;
// Calculate the position jump due to the reset
posResetD = stateStruct.position.z - posResetD;
// Add the offset to the output observer states
outputDataNew.position.z += posResetD;
outputDataDelayed.position.z += posResetD;
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedOutput[i].position.z += posResetD;
}
// store the time of the reset
lastPosResetD_ms = imuSampleTime_ms;
}
}
// If we are operating without any aiding, fuse in the last known position
// to constrain tilt drift. This assumes a non-manoeuvring vehicle
// Do this to coincide with the height fusion
if (fuseHgtData && PV_AidingMode == AID_NONE) {
velPosObs[3] = lastKnownPositionNE.x;
velPosObs[4] = lastKnownPositionNE.y;
fusePosData = true;
fuseVelData = false;
}
// perform fusion
if (fuseVelData || fusePosData || fuseHgtData) {
FuseVelPosNED();
// clear the flags to prevent repeated fusion of the same data
fuseVelData = false;
fuseHgtData = false;
fusePosData = false;
}
}
