Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Check gps vertical accuracy for aligning the GPS

This commit is contained in:
Michael du Breuil 2017-02-22 11:53:31 -07:00 committed by Tom Pittenger
parent 8b69f1708e
commit 1de198883d
4 changed files with 21 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -384,7 +384,7 @@ const AP_Param::GroupInfo NavEKF3::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, NavEKF3, _gpsCheck, 31),

1
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -492,6 +492,7 @@ void NavEKF3_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)

19
libraries/AP_NavEKF3/AP_NavEKF3_VehicleStatus.cpp
View File

@ -142,6 +142,23 @@ bool NavEKF3_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 +224,7 @@ bool NavEKF3_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;
}

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1099,6 +1099,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;