From 20a3f9782e573bc77f3d022c9e53a4cf16e452f0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 10 Oct 2015 04:59:47 +1100 Subject: [PATCH] AP_NavEKF2: Add full set of selectable pre-flight GPS checks --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 6 + libraries/AP_NavEKF2/AP_NavEKF2.h | 1 + libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 14 +- .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 159 ++++++++++++++++-- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 5 + libraries/AP_NavEKF2/AP_NavEKF2_core.h | 27 ++- 6 files changed, 192 insertions(+), 20 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 485a17ba92..514aa51eb0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -364,6 +364,12 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("WIND_PSCALE", 31, NavEKF2, _wndVarHgtRateScale, 0.5f), + // @Param: GPS_CHECK + // @DisplayName: GPS preflight check + // @Description: 1 byte bitmap of GPS preflight checks to perform. Set to 0 to bypass all checks. Set to 255 perform all checks. Set to 3 to check just the number of satellites and HDoP. Set to 31 for the most rigorous checks that will still allow checks to pass when the copter is moving, eg launch from a boat. + // @Bitmask: 0:NSats,1:HDoP,2:speed error,3:horiz pos error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed + // @User: Advanced + AP_GROUPINFO("GPS_CHECK", 32, NavEKF2, _gpsCheck, 31), AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 3beb512d9a..335e6aa53a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -264,6 +264,7 @@ private: AP_Int8 _altSource; // Primary alt source during optical flow navigation. 0 = use Baro, 1 = use range finder. AP_Float _gyroScaleProcessNoise;// gyro scale factor state process noise : 1/s AP_Float _rngNoise; // Range finder noise : m + AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed // Tuning parameters const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index fd46784962..8594ca2edd 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -425,11 +425,15 @@ void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const 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_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 + 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_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 + faults.flags.bad_horiz_vel = gpsCheckStatus.bad_horiz_vel; // The GPS horizontal speed is excessive (check assumes the vehicle is static) } // send an EKF_STATUS message to GCS diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index d82c989f01..f6634fd1d5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -28,16 +28,60 @@ extern const AP_HAL::HAL& hal; */ bool NavEKF2_core::calcGpsGoodToAlign(void) { - // fail if reported speed accuracy greater than threshold - gpsCheckStatus.bad_sAcc = (gpsSpdAccuracy > 1.0f); + bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend._gpsCheck & MASK_GPS_SPD_ERR); + + // Report check result as a text string and bitmask + if (gpsSpdAccFail) { + 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 + bool numSatsFail = (_ahrs->get_gps().num_sats() < 6) && (frontend._gpsCheck & MASK_GPS_NSATS); + + // Report check result as a text string and bitmask + 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 + bool hdopFail = (_ahrs->get_gps().get_hdop() > 250) && (frontend._gpsCheck & MASK_GPS_HDOP); + + // Report check result as a text string and bitmask + 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 - float hAcc; + float hAcc = 0.0f; + bool hAccFail; if (_ahrs->get_gps().horizontal_accuracy(hAcc)) { - gpsCheckStatus.bad_hAcc = (hAcc > 5.0f); + hAccFail = (hAcc > 5.0f) && (frontend._gpsCheck & MASK_GPS_POS_ERR); + } else { + hAccFail = false; + } + + // Report check result as a text string and bitmask + if (hAccFail) { + 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; + 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 @@ -56,23 +100,110 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) // fail if magnetometer innovations are outside limits indicating bad yaw // with bad yaw we are unable to use GPS - gpsCheckStatus.bad_yaw = (magTestRatio.x > 1.0f || magTestRatio.y > 1.0f); + bool yawFail; + if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f) && (frontend._gpsCheck & MASK_GPS_YAW_ERR)) { + yawFail = true; + } else { + yawFail = false; + } - // fail if not enough sats - gpsCheckStatus.bad_sats = (_ahrs->get_gps().num_sats() < 6); + // Report check result as a text string and bitmask + if (yawFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "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 + // This check can only be used when the vehicle is stationary + const struct Location &gpsloc = _ahrs->get_gps().location(); // Current location + const float posFiltTimeConst = 10.0f; // time constant used to decay position drift + // calculate time lapsesd since last update and limit to prevent numerical errors + float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); + lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; + // Sum distance moved + gpsDriftNE += location_diff(gpsloc_prev, gpsloc).length(); + gpsloc_prev = gpsloc; + // Decay distance moved exponentially to zero + gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); + // Clamp the fiter state to prevent excessive persistence of large transients + gpsDriftNE = min(gpsDriftNE,10.0f); + // Fail if more than 3 metres drift after filtering whilst on-ground + // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m + bool gpsDriftFail = (gpsDriftNE > 3.0f) && onGround && (frontend._gpsCheck & MASK_GPS_POS_DRIFT); + + // Report check result as a text string and bitmask + if (gpsDriftFail) { + 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 + bool gpsVertVelFail; + if (_ahrs->get_gps().have_vertical_velocity() && onGround) { + // check that the average vertical GPS velocity is close to zero + gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; + gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); + gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_VERT_SPD); + } else if ((frontend._fusionModeGPS == 0) && !_ahrs->get_gps().have_vertical_velocity()) { + // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail + gpsVertVelFail = true; + } else { + gpsVertVelFail = false; + } + + // Report check result as a text string and bitmask + if (gpsVertVelFail) { + 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; + } - // record time of fail if failing - if (gpsCheckStatus.bad_sAcc || gpsCheckStatus.bad_sats || gpsCheckStatus.bad_hAcc || gpsCheckStatus.bad_yaw) { + // Check that the horizontal GPS vertical velocity is reasonable after noise filtering + // This check can only be used if the vehicle is stationary + bool gpsHorizVelFail; + if (onGround) { + gpsHorizVelFilt = 0.1f * pythagorous2(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; + gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); + gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f) && (frontend._gpsCheck & MASK_GPS_HORIZ_SPD); + } else { + gpsHorizVelFail = false; + } + + // Report check result as a text string and bitmask + if (gpsHorizVelFail) { + hal.util->snprintf(prearm_fail_string, + sizeof(prearm_fail_string), + "GPS horizontal speed %.2fm/s (needs 0.30)", (double)gpsDriftNE); + gpsCheckStatus.bad_horiz_vel = true; + } else { + gpsCheckStatus.bad_horiz_vel = false; + } + + // record time of fail - assume fail first time called + if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail || lastGpsVelFail_ms == 0) { lastGpsVelFail_ms = imuSampleTime_ms; } - // We need 10 seconds of continual good data to start using GPS to reduce the chance of encountering bad data during takeoff + // continuous period without fail required to return a healthy status if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { - // we have not failed in the last 10 seconds return true; + } else { + hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF checking GPS"); + return false; } - - return false; } // update inflight calculaton that determines if GPS data is good enough for reliable navigation diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index ac8e629eed..612e16d24b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -88,6 +88,7 @@ void NavEKF2_core::InitialiseVariables() magMeasTime_ms = imuSampleTime_ms; timeTasReceived_ms = 0; magYawResetTimer_ms = imuSampleTime_ms; + lastPreAlignGpsCheckTime_ms = imuSampleTime_ms; // initialise other variables gpsNoiseScaler = 1.0f; @@ -179,6 +180,10 @@ void NavEKF2_core::InitialiseVariables() lastInnovPassTime_ms = 0; lastInnovFailTime_ms = 0; gpsAccuracyGood = false; + memset(&gpsloc_prev, 0, sizeof(gpsloc_prev)); + gpsDriftNE = 0.0f; + gpsVertVelFilt = 0.0f; + gpsHorizVelFilt = 0.0f; } // Initialise the states from accelerometer and magnetometer data (if present) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index ebf25b5905..154d2a14b4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -32,6 +32,16 @@ #include #endif +// GPS pre-flight check bit locations +#define MASK_GPS_NSATS (1<<0) +#define MASK_GPS_HDOP (1<<1) +#define MASK_GPS_SPD_ERR (1<<2) +#define MASK_GPS_POS_ERR (1<<3) +#define MASK_GPS_YAW_ERR (1<<4) +#define MASK_GPS_POS_DRIFT (1<<5) +#define MASK_GPS_VERT_SPD (1<<6) +#define MASK_GPS_HORIZ_SPD (1<<7) + class AP_AHRS; class NavEKF2_core @@ -728,6 +738,13 @@ private: bool motorsArmed; // true when the motors have been armed bool prevMotorsArmed; // value of motorsArmed from previous frame + // variables used by the pre-initialisation GPS checks + struct Location gpsloc_prev; // LLH location of previous GPS measurement + uint32_t lastPreAlignGpsCheckTime_ms; // last time in msec the GPS quality was checked during pre alignment checks + float gpsDriftNE; // amount of drift detected in the GPS position during pre-flight GPs checks + float gpsVertVelFilt; // amount of filterred vertical GPS velocity detected durng pre-flight GPS checks + float gpsHorizVelFilt; // amount of filtered horizontal GPS velocity detected during pre-flight GPS checks + // variable used by the in-flight GPS quality check bool gpsSpdAccPass; // true when reported GPS speed accuracy passes in-flight checks bool ekfInnovationsPass; // true when GPS innovations pass in-flight checks @@ -822,9 +839,14 @@ private: struct { bool bad_sAcc:1; bool bad_hAcc:1; - bool bad_sats: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; bool bad_fix:1; + bool bad_horiz_vel:1; } gpsCheckStatus; // states held by magnetomter fusion across time steps @@ -849,6 +871,9 @@ private: } mag_state; + // string representing last reason for prearm failure + char prearm_fail_string[40]; + #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN // performance counters perf_counter_t _perf_UpdateFilter;