From 5177746c0098ea7282e7e9ce28db1461221b014c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 8 Oct 2015 12:34:23 -0700 Subject: [PATCH] AP_NavEKF: Add reporting of GPS check status --- libraries/AP_NavEKF/AP_NavEKF.cpp | 44 +++++++++++++++++++++++++++++++ libraries/AP_NavEKF/AP_NavEKF.h | 17 ++++++++++++ 2 files changed, 61 insertions(+) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index ec8b6bb424..a4571eab1e 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -4801,6 +4801,7 @@ void NavEKF::InitialiseVariables() gpsDriftNE = 0.0f; gpsVertVelFilt = 0.0f; gpsHorizVelFilt = 0.0f; + memset(&gpsCheckStatus, 0, sizeof(gpsCheckStatus)); } // return true if we should use the airspeed sensor @@ -4933,6 +4934,25 @@ void NavEKF::getFilterTimeouts(uint8_t &timeouts) const tasTimeout<<4); } +/* +return filter gps quality check status +*/ +void NavEKF::getFilterGpsStatus(nav_gps_status &faults) const +{ + // init return value + faults.value = 0; + + // set individual flags + faults.flags.bad_sAcc = gpsCheckStatus.bad_sAcc; // reported speed accuracy is insufficient + faults.flags.bad_hAcc = gpsCheckStatus.bad_hAcc; // reported horizontal position accuracy is insufficient + faults.flags.bad_yaw = gpsCheckStatus.bad_yaw; // EKF heading accuracy is too large for GPS use + faults.flags.bad_sats = gpsCheckStatus.bad_sats; // reported number of satellites is insufficient + faults.flags.bad_VZ = gpsCheckStatus.bad_VZ; // GPS vertical velocity is inconsistent with the IMU and Baro measurements + 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) +} + /* return filter function status as a bitmasked integer 0 = attitude estimate valid @@ -5242,11 +5262,17 @@ bool NavEKF::calcGpsGoodToAlign(void) hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS vert vel error %.1f", (double)velDiffAbs); + gpsCheckStatus.bad_VZ = true; + } else { + gpsCheckStatus.bad_VZ = false; } if (gpsSpdAccuracy > 1.0f) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS speed error %.1f", (double)gpsSpdAccuracy); + gpsCheckStatus.bad_sAcc = true; + } else { + gpsCheckStatus.bad_sAcc = false; } // fail if not enough sats @@ -5254,6 +5280,9 @@ bool NavEKF::calcGpsGoodToAlign(void) if (numSatsFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS numsats %u (needs 6)", _ahrs->get_gps().num_sats()); + gpsCheckStatus.bad_sats = true; + } else { + gpsCheckStatus.bad_sats = false; } // fail if satellite geometry is poor @@ -5261,6 +5290,9 @@ bool NavEKF::calcGpsGoodToAlign(void) if (hdopFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * _ahrs->get_gps().get_hdop())); + gpsCheckStatus.bad_hdop = true; + } else { + gpsCheckStatus.bad_hdop = false; } // fail if horiziontal position accuracy not sufficient @@ -5275,6 +5307,9 @@ bool NavEKF::calcGpsGoodToAlign(void) hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS horiz error %.1f", (double)hAcc); + gpsCheckStatus.bad_hAcc = true; + } else { + gpsCheckStatus.bad_hAcc = false; } // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states @@ -5305,6 +5340,9 @@ bool NavEKF::calcGpsGoodToAlign(void) "Mag yaw error x=%.1f y=%.1f", (double)magTestRatio.x, (double)magTestRatio.y); + gpsCheckStatus.bad_yaw = true; + } else { + gpsCheckStatus.bad_yaw = false; } // Check for significant change in GPS position if disarmed which indicates bad GPS @@ -5327,6 +5365,9 @@ bool NavEKF::calcGpsGoodToAlign(void) hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS drift %.1fm (needs 3.0)", (double)gpsDriftNE); + gpsCheckStatus.bad_horiz_drift = true; + } else { + gpsCheckStatus.bad_horiz_drift = false; } // Check that the vertical GPS vertical velocity is reasonable after noise filtering @@ -5346,6 +5387,9 @@ bool NavEKF::calcGpsGoodToAlign(void) hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS vertical speed %.2fm/s (needs 0.30)", (double)fabsf(gpsVertVelFilt)); + gpsCheckStatus.bad_vert_vel = true; + } else { + gpsCheckStatus.bad_vert_vel = false; } // Check that the horizontal GPS vertical velocity is reasonable after noise filtering diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 04e693c626..381cc52b21 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -259,6 +259,11 @@ public: */ void getFilterStatus(nav_filter_status &status) const; + /* + return filter gps quality check status + */ + void getFilterGpsStatus(nav_gps_status &status) const; + // send an EKF_STATUS_REPORT message to GCS void send_status_report(mavlink_channel_t chan); @@ -831,6 +836,18 @@ private: bool bad_sideslip:1; } faultStatus; + // flags indicating which GPS quality checks are failing + struct { + bool bad_sAcc:1; + bool bad_hAcc:1; + bool bad_yaw:1; + bool bad_sats:1; + bool bad_VZ:1; + bool bad_horiz_drift:1; + bool bad_hdop:1; + bool bad_vert_vel:1; + } gpsCheckStatus; + // states held by magnetomter fusion across time steps // magnetometer X,Y,Z measurements are fused across three time steps // to level computational load as this is an expensive operation