From db4dfce7b14a16ea0e70193687d9044b72ddaf1e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 8 Oct 2015 16:24:53 -0700 Subject: [PATCH] AP_NavEKF2: Add fix status to GPS check report message --- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 158 +++++++++--------- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 1 + libraries/AP_NavEKF2/AP_NavEKF2_core.h | 1 + 3 files changed, 84 insertions(+), 76 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index fba3ea71cb..1d1206a206 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -343,89 +343,95 @@ bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3f &dVel, float &d void NavEKF2_core::readGpsData() { // check for new GPS data - if ((_ahrs->get_gps().last_message_time_ms() != lastTimeGpsReceived_ms) && - (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) - { - // store fix time from previous read - secondLastGpsTime_ms = lastTimeGpsReceived_ms; + 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; - // get current fix time - lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms(); + // store fix time from previous read + secondLastGpsTime_ms = lastTimeGpsReceived_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; + // get current fix time + lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms(); - // read the NED velocity from the GPS - gpsDataNew.vel = _ahrs->get_gps().velocity(); + // 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; - // 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); - } + // read the NED velocity from the GPS + gpsDataNew.vel = _ahrs->get_gps().velocity(); - // 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(); + // 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; } - - // 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; } // We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 726334ed74..fd46784962 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -429,6 +429,7 @@ void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use + faults.flags.bad_fix = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required } // send an EKF_STATUS message to GCS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 8ee8b40bb6..ebf25b5905 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -824,6 +824,7 @@ private: bool bad_hAcc:1; bool bad_sats:1; bool bad_yaw:1; + bool bad_fix:1; } gpsCheckStatus; // states held by magnetomter fusion across time steps