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);