zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF: Add fix status to GPS check report message

mission-4.1.18
Paul Riseborough 9 years ago committed by Randy Mackay
parent
ba6387f206
commit
ef5049862a
3 changed files with 85 additions and 75 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 158
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 1
      libraries/AP_NavEKF/AP_NavEKF.h
  3. 1
      libraries/AP_NavEKF/AP_Nav_Common.h

158
libraries/AP_NavEKF/AP_NavEKF.cpp
Unescape View File

@ -4234,88 +4234,95 @@ void NavEKF::readIMUData() @@ -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;
// get current fix time
lastFixTime_ms = _ahrs->get_gps().last_message_time_ms();
// set flag that lets other functions know that new GPS data has arrived
newDataGps = true;
// 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)));
// read the NED velocity from the GPS
velNED = _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 * (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);
}
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;
// store fix time from previous read
secondLastFixTime_ms = lastFixTime_ms;
// get current fix time
lastFixTime_ms = _ahrs->get_gps().last_message_time_ms();
// set flag that lets other functions know that new GPS data has arrived
newDataGps = true;
// 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)));
// read the NED velocity from the GPS
velNED = _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 * (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);
}
// 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 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;
}
// 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();
// 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();
// 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 @@ -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
}
/*

1
libraries/AP_NavEKF/AP_NavEKF.h
Unescape View File

@ -846,6 +846,7 @@ private: @@ -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

1
libraries/AP_NavEKF/AP_Nav_Common.h
Unescape View File

@ -50,6 +50,7 @@ union nav_gps_status { @@ -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;
};

Write Preview
Loading…
Cancel
Save