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

NavEKF2: Add optional check for GPS vertical accuracy before aligning the EKF

Browse Source
This commit is contained in:
Michael du Breuil 2017-02-17 15:09:16 -07:00 committed by Tom Pittenger
parent 61023fc636
commit 8b69f1708e
4 changed files with 21 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -397,7 +397,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Param: GPS_CHECK
// @DisplayName: GPS preflight check
// @Description: This is a 1 byte bitmap controlling which GPS preflight checks are performed. 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
// @Bitmask: 0:NSats,1:HDoP,2:speed error,3:position error,4:yaw error,5:pos drift,6:vert speed,7:horiz speed
// @User: Advanced
AP_GROUPINFO("GPS_CHECK", 32, NavEKF2, _gpsCheck, 31),

1
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -506,6 +506,7 @@ void NavEKF2_core::getFilterGpsStatus(nav_gps_status &faults) const
// 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_vAcc = gpsCheckStatus.bad_vAcc; // reported vertical 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)

18
libraries/AP_NavEKF2/AP_NavEKF2_VehicleStatus.cpp
View File

@ -142,6 +142,22 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
gpsCheckStatus.bad_hAcc = false;
}
// Check for vertical GPS accuracy
float vAcc = 0.0f;
bool vAccFail = false;
if (_ahrs->get_gps().vertical_accuracy(vAcc)) {
vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
}
// Report check result as a text string and bitmask
if (vAccFail) {
hal.util->snprintf(prearm_fail_string,
sizeof(prearm_fail_string),
"GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
gpsCheckStatus.bad_vAcc = true;
} else {
gpsCheckStatus.bad_vAcc = false;
}
// fail if reported speed accuracy greater than threshold
bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
@ -207,7 +223,7 @@ bool NavEKF2_core::calcGpsGoodToAlign(void)
}
// record time of fail
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || vAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
lastGpsVelFail_ms = imuSampleTime_ms;
}

3
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -34,7 +34,7 @@
#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_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)
@ -1064,6 +1064,7 @@ private:
struct {
bool bad_sAcc:1;
bool bad_hAcc:1;
bool bad_vAcc:1;
bool bad_yaw:1;
bool bad_sats:1;
bool bad_VZ:1;