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