From 5d7faefa844d53945966d5cbb3b26d34d53a0072 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 29 Nov 2022 16:43:36 +0100 Subject: [PATCH] ekf2: don't add invalid GNSS samples to the buffer --- src/modules/ekf2/EKF/estimator_interface.cpp | 5 ++++- src/modules/ekf2/EKF/estimator_interface.h | 1 - 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index daea2db9d9..e08c7b7124 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -151,13 +151,16 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) if (time_us >= static_cast(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) { + if (!gps.vel_ned_valid) { + return; + } + gpsSample gps_sample_new; gps_sample_new.time_us = time_us; gps_sample_new.vel = gps.vel_ned; - _gps_speed_valid = gps.vel_ned_valid; gps_sample_new.sacc = gps.sacc; gps_sample_new.hacc = gps.eph; gps_sample_new.vacc = gps.epv; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index edbd29776c..5dd181bde9 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -330,7 +330,6 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized bool _NED_origin_initialised{false}; - bool _gps_speed_valid{false}; float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin