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.
This commit is contained in:
bresch
2023-07-13 14:26:12 +02:00
parent 715a1ff701
commit 141c6d77b7
3 changed files with 5 additions and 1 deletions
+1
View File
@@ -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 {
@@ -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)) {
+2 -1
View File
@@ -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);