// Update the filter status
void NavEKF2_core::updateFilterStatus(void)
{
// init return value
filterStatus.value = 0;
bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
// set individual flags
filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE); // The GPS is glitching
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3);
// Deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user
bool magCalDenied = !use_compass() || (PV_AidingMode == AID_NONE) || (frontend._magCal == 2);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core::readMagData()
{
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
hal.console->printf("EKF2 IMU%u switching to compass %u\n",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
}
}
}
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
storedMag.push(magDataNew);
}
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE));
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
bool magCalRequested =
((frontend->_magCal == 0) && inFlight) || // when flying
((frontend->_magCal == 1) && manoeuvring) || // when manoeuvring
((frontend->_magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed
(frontend->_magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (frontend->_magCal == 2) ||(onGround && frontend->_magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
inhibitMagStates = (!magCalRequested || magCalDenied);
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if (onGround) {
firstMagYawInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// decay GPS horizontal position offset to close to zero at a rate of 1 m/s for copters and 5 m/s for planes
// limit radius to a maximum of 50m
void NavEKF2_core::decayGpsOffset()
{
float offsetDecaySpd;
if (assume_zero_sideslip()) {
offsetDecaySpd = 5.0f;
} else {
offsetDecaySpd = 1.0f;
}
float lapsedTime = 0.001f*float(imuSampleTime_ms - lastDecayTime_ms);
lastDecayTime_ms = imuSampleTime_ms;
float offsetRadius = pythagorous2(gpsPosGlitchOffsetNE.x, gpsPosGlitchOffsetNE.y);
// decay radius if larger than offset decay speed multiplied by lapsed time (plus a margin to prevent divide by zero)
if (offsetRadius > (offsetDecaySpd * lapsedTime + 0.1f)) {
// Calculate the GPS velocity offset required. This is necessary to prevent the position measurement being rejected for inconsistency when the radius is being pulled back in.
gpsVelGlitchOffset = -gpsPosGlitchOffsetNE*offsetDecaySpd/offsetRadius;
// calculate scale factor to be applied to both offset components
float scaleFactor = constrain_float((offsetRadius - offsetDecaySpd * lapsedTime), 0.0f, 50.0f) / offsetRadius;
gpsPosGlitchOffsetNE.x *= scaleFactor;
gpsPosGlitchOffsetNE.y *= scaleFactor;
} else {
gpsVelGlitchOffset.zero();
gpsPosGlitchOffsetNE.zero();
}
}
// Detect if we are in flight or on ground
void NavEKF2_core::detectFlight()
{
/*
If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria.
Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where
onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for
both onGround and inFlight to be false if the status is uncertain, but they cannot both be true.
If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used
TODO - this logic should be moved out of the EKF and into the flight vehicle code.
*/
if (assume_zero_sideslip()) {
// To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
bool highGndSpd = false;
bool highAirSpd = false;
bool largeHgtChange = false;
// trigger at 8 m/s airspeed
if (_ahrs->airspeed_sensor_enabled()) {
const AP_Airspeed *airspeed = _ahrs->get_airspeed();
if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) {
highAirSpd = true;
}
}
// trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
highGndSpd = true;
}
// trigger if more than 10m away from initial height
if (fabsf(baroDataDelayed.hgt) > 10.0f) {
largeHgtChange = true;
}
// Determine to a high certainty we are flying
if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {
onGround = false;
inFlight = true;
}
// if is possible we are in flight, set the time this condition was last detected
if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) {
airborneDetectTime_ms = imuSampleTime_ms;
onGround = false;
}
// Determine if is is possible we are on the ground
if (highGndSpd || highAirSpd || largeHgtChange) {
inFlight = false;
}
// Determine to a high certainty we are not flying
// after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode
if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
onGround = true;
inFlight = false;
}
} else {
// Non fly forward vehicle, so can only use height and motor arm status
// If the motors are armed then we could be flying and if they are not armed then we are definitely not flying
if (motorsArmed) {
onGround = false;
} else {
inFlight = false;
onGround = true;
}
// If height has increased since exiting on-ground, then we definitely are flying
if (!onGround && ((stateStruct.position.z - posDownAtTakeoff) < -1.5f)) {
inFlight = true;
}
// If rangefinder has increased since exiting on-ground, then we definitely are flying
if (!onGround && ((rngMea - rngAtStartOfFlight) > 0.5f)) {
inFlight = true;
}
}
// store current on-ground and in-air status for next time
prevOnGround = onGround;
prevInFlight = inFlight;
// Store vehicle height and range prior to takeoff for use in post takeoff checks
if (!onGround && !prevOnGround) {
// store vertical position at start of flight to use as a reference for ground relative checks
posDownAtTakeoff = stateStruct.position.z;
// store the range finder measurement which will be used as a reference to detect when we have taken off
rngAtStartOfFlight = rngMea;
}
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_core::setWindMagStateLearningMode()
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
if (!inhibitWindStates && setWindInhibit) {
inhibitWindStates = true;
} else if (inhibitWindStates && !setWindInhibit) {
inhibitWindStates = false;
// set states and variances
if (yawAlignComplete && useAirspeed()) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
// which assumes the vehicle has launched into the wind
Vector3f tempEuler;
stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z);
stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z);
// set the wind sate variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f));
}
} else {
// set the variances using a typical wind speed
for (uint8_t index=22; index<=23; index++) {
P[index][index] = sq(5.0f);
}
}
}
// determine if the vehicle is manoevring
if (accNavMagHoriz > 0.5f) {
manoeuvring = true;
} else {
manoeuvring = false;
}
// Determine if learning of magnetic field states has been requested by the user
uint8_t magCal = effective_magCal();
bool magCalRequested =
((magCal == 0) && inFlight) || // when flying
((magCal == 1) && manoeuvring) || // when manoeuvring
((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
(magCal == 4); // all the time
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = !magCalRequested || magCalDenied;
if (!inhibitMagStates && setMagInhibit) {
inhibitMagStates = true;
} else if (inhibitMagStates && !setMagInhibit) {
inhibitMagStates = false;
if (magFieldLearned) {
// if we have already learned the field states, then retain the learned variances
P[16][16] = earthMagFieldVar.x;
P[17][17] = earthMagFieldVar.y;
P[18][18] = earthMagFieldVar.z;
P[19][19] = bodyMagFieldVar.x;
P[20][20] = bodyMagFieldVar.y;
P[21][21] = bodyMagFieldVar.z;
} else {
// set the variances equal to the observation variances
for (uint8_t index=18; index<=21; index++) {
P[index][index] = sq(frontend->_magNoise);
}
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination();
}
// request a reset of the yaw and magnetic field states if not done before
if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
magYawResetRequest = true;
}
}
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if (onGround) {
finalInflightYawInit = false;
finalInflightMagInit = false;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
if (inhibitMagStates && inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
} else {
stateIndexLim = 23;
}
}
// check for new valid GPS data and update stored measurement if available
void NavEKF2_core::readGpsData()
{
// check for new GPS data
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > 70) {
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer
gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity();
// Use the speed accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data before and after alignment using separate checks
if (PV_AidingMode != AID_ABSOLUTE) {
// Pre-alignment checks
gpsGoodToAlign = calcGpsGoodToAlign();
} else {
// Post-alignment checks
calcGpsGoodForFlight();
}
// Read the GPS locaton in WGS-84 lat,long,height coordinates
const struct Location &gpsloc = _ahrs->get_gps().location();
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if (gpsGoodToAlign && !validOrigin) {
setOrigin();
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
alignMagStateDeclination();
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
}
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
storedGPS.push(gpsDataNew);
// declare GPS available for use
gpsNotAvailable = false;
}
// Commence GPS aiding when able to
if (readyToUseGPS() && PV_AidingMode != AID_ABSOLUTE) {
PV_AidingMode = AID_ABSOLUTE;
// Initialise EKF position and velocity states to last GPS measurement
ResetPosition();
ResetVelocity();
}
} else {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
}
}
// We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
// If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
// and use the last estimated position as a synthetic GPS position
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
// Let other processes know that GPS is not available and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
// If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
// If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
if (PV_AidingMode == AID_ABSOLUTE && !useAirspeed() && !assume_zero_sideslip()) {
if (optFlowBackupAvailable) {
// we can do optical flow only nav
frontend->_fusionModeGPS = 3;
PV_AidingMode = AID_RELATIVE;
} else {
// store the current position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// put the filter into constant position mode
PV_AidingMode = AID_NONE;
// Reset the velocity and position states
ResetVelocity();
ResetPosition();
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f;
posTestRatio = 0.0f;
}
}
}
}
// 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
if (PV_AidingMode == 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 perferred option unless excluded by the user
if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit) {
PV_AidingMode = AID_ABSOLUTE;
} else if (optFlowDataPresent() && filterIsStable) {
PV_AidingMode = AID_RELATIVE;
}
} else if (PV_AidingMode == 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;
}
} else if (PV_AidingMode == AID_ABSOLUTE) {
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend->gpsRetryTimeUseTAS_ms : frontend->gpsRetryTimeNoTAS_ms;
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend->gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
// Let other processes know that GPS is not available and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
// If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
// If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
if (!useAirspeed() && !assume_zero_sideslip()) {
if (optFlowBackupAvailable) {
// attempt optical flow navigation
PV_AidingMode = AID_RELATIVE;
} else {
// put the filter into constant position mode
PV_AidingMode = AID_NONE;
}
}
} else if (gpsInhibit) {
// put the filter into constant position mode in response to an exernal request
PV_AidingMode = AID_NONE;
}
}
// 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.
if (PV_AidingMode == AID_NONE) {
// We have ceased aiding
GCS_MAVLINK::send_statustext_all(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;
} else if (PV_AidingMode == AID_RELATIVE) {
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
GCS_MAVLINK::send_statustext_all(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;
} else if (PV_AidingMode == AID_ABSOLUTE) {
// We have commenced aiding and GPS usage is allowed
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
posTimeout = false;
velTimeout = false;
// we need to reset the GPS timers to prevent GPS timeout logic being invoked on entry into GPS aiding
// this is because the EKF can be interrupted for an arbitrary amount of time during vehicle arming checks
lastTimeGpsReceived_ms = imuSampleTime_ms;
secondLastGpsTime_ms = imuSampleTime_ms;
// reset the last valid position fix time to prevent unwanted activation of GPS glitch logic
lastPosPassTime_ms = imuSampleTime_ms;
}
// Always reset the position and velocity when changing mode
ResetVelocity();
ResetPosition();
}
}
// check for new valid GPS data and update stored measurement if available
void NavEKF2_core::readGpsData()
{
// check for new GPS data
if (_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) {
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend._gpsDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
// Prevent time delay exceeding age of oldest IMU data in the buffer
gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// read the NED velocity from the GPS
gpsDataNew.vel = _ahrs->get_gps().velocity();
// Use the speed accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw speed accuracy data
float alpha = constrain_float(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
gpsSpdAccuracy *= (1.0f - alpha);
float gpsSpdAccRaw;
if (!_ahrs->get_gps().speed_accuracy(gpsSpdAccRaw)) {
gpsSpdAccuracy = 0.0f;
} else {
gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
}
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
if (_ahrs->get_gps().num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.0f;
} else if (_ahrs->get_gps().num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
gpsNoiseScaler = 1.4f;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f;
}
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
if (_ahrs->get_gps().have_vertical_velocity() && frontend._fusionModeGPS == 0) {
useGpsVertVel = true;
} else {
useGpsVertVel = false;
}
// Monitor quality of the GPS velocity data before and after alignment using separate checks
if (PV_AidingMode != AID_ABSOLUTE) {
// Pre-alignment checks
gpsQualGood = calcGpsGoodToAlign();
} else {
// Post-alignment checks
calcGpsGoodForFlight();
}
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
// If we don't have an origin, then set it to the current GPS coordinates
const struct Location &gpsloc = _ahrs->get_gps().location();
if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
} else if (gpsQualGood) {
// Set the NE origin to the current GPS position
setOrigin();
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
alignMagStateDeclination();
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
EKF_origin.alt = gpsloc.alt - baroDataNew.hgt;
// We are by definition at the origin at the instant of alignment so set NE position to zero
gpsDataNew.pos.zero();
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
if (isAiding && frontend._fusionModeGPS != 3) {
PV_AidingMode = AID_ABSOLUTE;
// Initialise EKF position and velocity states
ResetPosition();
ResetVelocity();
}
}
// calculate a position offset which is applied to NE position and velocity wherever it is used throughout code to allow GPS position jumps to be accommodated gradually
decayGpsOffset();
// save measurement to buffer to be fused later
StoreGPS();
// declare GPS available for use
gpsNotAvailable = false;
} else {
// report GPS fix status
gpsCheckStatus.bad_fix = true;
}
}
// We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
// If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
// and use the last estimated position as a synthetic GPS position
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = (flowDataValid && !hgtTimeout);
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend.gpsRetryTimeUseTAS_ms : frontend.gpsRetryTimeNoTAS_ms;
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend.gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
if (imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms) {
// Let other processes know that GPS i snota vailable and that a timeout has occurred
posTimeout = true;
velTimeout = true;
gpsNotAvailable = true;
// If we are currently reliying on GPS for navigation, then we need to switch to a non-GPS mode of operation
if (PV_AidingMode == AID_ABSOLUTE) {
// If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
// If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
if (!useAirspeed() && !assume_zero_sideslip()) {
if (optFlowBackupAvailable) {
// we can do optical flow only nav
frontend._fusionModeGPS = 3;
PV_AidingMode = AID_RELATIVE;
} else {
// store the current position
lastKnownPositionNE.x = stateStruct.position.x;
lastKnownPositionNE.y = stateStruct.position.y;
// put the filter into constant position mode
PV_AidingMode = AID_NONE;
// reset all glitch states
gpsPosGlitchOffsetNE.zero();
gpsVelGlitchOffset.zero();
// Reset the velocity and position states
ResetVelocity();
ResetPosition();
// Reset the normalised innovation to avoid false failing the bad position fusion test
velTestRatio = 0.0f;
posTestRatio = 0.0f;
}
}
}
}
// If not aiding we synthesise the GPS measurements at the last known position
if (PV_AidingMode == AID_NONE) {
if (imuSampleTime_ms - gpsDataNew.time_ms > 200) {
gpsDataNew.pos.x = lastKnownPositionNE.x;
gpsDataNew.pos.y = lastKnownPositionNE.y;
gpsDataNew.time_ms = imuSampleTime_ms-frontend._gpsDelay_ms;
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
// Prevent time delay exceeding age of oldest IMU data in the buffer
gpsDataNew.time_ms = max(gpsDataNew.time_ms,imuDataDelayed.time_ms);
// save measurement to buffer to be fused later
StoreGPS();
}
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF3_core::readMagData()
{
if (!_ahrs->get_compass()) {
allMagSensorsFailed = true;
return;
}
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
uint8_t maxCount = _ahrs->get_compass()->get_count();
if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
allMagSensorsFailed = true;
return;
}
// limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
if (use_compass() && ((_ahrs->get_compass()->last_update_usec() - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
frontend->logging.log_compass = true;
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
// Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
// before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
if (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
// search through the list of magnetometers
for (uint8_t i=1; i<maxCount; i++) {
uint8_t tempIndex = magSelectIndex + i;
// loop back to the start index if we have exceeded the bounds
if (tempIndex >= maxCount) {
tempIndex -= maxCount;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if (_ahrs->get_compass()->use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
magSelectIndex = tempIndex;
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
// reset the timeout flag and timer
magTimeout = false;
lastHealthyMagTime_ms = imuSampleTime_ms;
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false;
// request a reset of the magnetic field states
magStateResetRequest = true;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false;
}
}
}
// detect changes to magnetometer offset parameters and reset states
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
if (changeDetected) {
// zero the learned magnetometer bias states
stateStruct.body_magfield.zero();
// clear the measurement buffer
storedMag.reset();
}
lastMagOffsets = nowMagOffsets;
lastMagOffsetsValid = true;
// store time of last measurement update
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
// estimate of time magnetometer measurement was taken, allowing for delays
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
// Correct for the average intersampling delay due to the filter updaterate
magDataNew.time_ms -= localFilterTimeStep_ms/2;
// read compass data and scale to improve numerical conditioning
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
// check for consistent data between magnetometers
consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later
storedMag.push(magDataNew);
}
}
/* Monitor GPS data to see if quality is good enough to initialise the EKF
Monitor magnetometer innovations to to see if the heading is good enough to use GPS
Return true if all criteria pass for 10 seconds
We also record the failure reason so that prearm_failure_reason()
can give a good report to the user on why arming is failing
*/
bool NavEKF2_core::calcGpsGoodToAlign(void)
{
if (inFlight && assume_zero_sideslip() && !use_compass()) {
// this is a special case where a plane has launched without magnetometer
// is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
return true;
}
// User defined multiplier to be applied to check thresholds
float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler;
// If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
// This enables us to handle large changes to the external magnetic field environment that occur before arming
if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
magYawResetTimer_ms = imuSampleTime_ms;
}
if (imuSampleTime_ms - magYawResetTimer_ms > 5000) {
// reset heading and field states
Vector3f eulerAngles;
getEulerAngles(eulerAngles);
stateStruct.quat = calcQuatAndFieldStates(eulerAngles.x, eulerAngles.y);
// reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
magYawResetTimer_ms = imuSampleTime_ms;
}
// Check for significant change in GPS position if disarmed which indicates bad GPS
// This check can only be used when the vehicle is stationary
const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location
const float posFiltTimeConst = 10.0f; // time constant used to decay position drift
// calculate time lapsesd since last update and limit to prevent numerical errors
float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
// Sum distance moved
gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length();
gpsloc_prev = gpsloc;
// Decay distance moved exponentially to zero
gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
// Clamp the fiter state to prevent excessive persistence of large transients
gpsDriftNE = MIN(gpsDriftNE,10.0f);
// Fail if more than 3 metres drift after filtering whilst on-ground
// This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT);
// Report check result as a text string and bitmask
if (gpsDriftFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
gpsCheckStatus.bad_horiz_drift = true;
} else {
gpsCheckStatus.bad_horiz_drift = false;
}
// Check that the vertical GPS vertical velocity is reasonable after noise filtering
bool gpsVertVelFail;
if (_ahrs->get_gps().have_vertical_velocity() && onGround) {
// check that the average vertical GPS velocity is close to zero
gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f);
gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
} else if ((frontend->_fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) {
// If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail
gpsVertVelFail = true;
// if we have a 3D fix with no vertical velocity and
// EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not
// capable of giving a vertical velocity
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
frontend->_fusionModeGPS.set(1);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1");
}
} else {
gpsVertVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsVertVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler));
gpsCheckStatus.bad_vert_vel = true;
} else {
gpsCheckStatus.bad_vert_vel = false;
}
// Check that the horizontal GPS vertical velocity is reasonable after noise filtering
// This check can only be used if the vehicle is stationary
bool gpsHorizVelFail;
if (onGround) {
gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt;
gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f);
gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
} else {
gpsHorizVelFail = false;
}
// Report check result as a text string and bitmask
if (gpsHorizVelFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
gpsCheckStatus.bad_horiz_vel = true;
} else {
gpsCheckStatus.bad_horiz_vel = false;
}
// fail if horiziontal position accuracy not sufficient
float hAcc = 0.0f;
bool hAccFail;
if (_ahrs->get_gps().horizontal_accuracy(hAcc)) {
hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
} else {
hAccFail = false;
}
// Report check result as a text string and bitmask
if (hAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
gpsCheckStatus.bad_hAcc = true;
} else {
gpsCheckStatus.bad_hAcc = false;
}
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
// Report check result as a text string and bitmask
if (gpsSpdAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
gpsCheckStatus.bad_sAcc = true;
} else {
gpsCheckStatus.bad_sAcc = false;
}
// fail if satellite geometry is poor
bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
// Report check result as a text string and bitmask
if (hdopFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop()));
gpsCheckStatus.bad_hdop = true;
} else {
gpsCheckStatus.bad_hdop = false;
}
// fail if not enough sats
bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
// Report check result as a text string and bitmask
if (numSatsFail) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string),
"GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats());
gpsCheckStatus.bad_sats = true;
} else {
gpsCheckStatus.bad_sats = false;
}
// fail if magnetometer innovations are outside limits indicating bad yaw
// with bad yaw we are unable to use GPS
bool yawFail;
if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
yawFail = true;
} else {
yawFail = false;
}
// Report check result as a text string and bitmask
if (yawFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"Mag yaw error x=%.1f y=%.1f",
(double)magTestRatio.x,
(double)magTestRatio.y);
gpsCheckStatus.bad_yaw = true;
} else {
gpsCheckStatus.bad_yaw = false;
}
// assume failed first time through and notify user checks have started
if (lastGpsVelFail_ms == 0) {
hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
lastGpsVelFail_ms = imuSampleTime_ms;
}
// record time of fail
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
lastGpsVelFail_ms = imuSampleTime_ms;
}
// continuous period without fail required to return a healthy status
if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
return true;
}
return false;
}
