From 54ac147ae8930f1c280a0bd5c25fa90eaf4c6e0d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 15 May 2019 09:36:11 +0200 Subject: [PATCH] Ekf: add set_min_required_gps_health_time() to allow configure minimum GPS health time This does not change the default of 10 seconds. --- EKF/ekf.h | 4 ++++ EKF/gps_checks.cpp | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 88cce68191..a5c95b1a55 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -261,6 +261,9 @@ public: // use the latest IMU data at the current time horizon. Quatf calculate_quaternion() const; + // set minimum continuous period without GPS fail required to mark a healthy GPS status + void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } + private: static constexpr uint8_t _k_num_states{24}; ///< number of EKF states @@ -408,6 +411,7 @@ private: uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks float _gps_error_norm{1.0f}; ///< normalised gps error + uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time // Variables used to publish the WGS-84 location of the EKF local NED origin uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index edaefae7ef..e70aca5981 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -242,6 +242,6 @@ bool Ekf::gps_is_good(const gps_message &gps) _last_gps_pass_us = _time_last_imu; } - // continuous period without fail of 10 seconds required to return a healthy status - return (_time_last_imu - _last_gps_fail_us > (uint64_t)1e7); + // continuous period without fail of x seconds required to return a healthy status + return _time_last_imu - _last_gps_fail_us > (uint64_t)_min_gps_health_time_us; }