From 204a7fb3056266e0a21b869391cee52edde613ab Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 26 Feb 2025 12:57:53 -0500 Subject: [PATCH] ekf2: GNSS checks relax pos/vel req once passing --- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 81b3fcc76b6f..269d05bb4b98 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -68,12 +68,17 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Check the position dilution of precision _gps_check_fail_status.flags.pdop = (gps.pdop > _params.req_pdop); - // Check the reported horizontal and vertical position accuracy - _gps_check_fail_status.flags.hacc = (gps.hacc > _params.req_hacc); - _gps_check_fail_status.flags.vacc = (gps.vacc > _params.req_vacc); + // Check the reported horizontal position accuracy (relaxed once passing) + const float max_horiz_accuracy = _control_status.flags.gps ? 2.f * _params.req_hacc : _params.req_hacc; + _gps_check_fail_status.flags.hacc = (gps.hacc > max_horiz_accuracy); - // Check the reported speed accuracy - _gps_check_fail_status.flags.sacc = (gps.sacc > _params.req_sacc); + // Check the reported vertical position accuracy (relaxed once passing) + const float max_vert_accuracy = _control_status.flags.gps ? 2.f * _params.req_vacc : _params.req_vacc; + _gps_check_fail_status.flags.vacc = (gps.vacc > max_vert_accuracy); + + // Check the reported speed accuracy (relaxed once passing) + const float max_speed_accuracy = _control_status.flags.gps ? 2.f * _params.req_sacc : _params.req_sacc; + _gps_check_fail_status.flags.sacc = (gps.sacc > max_speed_accuracy); // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f;