// Control filter mode transitions
void NavEKF2_core::controlFilterModes()
{
// Determine motor arm status
prevMotorsArmed = motorsArmed;
motorsArmed = hal.util->get_soft_armed();
if (motorsArmed && !prevMotorsArmed) {
// set the time at which we arm to assist with checks
timeAtArming_ms = imuSampleTime_ms;
}
// Detect if we are in flight on or ground
detectFlight();
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
setWindMagStateLearningMode();
// Check the alignmnent status of the tilt and yaw attitude
// Used during initial bootstrap alignment of the filter
checkAttitudeAlignmentStatus();
// Control reset of yaw and magnetic field states
controlMagYawReset();
// Set the type of inertial navigation aiding used
setAidingMode();
}
// Set inertial navigation aiding mode
void NavEKF2_core::setAidingMode()
{
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode;
// Determine if we should change aiding mode
switch (PV_AidingMode) {
case AID_NONE: {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
bool canUseExtNav = readyToUseExtNav();
if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && filterIsStable) {
PV_AidingMode = AID_RELATIVE;
}
}
break;
case AID_RELATIVE: {
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
// Enable switch to absolute position mode if GPS is available
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) {
PV_AidingMode = AID_ABSOLUTE;
} else if (flowSensorTimeout || flowFusionTimeout) {
PV_AidingMode = AID_NONE;
}
}
break;
case AID_ABSOLUTE: {
// Find the minimum time without data required to trigger any check
uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
// Check if optical flow data is being used
bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
// Check if airspeed data is being used
bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
// Check if range beacon data is being used
bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
// Check if GPS is being used
bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
// Check if attitude drift has been constrained by a measurement source
bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
// check if position drift has been constrained by a measurement source
bool posAiding = posUsed || rngBcnUsed;
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false;
if (!attAiding) {
attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
(imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false;
if (!posAiding ) {
uint16_t maxLossTime_ms;
if (!velAiding) {
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
} else {
maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
}
posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
(imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
}
if (attAidLossCritical) {
// if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE;
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
tasTimeout = true;
gpsNotAvailable = true;
} else if (posAidLossCritical) {
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true;
velTimeout = true;
rngBcnTimeout = true;
gpsNotAvailable = true;
}
break;
}
}
// check to see if we are starting or stopping aiding and set states and modes as required
if (PV_AidingMode != PV_AidingModePrev) {
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
switch (PV_AidingMode) {
case AID_NONE:
// We have ceased aiding
gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true;
velTimeout = true;
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f;
posTestRatio = 0.0f;
// store the current position to be used to keep reporting the last known position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
break;
case AID_RELATIVE:
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
posTimeout = true;
velTimeout = true;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms;
break;
case AID_ABSOLUTE: {
bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit);
bool canUseRangeBeacon = readyToUseRangeBeacon();
bool canUseExtNav = readyToUseExtNav();
// We have commenced aiding and GPS usage is allowed
if (canUseGPS) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
}
posTimeout = false;
velTimeout = false;
// We have commenced aiding and range beacon usage is allowed
if (canUseRangeBeacon) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
}
// We have commenced aiding and external nav usage is allowed
if (canUseExtNav) {
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z);
// handle yaw reset as special case
extNavYawResetRequest = true;
controlMagYawReset();
// handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
ResetHeight();
}
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms;
lastVelPassTime_ms = imuSampleTime_ms;
lastRngBcnPassTime_ms = imuSampleTime_ms;
}
break;
}
// Always reset the position and velocity when changing mode
ResetVelocity();
ResetPosition();
}
}
// 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;
}
}
