fix(gps): split RTCM corrections and moving-baseline uORB topics#27097
fix(gps): split RTCM corrections and moving-baseline uORB topics#27097
Conversation
The single gps_inject_data topic served two unrelated purposes: external fixed-base RTCM corrections (from MAVLink GPS_RTCM_DATA or UAVCAN RTCMStream) and moving-base-to-rover RTCM 4072. In a dual-GPS-with-moving-base plus fixed-base setup, the two streams collided on the same queue and the FMU UAVCAN bridge mirrored fixed-base RTCM onto the MovingBaselineData CAN message, breaking rover heading or RTK fix (see #27088). Split by role: - rtcm_corrections (renamed from gps_inject_data): external RTCM flowing into the vehicle; producers are MAVLink, UAVCAN RTCMStream, and GPS drivers in dump mode. - rtcm_moving_baseline (new): moving-base GPS output intended for a rover; single producer per vehicle. GPS driver routes publishRTCMCorrections() based on a new GPSHelper::isMovingBase() virtual (overridden in UBX). Septentrio's publish_rtcm_corrections() always publishes to rtcm_moving_baseline (only the Secondary moving base calls it). Rover-side consumers (gps, septentrio, uavcan bridge) drain both topics independently; each gets its own stale-link switchover state. FMU UAVCAN bridge: two independent drain loops, one per topic. No more dual-publish of a single uORB message onto both RTCMStream and MovingBaselineData CAN streams. CANnode MovingBaselineDataPub subscribes to rtcm_moving_baseline and drops the bus_type == UAVCAN heuristic that was papering over the conflation. CANnode RTCMStream subscriber switches to PublicationMulti so multiple CAN RTCM sources coexist (e.g. dual rovers outputting MSM7 for logging plus a fixed-base feed). Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
![github-actions[bot]](https://avatars.githubusercontent.com/in/15368?s=60&v=4){kind=small}
| } else { | ||
| gps_inject_data.flags = 0; | ||
| } | ||
| const size_t capacity = sizeof(moving_baseline.data) / sizeof(moving_baseline.data[0]); |
There was a problem hiding this comment.
[error] bugprone-sizeof-expression [error]
suspicious usage of sizeof pointer sizeof(T)/sizeof(T)
| #define GPS_RTCM_DATA_HPP | ||
|
|
||
| #include <uORB/topics/gps_inject_data.h> | ||
| #include <uORB/topics/rtcm_corrections.h> |
There was a problem hiding this comment.
[error] clang-diagnostic-error [error]
uORB/topics/rtcm_corrections.h file not found
| } else { | ||
| gps_inject_data.flags = 0; | ||
| } | ||
| const size_t capacity = sizeof(corrections.data) / sizeof(corrections.data[0]); |
There was a problem hiding this comment.
[error] bugprone-sizeof-expression [error]
suspicious usage of sizeof pointer sizeof(T)/sizeof(T)
🔎 FLASH Analysispx4_fmu-v5x [Total VM Diff: 976 byte (0.05 %)]px4_fmu-v6x [Total VM Diff: 1288 byte (0.07 %)]Updated: 2026-04-16T01:33:32 |
There was a problem hiding this comment.
Pull request overview
This PR resolves RTCM stream collisions by splitting the former gps_inject_data uORB topic into two role-specific topics: external RTCM corrections vs moving-baseline RTCM intended for a rover, preventing cross-pollination in dual-GPS + fixed-base configurations.
Changes:
- Replace
gps_inject_datawithrtcm_corrections(multi-instance) and introducertcm_moving_baseline(moving-base→rover). - Update MAVLink, GPS drivers, and UAVCAN FMU/CANnode bridges to publish/consume the new topics independently.
- Add new uORB message definitions and update build/Kconfig topic selections accordingly.
Reviewed changes
Copilot reviewed 17 out of 17 changed files in this pull request and generated 5 comments.
Show a summary per file
| File | Description |
|---|---|
| src/modules/zenoh/Kconfig.topics | Replaces Zenoh selection for gps_inject_data with the new RTCM topics. |
| src/modules/mavlink/streams/GPS_RTCM_DATA.hpp | MAVLink stream now sources RTCM from rtcm_corrections. |
| src/modules/mavlink/mavlink_receiver.h | Receiver publishes incoming GPS_RTCM_DATA into rtcm_corrections. |
| src/modules/mavlink/mavlink_receiver.cpp | Implements the GPS_RTCM_DATA → rtcm_corrections publication path. |
| src/drivers/uavcannode/Subscribers/RTCMStream.hpp | CANnode RTCMStream subscriber republishes into rtcm_corrections using multi-publication. |
| src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp | CANnode MovingBaselineData subscriber republishes into rtcm_moving_baseline. |
| src/drivers/uavcannode/Publishers/MovingBaselineData.hpp | CANnode publishes rtcm_moving_baseline out as CAN MovingBaselineData. |
| src/drivers/uavcan/sensors/gnss.hpp | FMU UAVCAN GNSS bridge subscribes to both new RTCM topics with independent state. |
| src/drivers/uavcan/sensors/gnss.cpp | Splits forwarding to distinct CAN messages and deduplicates drain logic via a helper. |
| src/drivers/gps/gps.cpp | GPS driver now drains both RTCM topics and routes published RTCM based on moving-base role. |
| src/drivers/gnss/septentrio/septentrio.h | Septentrio driver wiring updated to new RTCM topics and new drain helper. |
| src/drivers/gnss/septentrio/septentrio.cpp | Implements separate draining paths and publishes moving-baseline RTCM to the new topic. |
| msg/RtcmMovingBaseline.msg | Introduces rtcm_moving_baseline uORB message definition. |
| msg/RtcmCorrections.msg | Introduces rtcm_corrections uORB message definition. |
| msg/GpsInjectData.msg | Removes the conflated legacy message definition. |
| msg/CMakeLists.txt | Registers the new message files and removes the old one from the build. |
| rtcm_corrections_topic.timestamp = hrt_absolute_time(); | ||
|
|
There was a problem hiding this comment.
rtcm_corrections_topic.timestamp is assigned twice; the first assignment is redundant since it is overwritten just before publish. Consider keeping only a single timestamp assignment (ideally once, close to publish).
| rtcm_corrections_topic.timestamp = hrt_absolute_time(); |
|
|
||
| uint8 ORB_QUEUE_LENGTH = 16 | ||
|
|
||
| uint8 MAX_INSTANCES = 2 |
There was a problem hiding this comment.
MAX_INSTANCES is set to 2, and multiple consumers now implement multi-instance selection logic, but all publishers in this PR use single-instance uORB::Publication for rtcm_moving_baseline. If the topic is intended to be single-source, consider setting MAX_INSTANCES = 1 (and possibly using non-multi subscriptions); if multiple sources are expected, consider switching publishers to PublicationMulti so instances don’t collide on instance 0.
| uint8 MAX_INSTANCES = 2 | |
| uint8 MAX_INSTANCES = 1 |
| // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link | ||
| if (now > _last_rtcm_injection_time + 5_s) { | ||
|
|
||
| for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { | ||
| const bool exists = _orb_inject_data_sub[instance].advertised(); | ||
|
|
||
| if (exists && _orb_inject_data_sub[instance].copy(&msg)) { | ||
| for (int instance = 0; instance < subs.size(); instance++) { |
There was a problem hiding this comment.
drainRtcmSubscriptions() uses the shared member _last_rtcm_injection_time to decide when a subscription has gone stale. Now that handleInjectDataTopic() drains both rtcm_corrections and rtcm_moving_baseline, activity on one stream will keep _last_rtcm_injection_time fresh and can prevent stale-link switchover (and even injection) on the other stream. Track last-injection time per stream (e.g. separate members for corrections vs moving-baseline, or pass hrt_abstime& last_injection_time into drainRtcmSubscriptions()).
| // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link | ||
| if (now > _last_rtcm_injection_time + 5_s) { | ||
| for (int instance = 0; instance < _gps_inject_data_sub.size(); instance++) { | ||
| const bool exists = _gps_inject_data_sub[instance].advertised(); | ||
|
|
||
| if (exists) { | ||
| if (_gps_inject_data_sub[instance].copy(&msg)) { | ||
| if (now < msg.timestamp + 5_s) { | ||
| // Remember that we already did a copy on this instance. | ||
| already_copied = true; | ||
| _selected_rtcm_instance = instance; | ||
| break; | ||
| } | ||
| for (int instance = 0; instance < subs.size(); instance++) { |
There was a problem hiding this comment.
drain_rtcm_subscriptions() uses the shared _last_rtcm_injection_time for stale-link switchover. Since handle_inject_data_topic() now drains both rtcm_corrections and rtcm_moving_baseline, injection on one stream can mask staleness on the other and prevent instance re-selection. Consider keeping separate last-injection timestamps per stream (or pass a last_injection_time reference into drain_rtcm_subscriptions()).
| msg.len = corrections.len; | ||
| msg.flags = corrections.flags; | ||
| memcpy(msg.data, corrections.data, sizeof(msg.data)); |
There was a problem hiding this comment.
The MAVLink message is zero-initialized, but this copies sizeof(msg.data) bytes regardless of msg.len. If the uORB publisher doesn't clear bytes beyond len, this can leak stale bytes over MAVLink. Copy only msg.len bytes (and optionally leave the remainder zero) rather than always copying the full array.
| msg.len = corrections.len; | |
| msg.flags = corrections.flags; | |
| memcpy(msg.data, corrections.data, sizeof(msg.data)); | |
| const size_t payload_len = (corrections.len < sizeof(msg.data)) ? corrections.len : sizeof(msg.data); | |
| msg.len = payload_len; | |
| msg.flags = corrections.flags; | |
| memcpy(msg.data, corrections.data, payload_len); |
There was a problem hiding this comment.
A few non-blocking observations.
Inline: on MovingBaselineDataPub dropping the bus_type == UAVCAN filter, and on RtcmCorrections queue length staying at 8 while MAX_INSTANCES doubled.
Diagnostic parity — not anchorable to a changed line, so noting here: gps.cpp still sets only _sensor_gps.selected_rtcm_instance; consider adding _sensor_gps.selected_moving_baseline_instance so flight logs can tell which moving-baseline instance the rover is actually pulling from when debugging dual-CAN moving-base setups. Related: _rtcm_injection_rate now aggregates both streams (UavcanGnssBridge passes the same _rtcm_injection_rate_message_count into both drains — gnss.cpp:637 and :643). Separate rates would be more useful for diagnosing which side is flaky, but that's sensor_gps schema churn — reasonable as a follow-up rather than this PR.
Docs: docs/en/msg_docs/GpsInjectData.md should be deleted and RtcmCorrections.md / RtcmMovingBaseline.md regenerated (auto-gen, not hand-written).
Posted by Claude (Opus 4.7) on behalf of @dakejahl.
| // gps_inject_data -> ardupilot::gnss::MovingBaselineData | ||
| gps_inject_data_s gps_inject_data = {}; | ||
| // rtcm_moving_baseline -> ardupilot::gnss::MovingBaselineData | ||
| rtcm_moving_baseline_s moving_baseline = {}; |
There was a problem hiding this comment.
The bus_type == UAVCAN filter that used to sit a few lines below this was doing double duty: gps_inject_data role disambiguation (now solved by splitting the topic) and defensive loop-break — a CANnode that both subscribes and publishes MovingBaselineData wouldn't re-broadcast what it just received from the bus. The PR solves the first purpose but drops the second. If a future deployment ever has two CANnodes each running a moving-base publisher, they'd ping-pong each other's traffic over CAN indefinitely (neither self-filter — that's source-node-ID — catches a relayed message).
Suggest either keeping the one-line filter, or leaving a comment stating "only one CANnode may run MovingBaselineDataPub per bus" so the invariant is visible to future readers.
| uint8 flags # LSB: 1=fragmented | ||
| uint8[300] data # RTCM3 correction data (from a fixed base, or another GPS) | ||
|
|
||
| uint8 ORB_QUEUE_LENGTH = 8 |
There was a problem hiding this comment.
MAX_INSTANCES doubled from 2 → 4 (good — covers MAVLink + multiple CAN sources), but ORB_QUEUE_LENGTH stays at 8. With four active publishers each writing to their own instance, per-instance depth of 8 is tight when bursts arrive (RTCM often comes in 4-message clusters covering GPS/GLONASS/BeiDou/Galileo). Consider 16 to match RtcmMovingBaseline.msg, or a comment explaining why 8 is sufficient here. Nit.
| @@ -0,0 +1,11 @@ | |||
| uint64 timestamp # time since system start (microseconds) | |||
There was a problem hiding this comment.
Document all uorb topics to standard https://docs.px4.io/main/en/uorb/uorb_documentation
Summary
Fixes #27088.
gps_inject_datawas being used for two unrelated purposes — external fixed-base RTCM corrections (MAVLinkGPS_RTCM_DATA, UAVCANRTCMStream) and moving-base-to-rover RTCM 4072. In a dual-GPS (moving base + rover) plus fixed-base setup, the two streams collided on the same 8-deep queue, and the FMU UAVCAN bridge mirrored fixed-base RTCM onto theMovingBaselineDataCAN message, breaking rover heading or RTK fix depending on timing.Split by role:
rtcm_corrections(renamed fromgps_inject_data,MAX_INSTANCES = 4): external RTCM into the vehicle. Producers: MAVLink receiver, UAVCANRTCMStreamsubscriber, GPS drivers in dump mode.rtcm_moving_baseline(new, single-publisher): moving-base GPS output intended for a rover. Producers: on-FMU moving-base GPS or CANnodeMovingBaselineDatasubscriber.Key changes
GPSHelper::isMovingBase()virtual added in PX4/PX4-GPSDrivers#206 (submodule bump included here).GPS::publishRTCMCorrections()uses it to pick the destination topic.publish_rtcm_corrections()always publishes tortcm_moving_baseline(only the Secondary moving base decodes RTCM).gps,septentrio, FMUUavcanGnssBridge) drain both topics independently, each with its own stale-link switchover state. The FMU bridge no longer dual-publishes a single message onto both CAN streams.MovingBaselineDataPubsubscribes tortcm_moving_baseline, drops thebus_type == UAVCANworkaround.RTCMStreamsubscriber →PublicationMultiso multiple CAN RTCM sources coexist (e.g. dual rovers outputting MSM7 for logging plus a fixed-base feed).GPS_RTCM_DATAstream stays sourced fromrtcm_correctionsonly — no more upstream echo of moving-base RTCM 4072 to the GCS.Kconfig.topicsupdated.Dependencies
isMovingBase(). This PR bumps the submodule; needs the submodule PR merged before this can merge.Builds
px4_fmu-v6x_defaultark_can-rtk-gps_default