diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a4571eab1e..26650794db 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4234,88 +4234,95 @@ void NavEKF::readIMUData() void NavEKF::readGpsData() { // check for new GPS data - if ((_ahrs->get_gps().last_message_time_ms() != lastFixTime_ms) && - (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D)) - { - // store fix time from previous read - secondLastFixTime_ms = lastFixTime_ms; + if (_ahrs->get_gps().last_message_time_ms() != lastFixTime_ms) { + if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) { + // report GPS fix status + gpsCheckStatus.bad_fix = false; - // get current fix time - lastFixTime_ms = _ahrs->get_gps().last_message_time_ms(); + // store fix time from previous read + secondLastFixTime_ms = lastFixTime_ms; - // set flag that lets other functions know that new GPS data has arrived - newDataGps = true; + // get current fix time + lastFixTime_ms = _ahrs->get_gps().last_message_time_ms(); - // get state vectors that were stored at the time that is closest to when the the GPS measurement - // time after accounting for measurement delays - RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500))); - RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500))); + // set flag that lets other functions know that new GPS data has arrived + newDataGps = true; - // read the NED velocity from the GPS - velNED = _ahrs->get_gps().velocity(); + // get state vectors that were stored at the time that is closest to when the the GPS measurement + // time after accounting for measurement delays + RecallStates(statesAtVelTime, (imuSampleTime_ms - constrain_int16(_msecVelDelay, 0, 500))); + RecallStates(statesAtPosTime, (imuSampleTime_ms - constrain_int16(_msecPosDelay, 0, 500))); - // 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 * (lastFixTime_ms - secondLastFixTime_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 + velNED = _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 && !constPosMode) { - gpsNoiseScaler = 1.0f; - } else if (_ahrs->get_gps().num_sats() == 5 && !constPosMode) { - 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() && _fusionModeGPS == 0) { - useGpsVertVel = true; - } else { - useGpsVertVel = false; - } - - // Monitor quality of the GPS velocity data for alignment - gpsGoodToAlign = calcGpsGoodToAlign(); - - // Monitor qulaity of GPS data inflight - 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) { - gpsPosNE = location_diff(EKF_origin, gpsloc); - } else if (gpsGoodToAlign){ - // 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 - hgtMea; - // We are by definition at the origin at the instant of alignment so set NE position to zero - gpsPosNE.zero(); - // If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode - if (vehicleArmed && _fusionModeGPS != 3) { - constPosMode = false; - PV_AidingMode = AID_ABSOLUTE; - gpsNotAvailable = false; - // 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 * (lastFixTime_ms - secondLastFixTime_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); } - } - // 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(); + // check if we have enough GPS satellites and increase the gps noise scaler if we don't + if (_ahrs->get_gps().num_sats() >= 6 && !constPosMode) { + gpsNoiseScaler = 1.0f; + } else if (_ahrs->get_gps().num_sats() == 5 && !constPosMode) { + 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() && _fusionModeGPS == 0) { + useGpsVertVel = true; + } else { + useGpsVertVel = false; + } + + // Monitor quality of the GPS velocity data for alignment + gpsGoodToAlign = calcGpsGoodToAlign(); + + // Monitor qulaity of GPS data inflight + 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) { + gpsPosNE = location_diff(EKF_origin, gpsloc); + } else if (gpsGoodToAlign){ + // 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 - hgtMea; + // We are by definition at the origin at the instant of alignment so set NE position to zero + gpsPosNE.zero(); + // If the vehicle is in flight (use arm status to determine) and GPS useage isn't explicitly prohibited, we switch to absolute position mode + if (vehicleArmed && _fusionModeGPS != 3) { + constPosMode = false; + PV_AidingMode = AID_ABSOLUTE; + gpsNotAvailable = false; + // 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(); + } else { + // report GPS fix status + gpsCheckStatus.bad_fix = true; + } } + // If no previous GPS lock or told not to use it, or EKF origin not set, we declare the GPS unavailable for use if ((_ahrs->get_gps().status() < AP_GPS::GPS_OK_FIX_3D) || _fusionModeGPS == 3 || !validOrigin) { gpsNotAvailable = true; @@ -4951,6 +4958,7 @@ void NavEKF::getFilterGpsStatus(nav_gps_status &faults) const faults.flags.bad_horiz_drift = gpsCheckStatus.bad_horiz_drift; // GPS horizontal drift is too large to start using GPS (check assumes vehicle is static) faults.flags.bad_hdop = gpsCheckStatus.bad_hdop; // reported HDoP is too large to start using GPS faults.flags.bad_vert_vel = gpsCheckStatus.bad_vert_vel; // GPS vertical speed is too large to start using GPS (check assumes vehicle is static) + faults.flags.bad_fix = gpsCheckStatus.bad_fix; // The GPS cannot provide the 3D fix required } /* diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 381cc52b21..38a87eb528 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -846,6 +846,7 @@ private: bool bad_horiz_drift:1; bool bad_hdop:1; bool bad_vert_vel:1; + bool bad_fix:1; } gpsCheckStatus; // states held by magnetomter fusion across time steps diff --git a/libraries/AP_NavEKF/AP_Nav_Common.h b/libraries/AP_NavEKF/AP_Nav_Common.h index 9080930d00..4b940da35e 100644 --- a/libraries/AP_NavEKF/AP_Nav_Common.h +++ b/libraries/AP_NavEKF/AP_Nav_Common.h @@ -50,6 +50,7 @@ union nav_gps_status { uint16_t bad_horiz_drift : 1; // 5 - true if the GPS horizontal position is drifting (this check assumes vehicle is static) uint16_t bad_hdop : 1; // 6 - true if the reported HDoP is insufficient to start using GPS uint16_t bad_vert_vel : 1; // 7 - true if the GPS vertical speed is too large to start using GPS (this check assumes vehicle is static) + uint16_t bad_fix : 1; // 8 - true if the GPS is not providing a 3D fix } flags; uint16_t value; };