fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 12:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Use stricter GPS accuracy test when optical flow is being used

Browse Source 
This adds some hysteresis to the automated optical flow selection and also reduces likelihood of bad GPS being used when operating indoors,
This commit is contained in:
Paul Riseborough
2018-05-14 20:53:19 +10:00
committed by Lorenz Meier
parent a80b3ab610
commit 84516760c0
1 changed files with 16 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/gps_checks.cpp
+16 -2
View File
@@ -119,13 +119,27 @@ bool Ekf::gps_is_good(struct gps_message *gps)
_gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop);
// Check the reported horizontal position accuracy
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
if (_control_status.flags.opt_flow) {
// optical flow is used when low and slow and often in a confined environment
// so tighten required GPS accuracy that affects position hold
// This also applies some hysteresis to the logic used to activate optical flow
_gps_check_fail_status.flags.hacc = (gps->eph > 0.7f * _params.req_hacc);
} else {
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
}
// Check the reported vertical position accuracy
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
// Check the reported speed accuracy
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
if (_control_status.flags.opt_flow) {
// Optical flow is used when low and slow and often in a confined environment
// so tighten required GPS accuracy that affects position hold
// This also applies some hysteresis to the logic used to activate optical flow
_gps_check_fail_status.flags.sacc = (gps->sacc > 0.7f * _params.req_sacc);
} else {
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
}
// Calculate position movement since last measurement
float delta_posN = 0.0f;