Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-29 20:18:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Allow user to relax pre-flight GPS checks

Browse Source
This commit is contained in:
Paul Riseborough 2015-11-12 20:39:15 +11:00
parent d13b4c2dc3
commit 6522fb2621
3 changed files with 26 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

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

1
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

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

23
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

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