mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF3: Check gps vertical accuracy for aligning the GPS
This commit is contained in:
parent
8b69f1708e
commit
1de198883d
|
@ -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),
|
||||
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue