From 141c6d77b75fd5f1efa658e454e078f35bdea6df Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 13 Jul 2023 14:26:12 +0200 Subject: [PATCH] ekf2-gnss-yaw: require RTK "fixed" Heading data is already available in "float" mode but really inaccurate (can be off by 45 degrees). It's then better to not use it when in "float" mode as it will disturb the EKF2's estimate and the filter can continue to keep an accurate heading even without direct observations. --- src/modules/ekf2/EKF/common.h | 1 + src/modules/ekf2/EKF/estimator_interface.cpp | 2 ++ src/modules/ekf2/EKF/gps_control.cpp | 3 ++- 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 5610885d14..6c2577b20c 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -193,6 +193,7 @@ struct gpsSample { float vacc{}; ///< 1-std vertical position error (m) float sacc{}; ///< 1-std speed error (m/sec) float yaw_acc{}; ///< 1-std yaw error (rad) + uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic }; struct magSample { diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 0955251586..51f3d5abb7 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -178,6 +178,8 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) gps_sample_new.hgt = (float)gps.alt * 1e-3f; + gps_sample_new.fix_type = gps.fix_type; + #if defined(CONFIG_EKF2_GNSS_YAW) if (PX4_ISFINITE(gps.yaw)) { diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 9848ad6b0b..e67936711b 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -328,7 +328,8 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi if (is_new_data_available) { - const bool continuing_conditions_passing = !gps_checks_failing; + const bool continuing_conditions_passing = !gps_checks_failing + && gps_sample.fix_type >= 6; // RTK "fixed" is required for accurate heading const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);