From 6522fb262148873b0a211e4239b1674ddadfc655 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Nov 2015 20:39:15 +1100 Subject: [PATCH] AP_NavEKF2: Allow user to relax pre-flight GPS checks --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 12 ++++++++++ libraries/AP_NavEKF2/AP_NavEKF2.h | 1 + .../AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp | 23 +++++++++++-------- 3 files changed, 26 insertions(+), 10 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index f63e30a646..bea16998d7 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -34,6 +34,7 @@ #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 3 #define GSCALE_PNOISE_DEFAULT 3.0E-03f +#define CHECK_SCALER_DEFAULT 100 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2) // rover defaults @@ -57,6 +58,7 @@ #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 3 #define GSCALE_PNOISE_DEFAULT 3.0E-03f +#define CHECK_SCALER_DEFAULT 100 #elif APM_BUILD_TYPE(APM_BUILD_ArduPlane) // plane defaults @@ -80,6 +82,7 @@ #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 3 #define GSCALE_PNOISE_DEFAULT 3.0E-03f +#define CHECK_SCALER_DEFAULT 150 #else // build type not specified, use copter defaults @@ -103,6 +106,7 @@ #define FLOW_NOISE_DEFAULT 0.25f #define FLOW_GATE_DEFAULT 3 #define GSCALE_PNOISE_DEFAULT 3.0E-03f +#define CHECK_SCALER_DEFAULT 100 #endif // APM_BUILD_DIRECTORY @@ -407,6 +411,14 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { // @User: Advanced AP_GROUPINFO("IMU_MASK", 33, NavEKF2, _imuMask, 1), + // @Param: CHECK_SCALE + // @DisplayName: GPS accuracy check scaler (%) + // @Description: This scales the thresholds that are used to check GPS accuracy before it is used by the EKF. A value of 100 is the default. Values greater than 100 increase and values less than 100 reduce the maximum GPS error the EKF will accept. A value of 200 will double the allowable GPS error. + // @Range: 50 200 + // @User: Advanced + // @Units: % + AP_GROUPINFO("CHECK_SCALE", 34, NavEKF2, _gpsCheckScaler, CHECK_SCALER_DEFAULT), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 87d92415bd..4168093857 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -310,6 +310,7 @@ private: AP_Float _rngNoise; // Range finder noise : m AP_Int8 _gpsCheck; // Bitmask controlling which preflight GPS checks are bypassed AP_Int8 _imuMask; // Bitmask of IMUs to instantiate EKF2 for + AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds // 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_VehicleStatus.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp index 9a9124638f..1da0036d88 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp @@ -23,6 +23,9 @@ extern const AP_HAL::HAL& hal; */ bool NavEKF2_core::calcGpsGoodToAlign(void) { + // User defined multiplier to be applied to check thresholds + float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler; + // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states // This enables us to handle large changes to the external magnetic field environment that occur before arming if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) { @@ -53,13 +56,13 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) 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); + bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && 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); + "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); gpsCheckStatus.bad_horiz_drift = true; } else { gpsCheckStatus.bad_horiz_drift = false; @@ -71,7 +74,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) // 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); + gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (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; @@ -83,7 +86,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) if (gpsVertVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS vertical speed %.2fm/s (needs 0.30)", (double)fabsf(gpsVertVelFilt)); + "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); gpsCheckStatus.bad_vert_vel = true; } else { gpsCheckStatus.bad_vert_vel = false; @@ -95,7 +98,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) 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); + gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); } else { gpsHorizVelFail = false; } @@ -104,7 +107,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) if (gpsHorizVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS horizontal speed %.2fm/s (needs 0.30)", (double)gpsDriftNE); + "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); gpsCheckStatus.bad_horiz_vel = true; } else { gpsCheckStatus.bad_horiz_vel = false; @@ -114,7 +117,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) float hAcc = 0.0f; bool hAccFail; if (_ahrs->get_gps().horizontal_accuracy(hAcc)) { - hAccFail = (hAcc > 5.0f) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); + hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } else { hAccFail = false; } @@ -123,20 +126,20 @@ bool NavEKF2_core::calcGpsGoodToAlign(void) if (hAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), - "GPS horiz error %.1f", (double)hAcc); + "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); gpsCheckStatus.bad_hAcc = true; } else { gpsCheckStatus.bad_hAcc = false; } // fail if reported speed accuracy greater than threshold - bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR); + bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (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); + "GPS speed error %.1f (needs %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); gpsCheckStatus.bad_sAcc = true; } else { gpsCheckStatus.bad_sAcc = false;