From 84516760c0d2f10812b87af79763454b68b4c000 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 14 May 2018 20:53:19 +1000 Subject: [PATCH] EKF: Use stricter GPS accuracy test when optical flow is being used This adds some hysteresis to the automated optical flow selection and also reduces likelihood of bad GPS being used when operating indoors, --- EKF/gps_checks.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index c9cbc5e66c..764536fb25 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -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;