From ec556f17e3771b20e3d6526c24414180fd5cfc65 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 16 Apr 2026 02:13:25 -0800 Subject: [PATCH 1/5] refactor(ekf2): separate GNSS heading from position into independent topic Decouple GNSS heading (from dual-antenna / moving baseline RTK) from GNSS position throughout the pipeline. Previously, heading was bundled with position in sensor_gps/gnssSample with a single shared timestamp, causing timestamp corruption, quality gate coupling (position problems blocking heading fusion), and rate coupling. Changes: - Add VehicleGnssHeading.msg with independent timestamp - Extend VehicleGPSPosition to subscribe to sensor_gnss_relative and publish vehicle_gnss_heading (with fallback from sensor_gps.heading) - Add gnssYawSample struct and separate ring buffer in EKF2 - Decouple heading buffer pop from position in gps_control.cpp - Remove _gps_intermittent coupling from heading starting conditions - Update gnss_yaw_control.cpp to use gnssYawSample - Remove yaw fields from gnssSample (resolves the TODO at EKF2.cpp) Signed-off-by: Jacob Dahl --- msg/CMakeLists.txt | 1 + msg/VehicleGnssHeading.msg | 12 ++++ .../EKF/aid_sources/gnss/gnss_yaw_control.cpp | 30 ++++------ .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 20 ++++++- src/modules/ekf2/EKF/common.h | 10 +++- src/modules/ekf2/EKF/ekf.h | 4 +- src/modules/ekf2/EKF/estimator_interface.cpp | 39 +++++++++++-- src/modules/ekf2/EKF/estimator_interface.h | 7 ++- src/modules/ekf2/EKF2.cpp | 49 ++++++++++++---- src/modules/ekf2/EKF2.hpp | 5 ++ .../ekf2/test/sensor_simulator/gps.cpp | 11 ++-- src/modules/ekf2/test/sensor_simulator/gps.h | 1 + src/modules/logger/logged_topics.cpp | 1 + .../VehicleGPSPosition.cpp | 56 +++++++++++++++++++ .../VehicleGPSPosition.hpp | 10 ++++ 15 files changed, 208 insertions(+), 48 deletions(-) create mode 100644 msg/VehicleGnssHeading.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3ecb1ee59769..410a7d2f1592 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -234,6 +234,7 @@ set(msg_files VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg VehicleConstraints.msg + VehicleGnssHeading.msg VehicleImu.msg VehicleImuStatus.msg VehicleLocalPositionSetpoint.msg diff --git a/msg/VehicleGnssHeading.msg b/msg/VehicleGnssHeading.msg new file mode 100644 index 000000000000..324164bce0cc --- /dev/null +++ b/msg/VehicleGnssHeading.msg @@ -0,0 +1,12 @@ +# Validated GNSS heading from dual-antenna / moving baseline receiver pair. +# Published by the sensors module after selecting and validating heading from sensor_gnss_relative +# or falling back to sensor_gps heading fields. Consumed by EKF2 independently from position/velocity. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) - actual measurement time + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 heading # NED heading from dual antenna array (rad, [-PI, PI]) +float32 heading_accuracy # 1-sigma heading accuracy (rad) +float32 heading_offset # body-frame offset of antenna array (rad, [-PI, PI]) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index 709fb762117a..bcb81358cf91 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -45,7 +45,7 @@ #include -void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) +void Ekf::controlGnssYawFusion(const gnssYawSample &gnss_yaw_sample) { if (!(_params.ekf2_gps_ctrl & static_cast(GnssCtrl::YAW)) || _control_status.flags.gnss_yaw_fault) { @@ -54,11 +54,11 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) return; } - const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw); + const bool is_new_data_available = PX4_ISFINITE(gnss_yaw_sample.yaw); if (is_new_data_available) { - updateGnssYaw(gnss_sample); + updateGnssYaw(gnss_yaw_sample); const bool continuing_conditions_passing = _control_status.flags.tilt_align; @@ -67,13 +67,12 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed() - && !is_gnss_yaw_data_intermittent - && !_gps_intermittent; + && !is_gnss_yaw_data_intermittent; if (_control_status.flags.gnss_yaw) { if (continuing_conditions_passing) { - fuseGnssYaw(gnss_sample.yaw_offset); + fuseGnssYaw(gnss_yaw_sample.yaw_offset); const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); @@ -102,7 +101,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) || !isNorthEastAidingActive()) { // Reset before starting the fusion - if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) { + if (resetYawToGnss(gnss_yaw_sample.yaw, gnss_yaw_sample.yaw_offset)) { resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw); @@ -113,7 +112,7 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) } else if (!_aid_src_gnss_yaw.innovation_rejected) { // Do not force a reset but wait for the consistency check to pass _control_status.flags.gnss_yaw = true; - fuseGnssYaw(gnss_sample.yaw_offset); + fuseGnssYaw(gnss_yaw_sample.yaw_offset); } if (_control_status.flags.gnss_yaw) { @@ -122,31 +121,26 @@ void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) } } - } else if (_control_status.flags.gnss_yaw - && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) { - - // No yaw data in the message anymore. Stop until it comes back. - stopGnssYawFusion(); } } -void Ekf::updateGnssYaw(const gnssSample &gnss_sample) +void Ekf::updateGnssYaw(const gnssYawSample &gnss_yaw_sample) { // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset); + const float measured_hdg = wrap_pi(gnss_yaw_sample.yaw + gnss_yaw_sample.yaw_offset); - const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f; + const float yaw_acc = PX4_ISFINITE(gnss_yaw_sample.yaw_acc) ? gnss_yaw_sample.yaw_acc : 0.f; const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise)); float heading_pred; float heading_innov_var; VectorState H; - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON, + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_yaw_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); updateAidSourceStatus(_aid_src_gnss_yaw, - gnss_sample.time_us, // sample timestamp + gnss_yaw_sample.time_us, // sample timestamp measured_hdg, // observation R_YAW, // observation variance wrap_pi(heading_pred - measured_hdg), // innovation diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index dfc6d8e08d57..fd1a88aad1f4 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -96,12 +96,26 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } - if (_gps_data_ready) { + // GNSS yaw fusion - independent of position/velocity quality gates #if defined(CONFIG_EKF2_GNSS_YAW) - const gnssSample &gnss_sample = _gps_sample_delayed; - controlGnssYawFusion(gnss_sample); + + if (_gnss_yaw_buffer) { + const bool gnss_yaw_data_ready = _gnss_yaw_buffer->pop_first_older_than(imu_delayed.time_us, + &_gnss_yaw_sample_delayed); + + if (gnss_yaw_data_ready) { + controlGnssYawFusion(_gnss_yaw_sample_delayed); + + } else if (_control_status.flags.gnss_yaw + && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) { + // No yaw data arriving -- stop fusion + stopGnssYawFusion(); + } + } + #endif // CONFIG_EKF2_GNSS_YAW + if (_gps_data_ready) { controlGnssYawEstimator(_aid_src_gnss_vel); bool do_vel_pos_reset = false; diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 19c21baae807..ae353853007a 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -200,14 +200,18 @@ struct gnssSample { uint8_t fix_type{}; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time uint8_t nsats{}; ///< number of satellites used float pdop{}; ///< position dilution of precision - float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) - float yaw_acc{}; ///< 1-std yaw error (rad) - float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET bool spoofed{}; ///< true if GNSS data is spoofed bool jammed{}; ///< true if GNSS data is jammed Vector3f pos_body{}; ///< position of GPS antenna in body frame (m) }; +struct gnssYawSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + float yaw{}; ///< yaw angle from dual antenna GNSS (rad, [-PI, PI]) + float yaw_acc{}; ///< 1-std yaw error (rad) + float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET +}; + struct magSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector3f mag{}; ///< NED magnetometer body frame measurements (Gauss) diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index cd571e85fe42..62136fba65d1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -906,7 +906,7 @@ class Ekf final : public EstimatorInterface bool isGnssHgtResetAllowed(); # if defined(CONFIG_EKF2_GNSS_YAW) - void controlGnssYawFusion(const gnssSample &gps_sample); + void controlGnssYawFusion(const gnssYawSample &gnss_yaw_sample); void stopGnssYawFusion(); // fuse the yaw angle obtained from a dual antenna GPS unit @@ -916,7 +916,7 @@ class Ekf final : public EstimatorInterface // return true if the reset was successful bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset); - void updateGnssYaw(const gnssSample &gps_sample); + void updateGnssYaw(const gnssYawSample &gnss_yaw_sample); # endif // CONFIG_EKF2_GNSS_YAW diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index a6d6ca8038e6..e620b5e7ad3e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -48,6 +48,9 @@ EstimatorInterface::~EstimatorInterface() { #if defined(CONFIG_EKF2_GNSS) delete _gps_buffer; +# if defined(CONFIG_EKF2_GNSS_YAW) + delete _gnss_yaw_buffer; +# endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) delete _mag_buffer; @@ -192,19 +195,45 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) _gps_buffer->push(gnss_sample_new); _time_last_gps_buffer_push = _time_latest_us; + } else { + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); + } +} + #if defined(CONFIG_EKF2_GNSS_YAW) +void EstimatorInterface::setGnssYawData(const gnssYawSample &gnss_yaw_sample) +{ + if (!_initialised) { + return; + } + + if (_gnss_yaw_buffer == nullptr) { + _gnss_yaw_buffer = new TimestampedRingBuffer(_obs_buffer_length); - if (PX4_ISFINITE(gnss_sample.yaw)) { - _time_last_gnss_yaw_buffer_push = _time_latest_us; + if (_gnss_yaw_buffer == nullptr || !_gnss_yaw_buffer->valid()) { + delete _gnss_yaw_buffer; + _gnss_yaw_buffer = nullptr; + printBufferAllocationFailed("GNSS yaw"); + return; } + } -#endif // CONFIG_EKF2_GNSS_YAW + const int64_t time_us = gnss_yaw_sample.time_us - static_cast(_dt_ekf_avg * 5e5f); + + if (time_us >= static_cast(_gnss_yaw_buffer->get_newest().time_us + _min_obs_interval_us)) { + + gnssYawSample sample_new(gnss_yaw_sample); + sample_new.time_us = time_us; + + _gnss_yaw_buffer->push(sample_new); + _time_last_gnss_yaw_buffer_push = _time_latest_us; } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, - _min_obs_interval_us); + ECL_WARN("GNSS yaw data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gnss_yaw_buffer->get_newest().time_us, _min_obs_interval_us); } } +#endif // CONFIG_EKF2_GNSS_YAW + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_BAROMETER) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 93756090f4bd..d1b1845e90de 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -93,6 +93,10 @@ class EstimatorInterface const gnssSample &get_gps_sample_delayed() const { return _gps_sample_delayed; } +# if defined(CONFIG_EKF2_GNSS_YAW) + void setGnssYawData(const gnssYawSample &gnss_yaw_sample); +# endif // CONFIG_EKF2_GNSS_YAW + float gps_horizontal_position_drift_rate_m_s() const { return _gnss_checks.horizontal_position_drift_rate_m_s(); } float gps_vertical_position_drift_rate_m_s() const { return _gnss_checks.vertical_position_drift_rate_m_s(); } float gps_filtered_horizontal_velocity_m_s() const { return _gnss_checks.filtered_horizontal_velocity_m_s(); } @@ -420,7 +424,8 @@ class EstimatorInterface _control_status}; # if defined(CONFIG_EKF2_GNSS_YAW) - // innovation consistency check monitoring ratios + TimestampedRingBuffer *_gnss_yaw_buffer {nullptr}; + gnssYawSample _gnss_yaw_sample_delayed{}; uint64_t _time_last_gnss_yaw_buffer_push{0}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index d84cec52c0aa..57932ca701d5 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -807,6 +807,9 @@ void EKF2::Run() #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) UpdateGpsSample(ekf2_timestamps); +# if defined(CONFIG_EKF2_GNSS_YAW) + UpdateGnssYawSample(ekf2_timestamps); +# endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagSample(ekf2_timestamps); @@ -2623,15 +2626,6 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) return; //TODO: change and set to NAN } - if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) { - if (!PX4_ISFINITE(vehicle_gps_position.heading_offset) && PX4_ISFINITE(vehicle_gps_position.heading)) { - // Apply offset - float yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get())); - vehicle_gps_position.heading_offset = yaw_offset; - vehicle_gps_position.heading = matrix::wrap_pi(vehicle_gps_position.heading - yaw_offset); - } - } - const float altitude_amsl = static_cast(vehicle_gps_position.altitude_msl_m); const float altitude_ellipsoid = static_cast(vehicle_gps_position.altitude_ellipsoid_m); @@ -2652,9 +2646,6 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) .nsats = vehicle_gps_position.satellites_used, .pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop + vehicle_gps_position.vdop * vehicle_gps_position.vdop), - .yaw = vehicle_gps_position.heading, //TODO: move to different message - .yaw_acc = vehicle_gps_position.heading_accuracy, - .yaw_offset = vehicle_gps_position.heading_offset, .spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED, .jammed = vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED, .pos_body = Vector3f(vehicle_gps_position.antenna_offset_x, @@ -2680,6 +2671,40 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) } } +#if defined(CONFIG_EKF2_GNSS_YAW) +void EKF2::UpdateGnssYawSample(ekf2_timestamps_s &ekf2_timestamps) +{ + vehicle_gnss_heading_s gnss_heading; + + if (_vehicle_gnss_heading_sub.update(&gnss_heading)) { + + float yaw = gnss_heading.heading; + float yaw_offset = gnss_heading.heading_offset; + + // Apply EKF2_GPS_YAW_OFF if the driver didn't set an offset + if (fabsf(_param_ekf2_gps_yaw_off.get()) > 0.f) { + if (!PX4_ISFINITE(yaw_offset) && PX4_ISFINITE(yaw)) { + yaw_offset = matrix::wrap_pi(math::radians(_param_ekf2_gps_yaw_off.get())); + yaw = matrix::wrap_pi(yaw - yaw_offset); + } + } + + if (!PX4_ISFINITE(yaw_offset)) { + yaw_offset = 0.f; + } + + gnssYawSample gnss_yaw_sample{ + .time_us = (gnss_heading.timestamp_sample > 0) ? gnss_heading.timestamp_sample : gnss_heading.timestamp, + .yaw = yaw, + .yaw_acc = gnss_heading.heading_accuracy, + .yaw_offset = yaw_offset, + }; + + _ekf.setGnssYawData(gnss_yaw_sample); + } +} +#endif // CONFIG_EKF2_GNSS_YAW + float EKF2::altEllipsoidToAmsl(float ellipsoid_alt) const { return ellipsoid_alt - _geoid_height_lpf.getState(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index b350d9c10250..46bc1c3d4c3c 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -87,6 +87,7 @@ #include #include #include +#include #include #if defined(CONFIG_EKF2_AIRSPEED) @@ -224,6 +225,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem void PublishGnssHgtBias(const hrt_abstime ×tamp); void PublishYawEstimatorStatus(const hrt_abstime ×tamp); void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +# if defined(CONFIG_EKF2_GNSS_YAW) + void UpdateGnssYawSample(ekf2_timestamps_s &ekf2_timestamps); +# endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); @@ -507,6 +511,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem # if defined(CONFIG_EKF2_GNSS_YAW) hrt_abstime _status_gnss_yaw_pub_last {0}; uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; + uORB::Subscription _vehicle_gnss_heading_sub{ORB_ID(vehicle_gnss_heading)}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS diff --git a/src/modules/ekf2/test/sensor_simulator/gps.cpp b/src/modules/ekf2/test/sensor_simulator/gps.cpp index d75da46ba20a..d9bb49483e27 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.cpp +++ b/src/modules/ekf2/test/sensor_simulator/gps.cpp @@ -28,6 +28,11 @@ void Gps::send(const uint64_t time) } _ekf->setGpsData(_gps_data); + + if (PX4_ISFINITE(_gnss_yaw_data.yaw)) { + _gnss_yaw_data.time_us = _gps_data.time_us; + _ekf->setGnssYawData(_gnss_yaw_data); + } } void Gps::setData(const gnssSample &gps) @@ -57,12 +62,12 @@ void Gps::setVelocity(const Vector3f &vel) void Gps::setYaw(const float yaw) { - _gps_data.yaw = yaw; + _gnss_yaw_data.yaw = yaw; } void Gps::setYawOffset(const float yaw_offset) { - _gps_data.yaw_offset = yaw_offset; + _gnss_yaw_data.yaw_offset = yaw_offset; } void Gps::setFixType(const int fix_type) @@ -115,8 +120,6 @@ gnssSample Gps::getDefaultGpsData() gps_data.lat = 47.3566094; gps_data.lon = 8.5190237; gps_data.alt = 422.056f; - gps_data.yaw = NAN; - gps_data.yaw_offset = 0.0f; gps_data.fix_type = 3; gps_data.hacc = 0.5f; gps_data.vacc = 0.8f; diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index 1c144f391bb8..1ffd5cf1c9c2 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -74,6 +74,7 @@ class Gps: public Sensor static constexpr uint64_t kGpsDelayUs{110000}; gnssSample _gps_data{}; + gnssYawSample _gnss_yaw_data{}; Vector3f _gps_pos_rate{}; }; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index e3044c1ee71b..1b475b5bf11f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -142,6 +142,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_constraints", 1000); add_topic("vehicle_control_mode"); add_topic("vehicle_global_position", 200); + add_topic("vehicle_gnss_heading", 100); add_topic("vehicle_gps_position", 100); add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 306451984777..f14489643e55 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -45,6 +45,7 @@ VehicleGPSPosition::VehicleGPSPosition() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { _vehicle_gps_position_pub.advertise(); + _vehicle_gnss_heading_pub.advertise(); } VehicleGPSPosition::~VehicleGPSPosition() @@ -216,6 +217,61 @@ void VehicleGPSPosition::Run() } } + // Publish vehicle_gnss_heading from sensor_gnss_relative (preferred) or sensor_gps heading (fallback) + { + bool heading_published = false; + + // Preferred path: use sensor_gnss_relative which has independent timestamps + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + sensor_gnss_relative_s gnss_rel; + + if (_sensor_gnss_relative_sub[i].update(&gnss_rel)) { + _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; + + if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { + vehicle_gnss_heading_s heading_out{}; + heading_out.timestamp_sample = (gnss_rel.timestamp_sample > 0) ? gnss_rel.timestamp_sample : gnss_rel.timestamp; + heading_out.device_id = gnss_rel.device_id; + heading_out.heading = gnss_rel.heading; + heading_out.heading_accuracy = gnss_rel.heading_accuracy; + heading_out.heading_offset = NAN; // offset not available in sensor_gnss_relative + heading_out.timestamp = hrt_absolute_time(); + _vehicle_gnss_heading_pub.publish(heading_out); + heading_published = true; + } + } + } + + // Fallback: extract heading from sensor_gps for receivers that don't publish sensor_gnss_relative + // (e.g. Septentrio). Only use if no sensor_gnss_relative is being published. + if (!heading_published && any_gps_updated) { + bool any_gnss_relative_active = false; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_last_gnss_relative_timestamp[i] > 0 + && hrt_elapsed_time(&_last_gnss_relative_timestamp[i]) < 3_s) { + any_gnss_relative_active = true; + break; + } + } + + if (!any_gnss_relative_active) { + const sensor_gps_s &gps_out = _gps_blending.getOutputGpsData(); + + if (PX4_ISFINITE(gps_out.heading)) { + vehicle_gnss_heading_s heading_out{}; + heading_out.timestamp_sample = (gps_out.timestamp_sample > 0) ? gps_out.timestamp_sample : gps_out.timestamp; + heading_out.device_id = gps_out.device_id; + heading_out.heading = gps_out.heading; + heading_out.heading_accuracy = gps_out.heading_accuracy; + heading_out.heading_offset = gps_out.heading_offset; + heading_out.timestamp = hrt_absolute_time(); + _vehicle_gnss_heading_pub.publish(heading_out); + } + } + } + } + ScheduleDelayed(300_ms); // backup schedule perf_end(_cycle_perf); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index bf3cc6635f85..2f04dc35c538 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include #include #include "gps_blending.hpp" @@ -82,6 +84,7 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + uORB::Publication _vehicle_gnss_heading_pub{ORB_ID(vehicle_gnss_heading)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -92,6 +95,13 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; + uORB::Subscription _sensor_gnss_relative_sub[GPS_MAX_RECEIVERS] { + {ORB_ID(sensor_gnss_relative), 0}, + {ORB_ID(sensor_gnss_relative), 1}, + }; + + hrt_abstime _last_gnss_relative_timestamp[GPS_MAX_RECEIVERS] {}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; GpsBlending _gps_blending; From 5bdb5d0a61985792399b0dae7edd5476be8d3b4b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 16 Apr 2026 23:48:12 -0800 Subject: [PATCH 2/5] fix(sensors/vehicle_gps_position): apply per-receiver delay to GNSS heading timestamp Mirror the position path: match sensor_gnss_relative.device_id to a configured receiver slot and subtract SENS_GPS*_DELAY when the driver didn't distinguish timestamp_sample from timestamp. Without this, the heading fed into EKF2 used the publish time instead of the measurement time, negating a key motivation for decoupling heading from position. Signed-off-by: Jacob Dahl --- .../VehicleGPSPosition.cpp | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index f14489643e55..dcb6483ec540 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -229,8 +229,34 @@ void VehicleGPSPosition::Run() _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { + // Match device_id to receiver slot for per-receiver delay (mirrors position path) + hrt_abstime delay_us = 110_ms; // matches SENS_GPS*_DELAY default + bool matched = false; + + for (uint8_t slot = 0; slot < GPS_MAX_RECEIVERS; slot++) { + if (_gps_param_slots[slot].device_id != 0 + && _gps_param_slots[slot].device_id == gnss_rel.device_id) { + delay_us = _gps_param_slots[slot].delay_us; + matched = true; + break; + } + } + + if (!matched && _gps_param_slots[0].device_id == 0 && _gps_param_slots[1].device_id == 0) { + delay_us = _gps_param_slots[i].delay_us; + } + + uint64_t timestamp_sample = gnss_rel.timestamp_sample; + + // Apply delay if the driver didn't provide a measurement-time sample distinct from publish time + if (timestamp_sample == 0 || timestamp_sample == gnss_rel.timestamp) { + timestamp_sample = (delay_us > 0 && gnss_rel.timestamp > delay_us) + ? gnss_rel.timestamp - delay_us + : gnss_rel.timestamp; + } + vehicle_gnss_heading_s heading_out{}; - heading_out.timestamp_sample = (gnss_rel.timestamp_sample > 0) ? gnss_rel.timestamp_sample : gnss_rel.timestamp; + heading_out.timestamp_sample = timestamp_sample; heading_out.device_id = gnss_rel.device_id; heading_out.heading = gnss_rel.heading; heading_out.heading_accuracy = gnss_rel.heading_accuracy; From a5f37263fea2335124ce43e0efa5469f60ca4ffd Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 16 Apr 2026 23:48:15 -0800 Subject: [PATCH 3/5] style(ekf2): wrap GPS/GNSS-yaw ECL_WARN lines Match surrounding PX4 style for multi-argument log messages. Signed-off-by: Jacob Dahl --- src/modules/ekf2/EKF/estimator_interface.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index e620b5e7ad3e..d68d031d1894 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -196,7 +196,8 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) _time_last_gps_buffer_push = _time_latest_us; } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, + _min_obs_interval_us); } } @@ -229,7 +230,8 @@ void EstimatorInterface::setGnssYawData(const gnssYawSample &gnss_yaw_sample) _time_last_gnss_yaw_buffer_push = _time_latest_us; } else { - ECL_WARN("GNSS yaw data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gnss_yaw_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("GNSS yaw data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, + _gnss_yaw_buffer->get_newest().time_us, _min_obs_interval_us); } } #endif // CONFIG_EKF2_GNSS_YAW From 33fc15006fbf64f287ee684814db92268554e37f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 17 Apr 2026 00:45:20 -0800 Subject: [PATCH 4/5] fix(sensors/vehicle_gps_position): decouple heading fallback from blended position MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Two review-feedback fixes in the vehicle_gnss_heading publish path: - Fallback: read heading directly from raw sensor_gps subscriptions (primary instance first when SENS_GPS_PRIME is configured) instead of the blended output. The blended output's selection and weighting depend on position/velocity quality, which reintroduced the exact coupling this refactor is trying to remove. - Only mark a sensor_gnss_relative source "active" for the 3 s fallback gate when it actually publishes a valid heading. Previously any update — even heading_valid=false — would suppress the sensor_gps.heading fallback, potentially leaving the EKF without heading for 3 s when a receiver publishes sensor_gnss_relative without a heading solution (e.g., lost RTK fix). Also extract resolveReceiverDelay() / resolveSampleTimestamp() helpers so the preferred and fallback heading paths don't duplicate the delay/timestamp logic. Signed-off-by: Jacob Dahl --- .../VehicleGPSPosition.cpp | 103 ++++++++++++------ .../VehicleGPSPosition.hpp | 5 + 2 files changed, 73 insertions(+), 35 deletions(-) diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index dcb6483ec540..700d48db0bbd 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -226,34 +226,15 @@ void VehicleGPSPosition::Run() sensor_gnss_relative_s gnss_rel; if (_sensor_gnss_relative_sub[i].update(&gnss_rel)) { - _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; - if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { - // Match device_id to receiver slot for per-receiver delay (mirrors position path) - hrt_abstime delay_us = 110_ms; // matches SENS_GPS*_DELAY default - bool matched = false; - - for (uint8_t slot = 0; slot < GPS_MAX_RECEIVERS; slot++) { - if (_gps_param_slots[slot].device_id != 0 - && _gps_param_slots[slot].device_id == gnss_rel.device_id) { - delay_us = _gps_param_slots[slot].delay_us; - matched = true; - break; - } - } - - if (!matched && _gps_param_slots[0].device_id == 0 && _gps_param_slots[1].device_id == 0) { - delay_us = _gps_param_slots[i].delay_us; - } + // Only mark this source "active" for fallback gating when a valid heading is present — + // otherwise a receiver publishing sensor_gnss_relative without heading would suppress the + // sensor_gps.heading fallback from a different receiver. + _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; - uint64_t timestamp_sample = gnss_rel.timestamp_sample; - - // Apply delay if the driver didn't provide a measurement-time sample distinct from publish time - if (timestamp_sample == 0 || timestamp_sample == gnss_rel.timestamp) { - timestamp_sample = (delay_us > 0 && gnss_rel.timestamp > delay_us) - ? gnss_rel.timestamp - delay_us - : gnss_rel.timestamp; - } + const hrt_abstime delay_us = resolveReceiverDelay(gnss_rel.device_id, i); + const uint64_t timestamp_sample = resolveSampleTimestamp(gnss_rel.timestamp_sample, + gnss_rel.timestamp, delay_us); vehicle_gnss_heading_s heading_out{}; heading_out.timestamp_sample = timestamp_sample; @@ -268,8 +249,9 @@ void VehicleGPSPosition::Run() } } - // Fallback: extract heading from sensor_gps for receivers that don't publish sensor_gnss_relative - // (e.g. Septentrio). Only use if no sensor_gnss_relative is being published. + // Fallback: extract heading from raw sensor_gps for receivers that don't publish sensor_gnss_relative + // (e.g. Septentrio). Reads raw per-instance data — NOT the blended output — so heading selection is + // not coupled to position/velocity blending weights. if (!heading_published && any_gps_updated) { bool any_gnss_relative_active = false; @@ -282,17 +264,40 @@ void VehicleGPSPosition::Run() } if (!any_gnss_relative_active) { - const sensor_gps_s &gps_out = _gps_blending.getOutputGpsData(); + // Prefer primary instance when configured, otherwise first receiver with finite heading + const int32_t sens_gps_prime = _param_sens_gps_prime.get(); + uint8_t order[GPS_MAX_RECEIVERS] = {0, 1}; + + if (math::isInRange(sens_gps_prime, 0, static_cast(GPS_MAX_RECEIVERS - 1))) { + order[0] = static_cast(sens_gps_prime); + order[1] = static_cast(1 - sens_gps_prime); + } + + for (uint8_t idx = 0; idx < GPS_MAX_RECEIVERS; idx++) { + const uint8_t i = order[idx]; + sensor_gps_s gps_data; + + if (!_sensor_gps_sub[i].copy(&gps_data)) { + continue; + } + + if (!PX4_ISFINITE(gps_data.heading)) { + continue; + } + + const hrt_abstime delay_us = resolveReceiverDelay(gps_data.device_id, i); + const uint64_t timestamp_sample = resolveSampleTimestamp(gps_data.timestamp_sample, + gps_data.timestamp, delay_us); - if (PX4_ISFINITE(gps_out.heading)) { vehicle_gnss_heading_s heading_out{}; - heading_out.timestamp_sample = (gps_out.timestamp_sample > 0) ? gps_out.timestamp_sample : gps_out.timestamp; - heading_out.device_id = gps_out.device_id; - heading_out.heading = gps_out.heading; - heading_out.heading_accuracy = gps_out.heading_accuracy; - heading_out.heading_offset = gps_out.heading_offset; + heading_out.timestamp_sample = timestamp_sample; + heading_out.device_id = gps_data.device_id; + heading_out.heading = gps_data.heading; + heading_out.heading_accuracy = gps_data.heading_accuracy; + heading_out.heading_offset = gps_data.heading_offset; heading_out.timestamp = hrt_absolute_time(); _vehicle_gnss_heading_pub.publish(heading_out); + break; } } } @@ -303,6 +308,34 @@ void VehicleGPSPosition::Run() perf_end(_cycle_perf); } +hrt_abstime VehicleGPSPosition::resolveReceiverDelay(uint32_t device_id, uint8_t instance_index) const +{ + for (uint8_t slot = 0; slot < GPS_MAX_RECEIVERS; slot++) { + if (_gps_param_slots[slot].device_id != 0 && _gps_param_slots[slot].device_id == device_id) { + return _gps_param_slots[slot].delay_us; + } + } + + // No device IDs configured: match by instance index + if (_gps_param_slots[0].device_id == 0 && _gps_param_slots[1].device_id == 0 + && instance_index < GPS_MAX_RECEIVERS) { + return _gps_param_slots[instance_index].delay_us; + } + + return 110_ms; // matches SENS_GPS*_DELAY default +} + +uint64_t VehicleGPSPosition::resolveSampleTimestamp(uint64_t driver_timestamp_sample, uint64_t driver_timestamp, + hrt_abstime delay_us) +{ + // If the driver provided a distinct sample timestamp, trust it. Otherwise subtract the configured delay. + if (driver_timestamp_sample != 0 && driver_timestamp_sample != driver_timestamp) { + return driver_timestamp_sample; + } + + return (delay_us > 0 && driver_timestamp > delay_us) ? driver_timestamp - delay_us : driver_timestamp; +} + void VehicleGPSPosition::PrintStatus() { PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps()); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 2f04dc35c538..b12245a0ebbc 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -73,6 +73,11 @@ class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem void ParametersUpdate(bool force = false); + // Helpers shared between position path and heading publish paths + hrt_abstime resolveReceiverDelay(uint32_t device_id, uint8_t instance_index) const; + static uint64_t resolveSampleTimestamp(uint64_t driver_timestamp_sample, uint64_t driver_timestamp, + hrt_abstime delay_us); + // defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm static constexpr uint8_t BLEND_MASK_USE_SPD_ACC = 1; static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2; From 63dabc79b403368bd3afae9cc395bd92a8db422f Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Fri, 17 Apr 2026 00:45:26 -0800 Subject: [PATCH 5/5] fix(ekf2): default gnssYawSample yaw/yaw_acc to NAN MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A default-constructed gnssYawSample had yaw=0 (finite), so any consumer of an uninitialized sample — notably the EKF sensor-simulator — would fire yaw fusion with a bogus zero heading. Using NAN as the default matches the "no measurement" convention used throughout the EKF and the existing PX4_ISFINITE(yaw) gates in controlGnssYawFusion. Signed-off-by: Jacob Dahl --- src/modules/ekf2/EKF/common.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ae353853007a..19a1842540fc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -207,8 +207,8 @@ struct gnssSample { struct gnssYawSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float yaw{}; ///< yaw angle from dual antenna GNSS (rad, [-PI, PI]) - float yaw_acc{}; ///< 1-std yaw error (rad) + float yaw{NAN}; ///< yaw angle from dual antenna GNSS (rad, [-PI, PI]); NAN indicates no measurement + float yaw_acc{NAN}; ///< 1-std yaw error (rad); NAN indicates accuracy not provided float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET };