// 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; } } } }
// check for new valid GPS data and update stored measurement if available void NavEKF3_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); // Get which GPS we are using for position information gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor(); // read the NED velocity from the GPS gpsDataNew.vel = _ahrs->get_gps().velocity(); // Use the speed and position 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 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); gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f); } gpsPosAccuracy *= (1.0f - alpha); float gpsPosAccRaw; if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) { gpsPosAccuracy = 0.0f; } else { gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw); gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f); } gpsHgtAccuracy *= (1.0f - alpha); float gpsHgtAccRaw; if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) { gpsHgtAccuracy = 0.0f; } else { gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw); gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f); } // 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 { gpsGoodToAlign = false; } // 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(); // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances 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; // Set the uncertinty of the GPS origin height ekfOriginHgtVar = sq(gpsHgtAccuracy); } // 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; } frontend->logging.log_gps = true; } else { // report GPS fix status gpsCheckStatus.bad_fix = true; } } }
// 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(); } } }