refactor(ekf2): separate GNSS heading from position into independent topic#27102
refactor(ekf2): separate GNSS heading from position into independent topic#27102dakejahl wants to merge 5 commits into
Conversation
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 1456 byte (0.07 %)]px4_fmu-v6x [Total VM Diff: 1488 byte (0.08 %)]Updated: 2026-04-17T07:54:16 |
dakejahl
force-pushed
the
gnss-heading-separation
branch
3 times, most recently
from
82d5b9b to
4a82d4f
Compare
…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 <dahl.jakejacob@gmail.com>
dakejahl
force-pushed
the
gnss-heading-separation
branch
from
4a82d4f to
ec556f1
Compare
…eading 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 <dahl.jakejacob@gmail.com>
Match surrounding PX4 style for multi-argument log messages. Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
There was a problem hiding this comment.
Pull request overview
Refactors EKF2 GNSS yaw handling by decoupling dual-antenna GNSS heading from GNSS position/velocity, introducing a dedicated vehicle_gnss_heading uORB topic and a separate EKF2 yaw sample/buffer path.
Changes:
- Adds new
VehicleGnssHeadinguORB message and logs it by default. - Updates
VehicleGPSPositionto publishvehicle_gnss_headingfromsensor_gnss_relative, with a fallback fromsensor_gps.heading. - Updates EKF2 to ingest GNSS yaw via a new
gnssYawSampleand independentTimestampedRingBuffer, and updates GNSS yaw control code to use the new sample type.
Reviewed changes
Copilot reviewed 15 out of 15 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp | Adds subscriptions/publication for sensor_gnss_relative → vehicle_gnss_heading. |
| src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp | Publishes vehicle_gnss_heading (preferred from sensor_gnss_relative, fallback from sensor_gps). |
| src/modules/logger/logged_topics.cpp | Adds vehicle_gnss_heading to default logged topics. |
| src/modules/ekf2/test/sensor_simulator/gps.h | Introduces gnssYawSample storage in the GPS test simulator. |
| src/modules/ekf2/test/sensor_simulator/gps.cpp | Sends yaw samples to EKF via setGnssYawData() and moves yaw setters to the new sample. |
| src/modules/ekf2/EKF2.hpp | Adds subscription and update hook for vehicle_gnss_heading. |
| src/modules/ekf2/EKF2.cpp | Adds UpdateGnssYawSample() and removes yaw fields from the GNSS position sample. |
| src/modules/ekf2/EKF/estimator_interface.h | Adds setGnssYawData() and new yaw buffer/delayed sample members. |
| src/modules/ekf2/EKF/estimator_interface.cpp | Implements independent GNSS yaw buffering with rate limiting. |
| src/modules/ekf2/EKF/ekf.h | Updates GNSS yaw control/update APIs to use gnssYawSample. |
| src/modules/ekf2/EKF/common.h | Removes yaw from gnssSample, introduces gnssYawSample. |
| src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp | Pops/fuses GNSS yaw independently of _gps_data_ready. |
| src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp | Updates yaw fusion logic to take gnssYawSample and adjusts starting conditions. |
| msg/VehicleGnssHeading.msg | New uORB message for validated GNSS heading with independent timestamp. |
| msg/CMakeLists.txt | Registers VehicleGnssHeading.msg for generation. |
| _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; | ||
|
|
||
| if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { |
There was a problem hiding this comment.
_last_gnss_relative_timestamp[i] is updated on every sensor_gnss_relative update, even when heading_valid is false. This makes the fallback path treat the topic as active for 3s and can suppress publishing any vehicle_gnss_heading even if a valid sensor_gps.heading exists. Consider only updating the "active" timestamp when a valid heading is present, or track a separate last_valid_heading_timestamp for the fallback gating logic.
| _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; | |
| if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { | |
| if (gnss_rel.heading_valid && PX4_ISFINITE(gnss_rel.heading)) { | |
| _last_gnss_relative_timestamp[i] = gnss_rel.timestamp; |
| 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); | ||
| } |
There was a problem hiding this comment.
The fallback publishes heading from _gps_blending.getOutputGpsData(), but blending weights are based on position/velocity quality and can therefore still drop or switch heading when position checks fail (the coupling this PR is trying to remove). For the fallback, consider selecting heading directly from the raw sensor_gps instance(s) (eg selected primary receiver / highest fix type / first finite heading) rather than the blended output.
| _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); | ||
| } |
There was a problem hiding this comment.
_gnss_yaw_data is value-initialized ({}), so yaw starts at 0 (finite) and send() will always call setGnssYawData() even when tests never configured yaw. Previously the default yaw was explicitly set to NAN to indicate "not provided". Initialize _gnss_yaw_data.yaw to NAN (and consider doing the same for yaw_acc if you want it treated as unset) so existing tests don’t unintentionally fuse a zero heading.
| #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)) { | ||
|
|
There was a problem hiding this comment.
UpdateGnssYawSample() takes ekf2_timestamps but doesn't use it, which is likely to trigger -Wunused-parameter warnings in PX4 builds. Either mark it unused (eg (void)ekf2_timestamps;) or add an appropriate relative timestamp field (if desired) to Ekf2Timestamps.msg and populate it here.
| const bool starting_conditions_passing = continuing_conditions_passing | ||
| && _gnss_checks.passed() | ||
| && !is_gnss_yaw_data_intermittent | ||
| && !_gps_intermittent; | ||
| && !is_gnss_yaw_data_intermittent; |
There was a problem hiding this comment.
GNSS yaw fusion is now popped independently from the position buffer, but starting_conditions_passing still requires _gnss_checks.passed(), which is driven by the position/velocity gnssSample quality gates (nsats/PDOP/EPH/…). This keeps the heading startup coupled to position quality and contradicts the stated decoupling goal. If the intent is truly independent yaw fusion, consider removing this dependency or replacing it with yaw-specific validity checks (eg based on heading_accuracy / a dedicated heading health check).
…nded position 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 <dahl.jakejacob@gmail.com>
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 <dahl.jakejacob@gmail.com>
dakejahl
left a comment
dakejahl
left a comment
There was a problem hiding this comment.
Most of this lands cleanly — separate msg + separate buffer + separate pop is the right shape, and the timestamp-resolution helpers are a good extraction.
Main correctness concern: the fallback path in VehicleGPSPosition can republish stale sensor_gps.heading from a receiver that has stopped updating (see inline on line 280). Rest are DRY / consistency / defensive items. One latent bug this quietly fixes is worth advertising in the PR description (see inline on EKF2.cpp:2693).
+1 on Copilot's two open items (_gnss_checks.passed() contradiction on gnss_yaw_control.cpp:70 and the unused ekf2_timestamps parameter on EKF2.cpp:2680) — won't re-litigate inline.
Needs to be tested against a dual-F9P moving-baseline scenario before merge.
Posted by Claude (Opus 4.7) on behalf of @dakejahl.
| const uint8_t i = order[idx]; | ||
| sensor_gps_s gps_data; | ||
|
|
||
| if (!_sensor_gps_sub[i].copy(&gps_data)) { |
There was a problem hiding this comment.
_sensor_gps_sub[i].copy() returns the last cached value regardless of freshness, so this path can republish a stale heading after the chosen receiver has stopped updating. Concretely: receiver 0 is sens_gps_prime, stops publishing mid-flight; receiver 1 continues updating, making any_gps_updated true; the loop picks receiver 0 first and re-emits its old heading every tick. Gate on hrt_elapsed_time(&gps_data.timestamp) < N_ms before publishing, or iterate only over instances that were .updated() this cycle.
| perf_end(_cycle_perf); | ||
| } | ||
|
|
||
| hrt_abstime VehicleGPSPosition::resolveReceiverDelay(uint32_t device_id, uint8_t instance_index) const |
There was a problem hiding this comment.
resolveReceiverDelay and resolveSampleTimestamp reimplement the inline slot-matching + delay-subtraction block in Run() (the for slot loop at ~147–164, plus the timestamp_sample fix at ~166–172). Refactor the existing position path to call these so there's one source of truth — today a tweak to either the delay-match rules or the timestamp-resolution rules has to be made in two places.
| } | ||
|
|
||
| const hrt_abstime delay_us = resolveReceiverDelay(gps_data.device_id, i); | ||
| const uint64_t timestamp_sample = resolveSampleTimestamp(gps_data.timestamp_sample, |
There was a problem hiding this comment.
Position's timestamp_sample gets run through _pps_time_sync.correct_gps_timestamp() on line 209. The heading path here uses only the per-receiver delay. For PPS-synced receivers, vehicle_gps_position and vehicle_gnss_heading will disagree by up to ~100 ms on the same physical sample, undermining the "independent, correct timestamps" story. Plumb PPS correction here too, or comment on why heading isn't PPS-aligned.
| 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<int32_t>(GPS_MAX_RECEIVERS - 1))) { |
There was a problem hiding this comment.
sens_gps_prime is valid in three ranges: -1 (auto), [0,1] (instance), [2,127] (UAVCAN node ID). This only handles [0,1]. For UAVCAN the primary resolution happens inside _gps_blending (see the setPrimaryInstance(i) call in Run() when the UAVCAN address matches). Reuse _gps_blending.getSelectedGps() here for consistency, or fall back to it when sens_gps_prime > 1.
|
|
||
| // Publish vehicle_gnss_heading from sensor_gnss_relative (preferred) or sensor_gps heading (fallback) | ||
| { | ||
| bool heading_published = false; |
There was a problem hiding this comment.
heading_published is redundant with any_gnss_relative_active below. Any iteration that sets it to true also sets _last_gnss_relative_timestamp[i] = gnss_rel.timestamp on line 233, which makes any_gnss_relative_active true. Drop the flag and the if (!heading_published && any_gps_updated) prefix and let the any_gnss_relative_active gate do the whole job.
| } | ||
|
|
||
| if (!PX4_ISFINITE(yaw_offset)) { | ||
| yaw_offset = 0.f; |
There was a problem hiding this comment.
This yaw_offset = 0.f fallback silently fixes a latent bug in the old path: with the u-blox GPS driver (which sets sensor_gps.heading_offset = NAN) and EKF2_GPS_YAW_OFF = 0, the old code propagated NAN into gnssSample.yaw_offset and then into fuseGnssYaw / compute_gnss_yaw_pred_innov_var_and_h, poisoning the innovation variance. Worth calling out in the PR description — it's more than a refactor for that config.
| sample_new.time_us = time_us; | ||
|
|
||
| _gnss_yaw_buffer->push(sample_new); | ||
| _time_last_gnss_yaw_buffer_push = _time_latest_us; |
There was a problem hiding this comment.
_time_last_gnss_yaw_buffer_push is updated unconditionally, including for samples with NAN yaw. Today the only publishers filter NAN upstream, so this is latent — but the whole point of the new buffer is that anything producing a gnssYawSample can push into it. Gate the timestamp update on PX4_ISFINITE(gnss_yaw_sample.yaw) so the isNewestSampleRecent → stopGnssYawFusion path in gps_control.cpp still fires if a future publisher pushes NANs.
|
|
||
| if (PX4_ISFINITE(_gnss_yaw_data.yaw)) { | ||
| _gnss_yaw_data.time_us = _gps_data.time_us; | ||
| _ekf->setGnssYawData(_gnss_yaw_data); |
There was a problem hiding this comment.
The test sensor still always pairs heading with a GPS position sample. The fusion decoupling is the whole point of the PR; worth a dedicated test that exercises _gps_data_ready == false / yaw-only and asserts fuseGnssYaw continues. Otherwise the behavior change is un-gated by CI.
Summary
Decouples GNSS heading (from dual-antenna / moving baseline RTK) from GNSS position throughout the pipeline. This resolves the
//TODO: move to different messageat EKF2.cpp and addresses the architectural issues discussed in #25516.Problem: Heading was bundled with position in
sensor_gps/gnssSamplewith a single shared timestamp, causing:_min_obs_interval_us_gps_intermittentfrom position blocks heading startupChanges:
VehicleGnssHeading.msgwith independent timestampVehicleGPSPositionsubscribes tosensor_gnss_relativeand publishesvehicle_gnss_heading(with fallback fromsensor_gps.headingfor receivers that don't publishsensor_gnss_relative, e.g. Septentrio)gnssYawSamplestruct and separateTimestampedRingBufferin EKF2gps_control.cpp— no longer gated by_gps_data_ready_gps_intermittentcoupling from heading starting conditionsgnss_yaw_control.cppto usegnssYawSampleyaw,yaw_acc,yaw_offsetfields fromgnssSampleData flow (before → after):
Follow-up work (not in this PR):
sensor_gnss_relativepublication (currently uses fallback path)GPS_YAW_OFFSET,SEP_YAW_OFFS,EKF2_GPS_YAW_OFF)Relates to #25516, #27088