aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF2: Add fix status to GPS check report message

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-08 16:24:53 -07:00 committed by Randy Mackay
parent ef5049862a
commit db4dfce7b1
3 changed files with 84 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -343,9 +343,11 @@ 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))
{
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;
@ -426,6 +428,10 @@ void NavEKF2_core::readGpsData()
// 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

1
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -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