fix(gps): split RTCM corrections and moving-baseline uORB topics (#27097)

* fix(gps): split RTCM corrections and moving-baseline uORB topics

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 PX4/PX4-Autopilot#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
                       (MAX_INSTANCES = 1).

The GPS driver routes its own RTCM output to the right topic via
GPSHelper::isMovingBase(), and gates the two inbound streams per role
using new GPSHelper virtuals (PX4-GPSDrivers#212):
shouldInjectRTCMCorrections() is true for any configured receiver, so a
UART2 moving-base rover still accepts fixed-base corrections over its
main link; shouldInjectMovingBaseline() is true only for a UART1/CAN
heading rover, since a UART2 rover gets the baseline directly in
hardware and a moving base produces rather than consumes it. That
submodule PR also renames the ambiguous UBXMode fields to name their
UART explicitly (RoverWithMovingBase -> RoverWithMovingBaseUART2,
MovingBase -> MovingBaseUART2).

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 topic gets its own stale-link switchover timer so
corrections failover is not suppressed by moving-baseline traffic, or
vice versa.

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. The
bus_type == UAVCAN check is kept, now purely as a loop guard so a node
with both CANNODE_PUB_MBD and CANNODE_SUB_MBD does not rebroadcast a
peer's moving-baseline data back onto the bus. CANnode RTCMStream
subscriber maps each CAN source node ID to its own rtcm_corrections
instance (one PublicationMulti per source, capped at MAX_INSTANCES) so
multiple CAN RTCM sources (e.g. dual rovers outputting MSM7 for logging
plus a fixed-base feed) land on independent uORB instances instead of
interleaving on one, which would otherwise defeat the consumer's
per-instance stale-link selection.

Depends on PX4-GPSDrivers#212 (submodule bump included).

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* fix(gps): use separate RTCM parsers for corrections and moving baseline

On a rover injecting both fixed-base corrections and moving-baseline RTCM, feeding both streams through a single parser allowed a fragmented frame from one source to be corrupted by bytes interleaved from the other. Reassemble each stream in its own Rtcm3Parser so frames are recovered independently.

Also collapse the two near-identical topics into a single RtcmData.msg published under both rtcm_corrections and rtcm_moving_baseline (the SensorGps pattern), track corrections and moving-baseline injection on separate perf counters so the reported corrections rate is no longer inflated by moving-baseline traffic, and zero-initialize the CAN DeviceId unions before populating their fields.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* fix(septentrio): avoid bugprone sizeof division on RTCM byte buffer

moving_baseline.data is a uint8_t array, so sizeof(data)/sizeof(data[0]) divides by 1; clang-tidy's bugprone-sizeof-expression flags this as a suspicious sizeof(T)/sizeof(T). Use sizeof(data) directly - the capacity value is unchanged.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* refactor(gps): use dedicated per-stream RTCM drain functions

rtcm_moving_baseline has a single publisher (instance 0), so its
consumers are now a plain uORB::Subscription instead of a 4-instance
SubscriptionMultiArray, and the per-stream selected-instance and
stale-link timer members it no longer needs are removed.

With each RTCM stream now a fixed type with a single caller, the
templated drain helpers (drain_rtcm_subscriptions, the overloaded
drain_rtcm_to_can) bought nothing, so replace them with dedicated
functions: drainRtcmCorrections()/drainMovingBaseline() in the GPS
driver and the UAVCAN bridge, drain_rtcm_corrections()/
drain_moving_baseline() in Septentrio. The UAVCAN bridge calls
PublishRTCMStream/PublishMovingBaselineData directly instead of through
Forward lambdas.

Rename SeptentrioDriver::publish_rtcm_corrections() to
publish_moving_baseline(): it is only reached from the Secondary
moving-base decode path and only ever emits moving-baseline RTCM.

Corrections-path behavior (instance selection, generation-gap warning,
burst cap, self-injection filter) is unchanged.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* refactor(gps): rename RTCM inject gate to receiverReady and simplify chunk helper

Rename GPSHelper::shouldInjectRTCMCorrections() to receiverReady(). The
virtual gates injection of both RTCM corrections and moving-baseline, and
for UBX it simply reports whether the receiver is configured, so the name
now describes what it actually gates rather than implying it only concerns
corrections. Bumps the GPS-drivers submodule to the matching rename.

Drop the vestigial message-type template parameter from publish_rtcm_chunks:
both topics share rtcm_data_s, so only the publication type needs templating.

* fix(septentrio): log dropped RTCM uORB generations

Match the gps driver and warn when the RTCM corrections or moving-baseline
subscription skips a uORB generation, so dropped injection data is visible.

* docs(docs): Docs only update to the RtcmData msg

* chore(gps): pin GPSDrivers to merged main

Contains #212 (RTCM/moving-baseline gating virtuals), #213 (X20 CFG-ODO
NAK tolerance), and #215 (SPARTN input enable, best-effort VALSET).

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* fix(septentrio): reassemble RTCM frames per stream before injecting

Both drains wrote raw uORB chunks to the receiver, so a fragmented frame
on one stream could get the other stream's bytes spliced in mid-frame and
corrupt both. Reassemble each stream in its own parser and only write
complete frames, mirroring the gps driver. Injection stats now count
frames instead of uORB chunks.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

* refactor(mavlink)!: remove GPS_RTCM_DATA output stream

GPS_RTCM_DATA is a GCS-to-vehicle correction transport; echoing
rtcm_corrections back out over MAVLink had no consumer and the echo was
lossy anyway (uint8 len and 180-byte payload truncate 300-byte uORB
chunks). Receiving GPS_RTCM_DATA is unchanged.

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>

---------

Signed-off-by: Jacob Dahl <dahl.jakejacob@gmail.com>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>

msg/CMakeLists.txt

@@ -117,7 +117,6 @@ set(msg_files
 	GpioOut.msg
 	GpioRequest.msg
 	GpsDump.msg
-	GpsInjectData.msg
 	Gripper.msg
 	HealthReport.msg
 	HeaterStatus.msg
@@ -194,6 +193,7 @@ set(msg_files
 	RoverSteeringSetpoint.msg
 	RoverThrottleSetpoint.msg
 	Rpm.msg
+	RtcmData.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg

msg/GpsInjectData.msg

@@ -1,11 +0,0 @@
-uint64 timestamp		# time since system start (microseconds)
-
-uint32 device_id                # unique device ID for the sensor that does not change between power cycles
-
-uint16 len                       # length of data
-uint8 flags                     # LSB: 1=fragmented across multiple uORB publications
-uint8[300] data                 # data chunk to write to GPS device (RTCM message)
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-uint8 MAX_INSTANCES = 2

msg/RtcmData.msg

@@ -0,0 +1,28 @@
+# RTCM3 data exchanged with GNSS receivers.
+#
+# Published under two topic names that share this definition (see TOPICS below):
+#
+# rtcm_corrections - external fixed-base corrections fed into the vehicle (MAVLink
+# GPS_RTCM_DATA, UAVCAN RTCMStream, GPS drivers in dump mode). Multiple
+# sources are allowed, one uORB instance each; consumers select an instance
+# via their stale-link logic.
+#
+# rtcm_moving_baseline - moving-base GPS output (RTCM 4072 or equivalent) intended for a rover.
+# Single publisher per vehicle (on-board moving base, or a CANnode
+# forwarding MovingBaselineData); consumers only read instance 0.
+
+uint64 timestamp # [us] Time since system start
+
+uint32 device_id # [-] Unique device ID of the publisher that produced this RTCM
+
+uint16 len # [-] Length of data
+uint8 flags # [-] LSB: 1=fragmented
+uint8[300] data # RTCM3 payload (fixed-base corrections, or moving-baseline data)
+
+uint8 ORB_QUEUE_LENGTH = 16
+
+# Sized for the fixed-base corrections case (up to four independent sources). The moving-baseline
+# topic only uses instance 0 (single publisher per vehicle).
+uint8 MAX_INSTANCES = 4
+
+# TOPICS rtcm_corrections rtcm_moving_baseline

src/drivers/gnss/septentrio/CMakeLists.txt

@@ -49,6 +49,7 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		failure_injection
+		gnss
 	)
 
 if(TARGET fuzztest::fuzztest)

src/drivers/gnss/septentrio/septentrio.cpp

@@ -56,7 +56,7 @@
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/time.h>
 #include <lib/systemlib/mavlink_log.h>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 #include <uORB/topics/sensor_gps.h>
 
 #include "util.h"
@@ -1438,7 +1438,7 @@ int SeptentrioDriver::process_message()
 	}
 	case DecodingStatus::RTCMv3: {
 		SEP_TRACE_PARSING("Processing RTCMv3 message");
-		publish_rtcm_corrections(_rtcm_decoder->message(), _rtcm_decoder->received_bytes());
+		publish_moving_baseline(_rtcm_decoder->message(), _rtcm_decoder->received_bytes());
 		break;
 	}
 	}
@@ -1626,62 +1626,105 @@ int SeptentrioDriver::set_baudrate(uint32_t baud)
 	}
 }
 
-void SeptentrioDriver::handle_inject_data_topic()
+void SeptentrioDriver::drain_rtcm_corrections()
 {
-	// We don't want to call copy again further down if we have already done a copy in the selection process.
+	// rtcm_corrections may have several sources (MAVLink plus CAN nodes), one uORB instance each.
+	rtcm_data_s msg;
 	bool already_copied = false;
-	gps_inject_data_s msg;
 
 	const hrt_abstime now = hrt_absolute_time();
 
 	// 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 < _rtcm_corrections_sub.size(); instance++) {
+			if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) {
+				if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) {
+					already_copied = true;
+					_selected_rtcm_instance = instance;
+					break;
 				}
 			}
 		}
 	}
 
 	bool updated = already_copied;
-
-	// Limit maximum number of GPS injections to 8 since usually
-	// GPS injections should consist of 1-4 packets (GPS, GLONASS, BeiDou, Galileo).
-	// Looking at 8 packets thus guarantees, that at least a full injection
-	// data set is evaluated.
-	// Moving Base requires a higher rate, so we allow up to 8 packets.
-	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
 	size_t num_injections = 0;
 
 	do {
 		if (updated) {
 			num_injections++;
 
-			// Prevent injection of data from self or from ground if moving base and this is rover.
-			if ((_instance == Instance::Secondary && msg.device_id != get_device_id()) || (_instance == Instance::Main && msg.device_id == get_device_id()) || _receiver_setup != ReceiverSetup::MovingBase) {
-				/* Write the message to the gps device. Note that the message could be fragmented.
-				* But as we don't write anywhere else to the device during operation, we don't
-				* need to assemble the message first.
-				*/
-				write(msg.data, msg.len);
-
-				++_current_interval_rtcm_injections;
+			// Prevent injection of data from self
+			if (msg.device_id != get_device_id()) {
+				_rtcm_corrections_parser.addData(msg.data, msg.len);
 				_last_rtcm_injection_time = hrt_absolute_time();
 			}
 		}
 
-		updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg);
+		auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance];
+		const unsigned last_generation = sub.get_last_generation();
 
-	} while (updated && num_injections < max_num_injections);
+		updated = sub.update(&msg);
+
+		if (updated && sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name,
+				 last_generation, sub.get_last_generation());
+		}
+	} while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH);
+}
+
+void SeptentrioDriver::drain_moving_baseline()
+{
+	// rtcm_moving_baseline has a single publisher (instance 0); no stale-link selection needed.
+	rtcm_data_s msg;
+	size_t num_injections = 0;
+
+	while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) {
+		const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation();
+
+		if (!_rtcm_moving_baseline_sub.update(&msg)) {
+			break;
+		}
+
+		num_injections++;
+
+		if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name,
+				 last_generation, _rtcm_moving_baseline_sub.get_last_generation());
+		}
+
+		// Prevent injection of data from self
+		if (msg.device_id != get_device_id()) {
+			_rtcm_moving_baseline_parser.addData(msg.data, msg.len);
+		}
+	}
+}
+
+void SeptentrioDriver::inject_rtcm_frames(gnss::Rtcm3Parser &parser)
+{
+	size_t frame_len = {};
+	const uint8_t *frame_ptr = {};
+
+	while ((frame_ptr = parser.getNextMessage(&frame_len)) != nullptr) {
+		write(frame_ptr, frame_len);
+		parser.consumeMessage(frame_len);
+		++_current_interval_rtcm_injections;
+	}
+}
+
+void SeptentrioDriver::handle_inject_data_topic()
+{
+	// Fixed-base RTCM corrections (from MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream).
+	// Both the moving-base (Secondary) and the rover (Main) can benefit from external RTCM.
+	drain_rtcm_corrections();
+	inject_rtcm_frames(_rtcm_corrections_parser);
+
+	// Moving-baseline RTCM is only consumed by the rover (Main instance) in a moving-base setup.
+	// Single publisher, so no instance selection - just drain it into the receiver.
+	if (_receiver_setup == ReceiverSetup::MovingBase && _instance == Instance::Main) {
+		drain_moving_baseline();
+		inject_rtcm_frames(_rtcm_moving_baseline_parser);
+	}
 }
 
 void SeptentrioDriver::publish()
@@ -1713,37 +1756,27 @@ bool SeptentrioDriver::first_gps_uorb_message_created() const
 	return _sensor_gps.timestamp != 0;
 }
 
-void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
+void SeptentrioDriver::publish_moving_baseline(uint8_t *data, size_t len)
 {
-	gps_inject_data_s gps_inject_data{};
+	// The only path into this function is the moving-base Secondary decoding RTCM from its
+	// receiver (see _rtcm_decoder allocation in the constructor), so the output is always
+	// moving-baseline data intended for the rover.
+	rtcm_data_s moving_baseline{};
 
-	gps_inject_data.timestamp = hrt_absolute_time();
-	gps_inject_data.device_id = get_device_id();
+	moving_baseline.timestamp = hrt_absolute_time();
+	moving_baseline.device_id = get_device_id();
 
-	size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0]));
-
-	if (len > capacity) {
-		gps_inject_data.flags = 1; //LSB: 1=fragmented
-
-	} else {
-		gps_inject_data.flags = 0;
-	}
+	const size_t capacity = sizeof(moving_baseline.data);
+	moving_baseline.flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented
 
 	size_t written = 0;
 
 	while (written < len) {
-
-		gps_inject_data.len = len - written;
-
-		if (gps_inject_data.len > capacity) {
-			gps_inject_data.len = capacity;
-		}
-
-		memcpy(gps_inject_data.data, &data[written], gps_inject_data.len);
-
-		_gps_inject_data_pub.publish(gps_inject_data);
-
-		written = written + gps_inject_data.len;
+		const size_t chunk = math::min(len - written, capacity);
+		moving_baseline.len = chunk;
+		memcpy(moving_baseline.data, &data[written], chunk);
+		_rtcm_moving_baseline_pub.publish(moving_baseline);
+		written += chunk;
 	}
 }
 

src/drivers/gnss/septentrio/septentrio.h

@@ -50,12 +50,14 @@
 #include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_status.h>
 #include <uORB/topics/gps_dump.h>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
+#include <lib/gnss/rtcm.h>
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/Device.hpp>
 #include <lib/parameters/param.h>
@@ -527,6 +529,25 @@ private:
 	 */
 	void handle_inject_data_topic();
 
+	/**
+	 * @brief Drain the multi-instance rtcm_corrections subscription into its RTCM parser,
+	 * selecting an active instance if the current one goes stale.
+	 */
+	void drain_rtcm_corrections();
+
+	/**
+	 * @brief Drain the single-publisher rtcm_moving_baseline subscription into its RTCM parser.
+	 */
+	void drain_moving_baseline();
+
+	/**
+	 * @brief Write all complete RTCM frames reassembled in a parser to the receiver.
+	 *
+	 * The two inject streams are written frame-atomically so chunks of a fragmented frame on
+	 * one stream can never interleave with the other stream's bytes mid-frame.
+	 */
+	void inject_rtcm_frames(gnss::Rtcm3Parser &parser);
+
 	/**
 	 * @brief Send data to the receiver, such as RTCM injections.
 	 *
@@ -555,12 +576,12 @@ private:
 	bool first_gps_uorb_message_created() const;
 
 	/**
-	 * @brief Publish RTCM corrections.
+	 * @brief Publish moving-baseline RTCM produced by the Secondary moving base for the rover.
 	 *
 	 * @param data: The raw data to publish
 	 * @param len: The size of `data`
 	 */
-	void publish_rtcm_corrections(uint8_t *data, size_t len);
+	void publish_moving_baseline(uint8_t *data, size_t len);
 
 	/**
 	 * @brief Dump gps communication.
@@ -724,8 +745,12 @@ private:
 	DumpMode                               _dump_communication_mode {DumpMode::Disabled};                ///< GPS communication dump mode
 	device::Serial                         _uart {};                                                     ///< Serial UART port for communication with the receiver
 	char                                   _port[20] {};                                                 ///< The path of the used serial device
-	hrt_abstime                            _last_rtcm_injection_time {0};                                ///< Time of last RTCM injection
+	hrt_abstime                            _last_rtcm_injection_time {0};                                ///< Time of last RTCM corrections injection
 	uint8_t                                _selected_rtcm_instance {0};                                  ///< uORB instance that is being used for RTCM corrections
+	// Separate parser per inject stream: frames are only written to the receiver once complete,
+	// so fixed-base corrections and moving-baseline bytes cannot interleave mid-frame.
+	gnss::Rtcm3Parser                      _rtcm_corrections_parser {};                                  ///< Frame reassembly for rtcm_corrections
+	gnss::Rtcm3Parser                      _rtcm_moving_baseline_parser {};                              ///< Frame reassembly for rtcm_moving_baseline
 	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
 	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	bool                                   _time_synced {false};                                         ///< Receiver time in sync with GPS time
@@ -765,9 +790,10 @@ private:
 	uORB::PublicationMulti<sensor_gps_s>           _sensor_gps_pub {ORB_ID(sensor_gps)};           		///< uORB publication for gps position
 	uORB::PublicationMulti<sensor_gnss_status_s>   _sensor_gnss_status_pub {ORB_ID(sensor_gnss_status)};	///< uORB publication for gnss status
 	uORB::Publication<gps_dump_s>                  _gps_dump_pub {ORB_ID(gps_dump)};              		///< uORB publication for dump GPS data
-	uORB::Publication<gps_inject_data_s>           _gps_inject_data_pub {ORB_ID(gps_inject_data)}; 		///< uORB publication for injected data to the receiver
+	uORB::Publication<rtcm_data_s>      _rtcm_moving_baseline_pub {ORB_ID(rtcm_moving_baseline)}; ///< uORB publication for moving-baseline RTCM output
 	uORB::PublicationMulti<satellite_info_s>       _satellite_info_pub {ORB_ID(satellite_info)};   		///< uORB publication for satellite info
-	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver
+	uORB::SubscriptionMultiArray<rtcm_data_s, rtcm_data_s::MAX_INSTANCES> _rtcm_corrections_sub {ORB_ID::rtcm_corrections}; ///< uORB subscription for external RTCM corrections
+	uORB::Subscription _rtcm_moving_baseline_sub {ORB_ID(rtcm_moving_baseline)}; ///< uORB subscription for moving-baseline RTCM input (single publisher)
 
 	failure_injection::Config _failure_config;
 	failure_injection::Stuck<sensor_gps_s> _stuck;

src/drivers/gps/gps.cpp

@@ -65,7 +65,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/gps_dump.h>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_relative.h>
 
@@ -248,26 +248,34 @@ private:
 	failure_injection::Stuck<sensor_gps_s> _stuck;
 
 	float				_rate{0.0f};					///< position update rate
-	float				_rtcm_injection_rate{0.0f};			///< RTCM message injection rate
-	unsigned			_rtcm_injection_rate_message_count{0};		///< counter for number of RTCM messages
+	float				_rtcm_injection_rate{0.0f};			///< RTCM corrections injection rate (Hz)
+	uint64_t			_last_rtcm_corrections_injection_count{0};	///< corrections-injection perf snapshot for rate calc
 	unsigned			_num_bytes_read{0}; 				///< counter for number of read bytes from the UART (within update interval)
 	unsigned			_rate_reading{0}; 				///< reading rate in B/s
-	hrt_abstime			_last_rtcm_injection_time{0};			///< time of last rtcm injection
+	hrt_abstime			_last_rtcm_injection_time{0};			///< time of last RTCM corrections injection
 	uint8_t				_selected_rtcm_instance{0};			///< uorb instance that is being used for RTCM corrections
 
 	const Instance 			_instance;
 
-	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
-	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::SubscriptionMultiArray<rtcm_data_s, rtcm_data_s::MAX_INSTANCES> _rtcm_corrections_sub{ORB_ID::rtcm_corrections};
+	uORB::Subscription _rtcm_moving_baseline_sub{ORB_ID(rtcm_moving_baseline)};
+	uORB::PublicationMulti<rtcm_data_s> _rtcm_corrections_pub{ORB_ID(rtcm_corrections)};
+	uORB::Publication<rtcm_data_s> _rtcm_moving_baseline_pub{ORB_ID(rtcm_moving_baseline)};
 	uORB::Publication<gps_dump_s>	     _dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			     *_dump_to_device{nullptr};
 	gps_dump_s			     *_dump_from_device{nullptr};
 	gps_dump_comm_mode_t                 _dump_communication_mode{gps_dump_comm_mode_t::Disabled};
 
-	gnss::Rtcm3Parser		     _rtcm_parser{};
+	// Separate RTCM parsers per source. A fragmented fixed-base frame must not be corrupted by
+	// moving-baseline bytes appended mid-frame (e.g. fixed-base + moving-base + rover setups, where
+	// the rover injects both streams), so each stream reassembles in its own buffer.
+	gnss::Rtcm3Parser		     _rtcm_corrections_parser{};
+	gnss::Rtcm3Parser		     _rtcm_moving_baseline_parser{};
 
 	perf_counter_t _uart_tx_buffer_full_perf{perf_alloc(PC_COUNT, MODULE_NAME": tx buf full")};
 	perf_counter_t _rtcm_buffer_full_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm buf full")};
+	perf_counter_t _rtcm_corrections_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm corrections injected")};
+	perf_counter_t _rtcm_moving_baseline_injection_perf{perf_alloc(PC_COUNT, MODULE_NAME": rtcm moving baseline injected")};
 
 	static px4::atomic_bool _is_gps_main_advertised; ///< for the second gps we want to make sure that it gets instance 1
 	/// and thus we wait until the first one publishes at least one message.
@@ -313,6 +321,23 @@ private:
 	 */
 	void handleInjectDataTopic();
 
+	/**
+	 * Drain the multi-instance rtcm_corrections subscription into its RTCM parser, selecting an
+	 * active instance if the current one goes stale.
+	 */
+	void drainRtcmCorrections();
+
+	/**
+	 * Drain the single-publisher rtcm_moving_baseline subscription into its RTCM parser.
+	 */
+	void drainMovingBaseline();
+
+	/**
+	 * Inject all complete RTCM frames reassembled in a parser into the receiver, counting each
+	 * injected frame on the given perf counter.
+	 */
+	void injectRtcmFrames(gnss::Rtcm3Parser &parser, perf_counter_t injection_perf);
+
 	/**
 	 * send data to the device, such as an RTCM stream
 	 * @param data
@@ -460,6 +485,8 @@ GPS::~GPS()
 
 	perf_free(_uart_tx_buffer_full_perf);
 	perf_free(_rtcm_buffer_full_perf);
+	perf_free(_rtcm_corrections_injection_perf);
+	perf_free(_rtcm_moving_baseline_injection_perf);
 
 	delete _sat_info;
 	delete _dump_to_device;
@@ -609,64 +636,42 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 	return ret;
 }
 
-void GPS::handleInjectDataTopic()
+void GPS::drainRtcmCorrections()
 {
-	if (!_helper->shouldInjectRTCM()) {
-		return;
-	}
-
-	// We don't want to call copy again further down if we have already done a
-	// copy in the selection process.
+	// rtcm_corrections may have several sources (MAVLink plus CAN nodes), one uORB instance each.
+	rtcm_data_s msg;
 	bool already_copied = false;
-	gps_inject_data_s msg;
 
 	const hrt_abstime now = hrt_absolute_time();
 
 	// 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 < _rtcm_corrections_sub.size(); instance++) {
+			if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) {
 				/* Don't select the own RTCM instance. In case it has a lower
 				 * instance number, it will be selected and will be rejected
 				 * later in the code, resulting in no RTCM injection at all.
 				 */
-				if (msg.device_id != get_device_id()) {
-					// Only use the message if it is up to date
-					if (now < msg.timestamp + 5_s) {
-						// Remember that we already did a copy on this instance.
-						already_copied = true;
-						_selected_rtcm_instance = instance;
-						break;
-					}
+				if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) {
+					already_copied = true;
+					_selected_rtcm_instance = instance;
+					break;
 				}
 			}
 		}
 	}
 
 	bool updated = already_copied;
-
-	// Limit maximum number of GPS injections to 8 since usually
-	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 8 packets thus guarantees, that at least a full injection
-	// data set is evaluated.
-	// Moving Base reuires a higher rate, so we allow up to 8 packets.
-	// Drain uORB messages into RTCM parser and inject full messages after draining the queue.
-	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
 	size_t num_injections = 0;
 
+	// Limit maximum number of injections per call so a burst can't starve the driver loop.
 	do {
 		if (updated) {
 			num_injections++;
 
 			// Prevent injection of data from self
 			if (msg.device_id != get_device_id()) {
-				// Add data to the RTCM parser buffer for frame reassembly
-				size_t added = _rtcm_parser.addData(msg.data, msg.len);
-
-				if (added < msg.len) {
+				if (_rtcm_corrections_parser.addData(msg.data, msg.len) < msg.len) {
 					perf_count(_rtcm_buffer_full_perf);
 				}
 
@@ -674,25 +679,81 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance];
+		auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance];
+		const unsigned last_generation = sub.get_last_generation();
 
-		const unsigned last_generation = gps_inject_data_sub.get_last_generation();
+		updated = sub.update(&msg);
 
-		updated = gps_inject_data_sub.update(&msg);
+		if (updated && sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name,
+				 last_generation, sub.get_last_generation());
+		}
+	} while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH);
+}
 
-		if (updated) {
-			if (gps_inject_data_sub.get_last_generation() != last_generation + 1) {
-				PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation());
-			}
+void GPS::drainMovingBaseline()
+{
+	// rtcm_moving_baseline has a single publisher (instance 0), so there is no stale-link instance
+	// selection - just drain the queue into the parser.
+	rtcm_data_s msg;
+	size_t num_injections = 0;
+
+	while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) {
+		const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation();
+
+		if (!_rtcm_moving_baseline_sub.update(&msg)) {
+			break;
 		}
 
-	} while (updated && num_injections < max_num_injections);
+		num_injections++;
 
-	// Now inject all complete RTCM frames from the parser buffer
+		if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name,
+				 last_generation, _rtcm_moving_baseline_sub.get_last_generation());
+		}
+
+		// Prevent injection of data from self
+		if (msg.device_id != get_device_id()) {
+			if (_rtcm_moving_baseline_parser.addData(msg.data, msg.len) < msg.len) {
+				perf_count(_rtcm_buffer_full_perf);
+			}
+		}
+	}
+}
+
+void GPS::handleInjectDataTopic()
+{
+	// receiverReady() is false until the receiver is configured, which keeps us from writing to the
+	// device mid-configuration.
+	if (!_helper->receiverReady()) {
+		return;
+	}
+
+	// Fixed-base RTCM corrections (MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream). Every configured
+	// receiver can use these - including a UART2 moving-base rover, whose baseline arrives in
+	// hardware but which still wants fixed-base corrections over its main link. Reassemble and inject
+	// them through their own parser before touching the moving-baseline stream.
+	drainRtcmCorrections();
+	injectRtcmFrames(_rtcm_corrections_parser, _rtcm_corrections_injection_perf);
+
+	// Moving-baseline RTCM (RTCM 4072 etc.) from a peer moving-base GPS. Only a heading rover that
+	// receives the baseline through the flight controller (UART1 / CAN) injects it here; a UART2
+	// rover gets it in hardware and a moving base produces it. Single publisher, so no instance
+	// selection - and a separate parser keeps the two byte streams from interleaving into corrupt
+	// frames.
+	if (_helper->shouldInjectMovingBaseline()) {
+		drainMovingBaseline();
+		injectRtcmFrames(_rtcm_moving_baseline_parser, _rtcm_moving_baseline_injection_perf);
+	}
+}
+
+void GPS::injectRtcmFrames(gnss::Rtcm3Parser &parser, perf_counter_t injection_perf)
+{
+	// Inject all complete RTCM frames reassembled in this parser.
 	size_t frame_len = {};
 	const uint8_t *frame_ptr = {};
 
-	while ((frame_ptr = _rtcm_parser.getNextMessage(&frame_len)) != nullptr) {
+	while ((frame_ptr = parser.getNextMessage(&frame_len)) != nullptr) {
 		// Check TX buffer space before writing
 		if (_interface == GPSHelper::Interface::UART) {
 			ssize_t tx_available = _uart.txSpaceAvailable();
@@ -705,8 +766,8 @@ void GPS::handleInjectDataTopic()
 		}
 
 		injectData(frame_ptr, frame_len);
-		_rtcm_parser.consumeMessage(frame_len);
-		_rtcm_injection_rate_message_count++;
+		parser.consumeMessage(frame_len);
+		perf_count(injection_perf);
 	}
 }
 
@@ -873,16 +934,16 @@ GPS::run()
 		switch (gps_ubx_mode) {
 		case 1:  // heading
 			if (_instance == Instance::Main) {
-				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase;
+				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART2;
 
 			} else {
-				ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+				ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART2;
 			}
 
 			break;
 
 		case 2:
-			ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART2;
 			break;
 
 		case 3:
@@ -900,7 +961,7 @@ GPS::run()
 			break;
 
 		case 5:  // rover with static base on Uart2
-			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
+			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUART2;
 			break;
 
 		case 6:
@@ -1190,7 +1251,7 @@ GPS::run()
 
 #if defined(CONFIG_GPS_UBX)
 
-			if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase)
+			if ((ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART2)
 			    || (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1)) {
 				/* The MB rover will wait as long as possible to compute a navigation solution,
 				 * possibly lowering the navigation rate all the way to 1 Hz while doing so. */
@@ -1228,11 +1289,14 @@ GPS::run()
 				if (now > last_rate_measurement + 5_s) {
 					float dt = (float)((now - last_rate_measurement)) / 1e6f;
 					_rate = last_rate_count / dt;
-					_rtcm_injection_rate = _rtcm_injection_rate_message_count / dt;
+					// Report the fixed-base corrections injection rate; moving-baseline injection is
+					// tracked separately on its own perf counter.
+					const uint64_t corrections_count = perf_event_count(_rtcm_corrections_injection_perf);
+					_rtcm_injection_rate = (corrections_count - _last_rtcm_corrections_injection_count) / dt;
+					_last_rtcm_corrections_injection_count = corrections_count;
 					_rate_reading = _num_bytes_read / dt;
 					last_rate_measurement = now;
 					last_rate_count = 0;
-					_rtcm_injection_rate_message_count = 0;
 					_num_bytes_read = 0;
 					_helper->storeUpdateRates();
 					_helper->resetUpdateRates();
@@ -1397,6 +1461,8 @@ GPS::print_status()
 
 	perf_print_counter(_uart_tx_buffer_full_perf);
 	perf_print_counter(_rtcm_buffer_full_perf);
+	perf_print_counter(_rtcm_corrections_injection_perf);
+	perf_print_counter(_rtcm_moving_baseline_injection_perf);
 
 	if (_instance == Instance::Main && _secondary_instance.load()) {
 		GPS *secondary_instance = _secondary_instance.load();
@@ -1477,38 +1543,45 @@ GPS::publishSatelliteInfo()
 	}
 }
 
-void
-GPS::publishRTCMCorrections(uint8_t *data, size_t len)
+// Chunk an RTCM byte stream into a uORB message and publish it. RTCM frames larger than the
+// message payload are split across consecutive publications (flags LSB = fragmented). Templated
+// on the publication so the same code serves both the corrections and moving-baseline topics.
+template <typename PubT>
+static void publish_rtcm_chunks(PubT &pub, const uint8_t *data, size_t len, hrt_abstime timestamp,
+				uint32_t device_id)
 {
-	gps_inject_data_s gps_inject_data{};
+	rtcm_data_s msg{};
+	msg.timestamp = timestamp;
+	msg.device_id = device_id;
 
-	gps_inject_data.timestamp = hrt_absolute_time();
-	gps_inject_data.device_id = get_device_id();
-
-	size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0]));
-
-	if (len > capacity) {
-		gps_inject_data.flags = 1; //LSB: 1=fragmented
-
-	} else {
-		gps_inject_data.flags = 0;
-	}
+	const size_t capacity = sizeof(msg.data);
+	msg.flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented
 
 	size_t written = 0;
 
 	while (written < len) {
+		const size_t chunk = math::min(len - written, capacity);
+		msg.len = chunk;
+		memcpy(msg.data, &data[written], chunk);
+		pub.publish(msg);
+		written += chunk;
+	}
+}
 
-		gps_inject_data.len = len - written;
+void
+GPS::publishRTCMCorrections(uint8_t *data, size_t len)
+{
+	const hrt_abstime timestamp = hrt_absolute_time();
+	const uint32_t device_id = get_device_id();
 
-		if (gps_inject_data.len > capacity) {
-			gps_inject_data.len = capacity;
-		}
+	// If this GPS is a moving base, its RTCM output is moving-baseline data for a rover (not
+	// external corrections). Route it to a dedicated topic so downstream consumers can tell it
+	// apart from fixed-base RTCM.
+	if (_helper && _helper->isMovingBase()) {
+		publish_rtcm_chunks(_rtcm_moving_baseline_pub, data, len, timestamp, device_id);
 
-		memcpy(gps_inject_data.data, &data[written], gps_inject_data.len);
-
-		_gps_inject_data_pub.publish(gps_inject_data);
-
-		written = written + gps_inject_data.len;
+	} else {
+		publish_rtcm_chunks(_rtcm_corrections_pub, data, len, timestamp, device_id);
 	}
 }
 

src/drivers/uavcan/sensors/gnss.cpp

@@ -625,16 +625,85 @@ void UavcanGnssBridge::update()
 	handleInjectDataTopic();
 }
 
-// Partially taken from src/drivers/gps/gps.cpp
-// This listens on the gps_inject_data uORB topic for RTCM data
-// sent from a GCS (usually over MAVLINK GPS_RTCM_DATA).
-// Forwarding this data to the UAVCAN bus enables DGPS/RTK GPS
-// to work.
+// Drains rtcm_corrections (fixed-base RTCM from a GCS over MAVLink, or CAN nodes) to
+// uavcan::RTCMStream. Several sources may be active (one uORB instance each); switch to a
+// different instance if the selected one goes stale.
+void UavcanGnssBridge::drainRtcmCorrections()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	bool already_copied = false;
+	rtcm_data_s msg;
+
+	if (now > _last_rtcm_injection_time + 5_s) {
+		for (int instance = 0; instance < _rtcm_corrections_sub.size(); instance++) {
+			if (_rtcm_corrections_sub[instance].advertised() && _rtcm_corrections_sub[instance].copy(&msg)) {
+				if (now < msg.timestamp + 5_s) {
+					already_copied = true;
+					_selected_rtcm_instance = instance;
+					break;
+				}
+			}
+		}
+	}
+
+	bool updated = already_copied;
+	size_t num_injections = 0;
+
+	do {
+		if (updated) {
+			num_injections++;
+			PublishRTCMStream(msg.data, msg.len);
+			_rtcm_injection_rate_message_count++;
+			_last_rtcm_injection_time = hrt_absolute_time();
+		}
+
+		auto &sub = _rtcm_corrections_sub[_selected_rtcm_instance];
+		const unsigned last_generation = sub.get_last_generation();
+
+		updated = sub.update(&msg);
+
+		if (updated && sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", sub.get_topic()->o_name,
+				 last_generation, sub.get_last_generation());
+		}
+	} while (updated && num_injections < rtcm_data_s::ORB_QUEUE_LENGTH);
+}
+
+// Drains rtcm_moving_baseline (moving-base RTCM 4072 from a peer GPS) to
+// ardupilot::MovingBaselineData. Single publisher (instance 0), so no stale-link selection.
+void UavcanGnssBridge::drainMovingBaseline()
+{
+	rtcm_data_s msg;
+	size_t num_injections = 0;
+
+	while (num_injections < rtcm_data_s::ORB_QUEUE_LENGTH) {
+		const unsigned last_generation = _rtcm_moving_baseline_sub.get_last_generation();
+
+		if (!_rtcm_moving_baseline_sub.update(&msg)) {
+			break;
+		}
+
+		num_injections++;
+
+		if (_rtcm_moving_baseline_sub.get_last_generation() != last_generation + 1) {
+			PX4_WARN("%s lost, generation %u -> %u", _rtcm_moving_baseline_sub.get_topic()->o_name,
+				 last_generation, _rtcm_moving_baseline_sub.get_last_generation());
+		}
+
+		PublishMovingBaselineData(msg.data, msg.len);
+	}
+}
+
+// The two RTCM streams use separate CAN publishers: fixed-base corrections go to
+// uavcan::RTCMStream and moving-baseline RTCM to ardupilot::MovingBaselineData, so corrections
+// are never mirrored onto the moving-baseline message.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
-	hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
 
-	// measure RTCM update rate every 5 seconds
+	// Measure the fixed-base corrections injection rate every 5 seconds (moving-baseline forwarding
+	// is tracked separately via _moving_baseline_data_pub_perf, so it is excluded here).
 	if (now > _last_rate_measurement + 5_s) {
 		float dt = (now - _last_rate_measurement) / 1e6f;
 		_rtcm_injection_rate = _rtcm_injection_rate_message_count / dt;
@@ -642,72 +711,13 @@ void UavcanGnssBridge::handleInjectDataTopic()
 		_rtcm_injection_rate_message_count = 0;
 	}
 
-	// We don't want to call copy again further down if we have already done a
-	// copy in the selection process.
-	bool already_copied = false;
-	gps_inject_data_s msg;
-
-	// 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) {
-				if (_orb_inject_data_sub[instance].copy(&msg)) {
-					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
-						// Remember that we already did a copy on this instance.
-						already_copied = true;
-						_selected_rtcm_instance = instance;
-						break;
-					}
-				}
-			}
-		}
+	if (_publish_rtcm_stream) {
+		drainRtcmCorrections();
 	}
 
-	bool updated = already_copied;
-
-	// Limit maximum number of GPS injections to 8 since usually
-	// GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo).
-	// Looking at 8 packets thus guarantees, that at least a full injection
-	// data set is evaluated.
-	// Moving Base requires a higher rate, so we allow up to 8 packets.
-	const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH;
-	size_t num_injections = 0;
-
-	do {
-		if (updated) {
-			num_injections++;
-
-			// Write the message to the gps device. Note that the message could be fragmented.
-			// But as we don't write anywhere else to the device during operation, we don't
-			// need to assemble the message first.
-			if (_publish_rtcm_stream) {
-				PublishRTCMStream(msg.data, msg.len);
-			}
-
-			if (_publish_moving_baseline_data) {
-				PublishMovingBaselineData(msg.data, msg.len);
-			}
-
-			++_rtcm_injection_rate_message_count;
-			_last_rtcm_injection_time = hrt_absolute_time();
-		}
-
-		auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance];
-
-		const unsigned last_generation = gps_inject_data_sub.get_last_generation();
-
-		updated = gps_inject_data_sub.update(&msg);
-
-		if (updated) {
-			if (gps_inject_data_sub.get_last_generation() != last_generation + 1) {
-				PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, gps_inject_data_sub.get_last_generation());
-			}
-		}
-
-	} while (updated && num_injections < max_num_injections);
+	if (_publish_moving_baseline_data) {
+		drainMovingBaseline();
+	}
 }
 
 bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)

src/drivers/uavcan/sensors/gnss.hpp

@@ -48,7 +48,7 @@
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gps.h>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 #include <uORB/topics/gps_dump.h>
 
 #include <lib/failure_injection/FailureInjection.hpp>
@@ -103,6 +103,8 @@ private:
 			  const uint8_t spoofing_state);
 
 	void handleInjectDataTopic();
+	void drainRtcmCorrections();
+	void drainMovingBaseline();
 	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
 	bool PublishMovingBaselineData(const uint8_t *data, size_t data_len);
 
@@ -147,7 +149,8 @@ private:
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};
 
-	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
+	uORB::SubscriptionMultiArray<rtcm_data_s, rtcm_data_s::MAX_INSTANCES> _rtcm_corrections_sub{ORB_ID::rtcm_corrections};
+	uORB::Subscription _rtcm_moving_baseline_sub{ORB_ID(rtcm_moving_baseline)};
 	hrt_abstime		_last_rtcm_injection_time{0};	///< time of last rtcm injection
 	uint8_t			_selected_rtcm_instance{0};	///< uorb instance that is being used for RTCM corrections
 
@@ -180,6 +183,6 @@ private:
 	perf_counter_t _moving_baseline_data_sub_perf{nullptr};
 
 	hrt_abstime _last_rate_measurement{0};
-	float _rtcm_injection_rate{0.f}; ///< RTCM message injection rate
-	unsigned _rtcm_injection_rate_message_count{0}; ///< number of RTCM messages since last rate calculation
+	float _rtcm_injection_rate{0.f}; ///< fixed-base corrections injection rate (Hz); moving-baseline tracked via _moving_baseline_data_pub_perf
+	unsigned _rtcm_injection_rate_message_count{0}; ///< fixed-base corrections messages since last rate calculation
 };

src/drivers/uavcannode/Publishers/MovingBaselineData.hpp

@@ -39,7 +39,7 @@
 
 #include <lib/drivers/device/Device.hpp>
 #include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 
 namespace uavcannode
 {
@@ -52,7 +52,7 @@ class MovingBaselineDataPub :
 public:
 	MovingBaselineDataPub(px4::WorkItem *work_item, uavcan::INode &node) :
 		UavcanPublisherBase(ardupilot::gnss::MovingBaselineData::DefaultDataTypeID),
-		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(gps_inject_data)),
+		uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(rtcm_moving_baseline)),
 		uavcan::Publisher<ardupilot::gnss::MovingBaselineData>(node)
 	{
 		this->setPriority(uavcan::TransferPriority::NumericallyMax);
@@ -70,26 +70,28 @@ public:
 
 	void BroadcastAnyUpdates() override
 	{
-		// gps_inject_data -> ardupilot::gnss::MovingBaselineData
-		gps_inject_data_s gps_inject_data = {};
+		// rtcm_moving_baseline -> ardupilot::gnss::MovingBaselineData
+		rtcm_data_s moving_baseline = {};
 
 		unsigned last_generation = uORB::SubscriptionCallbackWorkItem::get_last_generation();
 		bool updated = false;
 
 		// Drain all available messages from the queue and publish to CAN.
-		while ((updated = uORB::SubscriptionCallbackWorkItem::update(&gps_inject_data))) {
+		while ((updated = uORB::SubscriptionCallbackWorkItem::update(&moving_baseline))) {
 
 			unsigned current_generation = uORB::SubscriptionCallbackWorkItem::get_last_generation();
 
 			if (current_generation != last_generation + 1) {
-				PX4_WARN("gps_inject_data lost, generation %u -> %u", last_generation, current_generation);
+				PX4_WARN("rtcm_moving_baseline lost, generation %u -> %u", last_generation, current_generation);
 			}
 
 			last_generation = current_generation;
 
-			// Prevent republishing rtcm data we received from uavcan
+			// Don't rebroadcast moving-baseline RTCM that we received over CAN. Without this, a node
+			// configured with both CANNODE_PUB_MBD and CANNODE_SUB_MBD would echo peer broadcasts back
+			// onto the bus, creating a CAN rebroadcast loop.
 			union device::Device::DeviceId device_id;
-			device_id.devid = gps_inject_data.device_id;
+			device_id.devid = moving_baseline.device_id;
 
 			if (device_id.devid_s.bus_type == device::Device::DeviceBusType::DeviceBusType_UAVCAN) {
 				continue;
@@ -101,15 +103,15 @@ public:
 			size_t written = 0;
 			int result = 0;
 
-			while ((result >= 0) && written < gps_inject_data.len) {
-				size_t chunk_size = gps_inject_data.len - written;
+			while ((result >= 0) && written < moving_baseline.len) {
+				size_t chunk_size = moving_baseline.len - written;
 
 				if (chunk_size > capacity) {
 					chunk_size = capacity;
 				}
 
 				for (size_t i = 0; i < chunk_size; i++) {
-					mbd.data.push_back(gps_inject_data.data[written]);
+					mbd.data.push_back(moving_baseline.data[written]);
 					written += 1;
 				}
 

src/drivers/uavcannode/Subscribers/MovingBaselineData.hpp

@@ -39,7 +39,7 @@
 
 #include <lib/drivers/device/Device.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 
 namespace uavcannode
 {
@@ -75,7 +75,7 @@ public:
 		printf("\t%s:%d -> %s\n",
 		       ardupilot::gnss::MovingBaselineData::getDataTypeFullName(),
 		       ardupilot::gnss::MovingBaselineData::DefaultDataTypeID,
-		       _gps_inject_data_pub.get_topic()->o_name);
+		       _rtcm_moving_baseline_pub.get_topic()->o_name);
 	}
 
 private:
@@ -83,26 +83,26 @@ private:
 	{
 		// Don't republish a message from ourselves
 		if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) {
-			gps_inject_data_s gps_inject_data{};
+			rtcm_data_s moving_baseline{};
 
-			gps_inject_data.len = msg.data.size();
+			moving_baseline.len = msg.data.size();
 
-			memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
+			memcpy(moving_baseline.data, &msg.data[0], moving_baseline.len);
 
-			gps_inject_data.timestamp = hrt_absolute_time();
+			moving_baseline.timestamp = hrt_absolute_time();
 
-			union device::Device::DeviceId device_id;
+			union device::Device::DeviceId device_id {};
 
 			device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
 			device_id.devid_s.address = msg.getSrcNodeID().get();
 			device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN;
 
-			gps_inject_data.device_id = device_id.devid;
+			moving_baseline.device_id = device_id.devid;
 
-			_gps_inject_data_pub.publish(gps_inject_data);
+			_rtcm_moving_baseline_pub.publish(moving_baseline);
 		}
 	}
 
-	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::Publication<rtcm_data_s> _rtcm_moving_baseline_pub{ORB_ID(rtcm_moving_baseline)};
 };
 } // namespace uavcannode

src/drivers/uavcannode/Subscribers/RTCMStream.hpp

@@ -38,8 +38,8 @@
 #include <uavcan/equipment/gnss/RTCMStream.hpp>
 
 #include <lib/drivers/device/Device.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/rtcm_data.h>
 
 namespace uavcannode
 {
@@ -75,34 +75,77 @@ public:
 		printf("\t%s:%d -> %s\n",
 		       uavcan::equipment::gnss::RTCMStream::getDataTypeFullName(),
 		       uavcan::equipment::gnss::RTCMStream::DefaultDataTypeID,
-		       _gps_inject_data_pub.get_topic()->o_name);
+		       _sources[0].pub.get_topic()->o_name);
 	}
 
 private:
 	void callback(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::RTCMStream> &msg)
 	{
+		const uint8_t source_node_id = msg.getSrcNodeID().get();
+
 		// Don't republish a message from ourselves
-		if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) {
-			gps_inject_data_s gps_inject_data{};
-
-			gps_inject_data.len = msg.data.size();
-
-			memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
-
-			gps_inject_data.timestamp = hrt_absolute_time();
-
-			union device::Device::DeviceId device_id;
-
-			device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
-			device_id.devid_s.address = msg.getSrcNodeID().get();
-			device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN;
-
-			gps_inject_data.device_id = device_id.devid;
-
-			_gps_inject_data_pub.publish(gps_inject_data);
+		if (source_node_id == getNode().getNodeID().get()) {
+			return;
 		}
+
+		// Route each source node to its own rtcm_corrections instance. Independent CAN
+		// sources (e.g. a fixed base plus another rover's MSM output) must not interleave
+		// on a single instance, or the consumer's per-instance stale-link selection breaks.
+		uORB::PublicationMulti<rtcm_data_s> *pub = publicationForNode(source_node_id);
+
+		if (pub == nullptr) {
+			// More distinct sources than rtcm_corrections instances: drop.
+			return;
+		}
+
+		rtcm_data_s corrections{};
+
+		corrections.len = msg.data.size();
+
+		memcpy(corrections.data, &msg.data[0], corrections.len);
+
+		corrections.timestamp = hrt_absolute_time();
+
+		union device::Device::DeviceId device_id {};
+
+		device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_UAVCAN;
+		device_id.devid_s.address = source_node_id;
+		device_id.devid_s.devtype = DRV_GPS_DEVTYPE_UAVCAN;
+
+		corrections.device_id = device_id.devid;
+
+		pub->publish(corrections);
 	}
 
-	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	// Map a CAN source node ID to a stable rtcm_corrections publication, one uORB
+	// instance per source node. Returns nullptr once every instance is claimed.
+	uORB::PublicationMulti<rtcm_data_s> *publicationForNode(uint8_t node_id)
+	{
+		for (auto &source : _sources) {
+			if (source.assigned && source.node_id == node_id) {
+				return &source.pub;
+			}
+		}
+
+		for (auto &source : _sources) {
+			if (!source.assigned) {
+				source.assigned = true;
+				source.node_id = node_id;
+				return &source.pub;
+			}
+		}
+
+		return nullptr;
+	}
+
+	struct SourcePublication {
+		uORB::PublicationMulti<rtcm_data_s> pub{ORB_ID(rtcm_corrections)};
+		uint8_t node_id{0};
+		bool assigned{false};
+	};
+
+	// One publication per source node, capped at the topic's instance count so we never
+	// advertise an instance the consumer's SubscriptionMultiArray cannot read.
+	SourcePublication _sources[rtcm_data_s::MAX_INSTANCES] {};
 };
 } // namespace uavcannode

src/modules/mavlink/mavlink_messages.cpp

@@ -85,7 +85,6 @@
 #endif
 #include "streams/GPS_GLOBAL_ORIGIN.hpp"
 #include "streams/GPS_RAW_INT.hpp"
-#include "streams/GPS_RTCM_DATA.hpp"
 #include "streams/GPS_STATUS.hpp"
 #include "streams/HEARTBEAT.hpp"
 #include "streams/HIGHRES_IMU.hpp"
@@ -512,9 +511,6 @@ static const StreamListItem streams_list[] = {
 #if defined(EFI_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamEfiStatus>(),
 #endif // EFI_STATUS_HPP
-#if defined(GPS_RTCM_DATA_HPP)
-	create_stream_list_item<MavlinkStreamGPSRTCMData>(),
-#endif // GPS_RTCM_DATA_HPP
 #if defined(UAVIONIX_ADSB_OUT_CFG_HPP)
 	create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
 #endif // UAVIONIX_ADSB_OUT_CFG_HPP

src/modules/mavlink/mavlink_receiver.cpp

@@ -86,7 +86,7 @@ MavlinkReceiver::~MavlinkReceiver()
 #endif // !CONSTRAINED_FLASH
 
 	_distance_sensor_pub.unadvertise();
-	_gps_inject_data_pub.unadvertise();
+	_rtcm_corrections_pub.unadvertise();
 	_rc_pub.unadvertise();
 	_manual_control_input_pub.unadvertise();
 	_ping_pub.unadvertise();
@@ -2909,38 +2909,38 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 				 packet_len, now, message_len);
 
 	if (message != nullptr) {
-		publish_gps_inject_data(message, message_len);
+		publish_rtcm_corrections(message, message_len);
 
 		// addPacket() can queue at most one deferred message.
 		const uint8_t *deferred_message = _gps_rtcm_message_assembler.takeDeferredMessage(message_len);
 
 		if (deferred_message != nullptr) {
-			publish_gps_inject_data(deferred_message, message_len);
+			publish_rtcm_corrections(deferred_message, message_len);
 		}
 	}
 }
 
 void
-MavlinkReceiver::publish_gps_inject_data(const uint8_t *data, size_t len)
+MavlinkReceiver::publish_rtcm_corrections(const uint8_t *data, size_t len)
 {
-	gps_inject_data_s gps_inject_data_topic{};
-	constexpr uint8_t gps_inject_data_flag_fragmented = 1;
+	rtcm_data_s rtcm_corrections_topic{};
+	constexpr uint8_t rtcm_corrections_flag_fragmented = 1;
 
-	const size_t capacity = sizeof(gps_inject_data_topic.data);
-	// gps_inject_data only carries the transport-level fragmented bit. The
+	const size_t capacity = sizeof(rtcm_corrections_topic.data);
+	// rtcm_corrections only carries the transport-level fragmented bit. The
 	// MAVLink fragment/sequence bits are consumed by the assembler above.
-	gps_inject_data_topic.flags = (len > capacity) ? gps_inject_data_flag_fragmented : 0;
+	rtcm_corrections_topic.flags = (len > capacity) ? rtcm_corrections_flag_fragmented : 0;
 
 	size_t written = 0;
 
-	// gps_inject_data transports RTCM in 300-byte uORB chunks, so a fully
+	// rtcm_corrections transports RTCM in 300-byte uORB chunks, so a fully
 	// reassembled RTCM frame may still require multiple publications.
 	while (written < len) {
 		const size_t chunk_len = math::min(len - written, capacity);
-		gps_inject_data_topic.timestamp = hrt_absolute_time();
-		gps_inject_data_topic.len = static_cast<decltype(gps_inject_data_topic.len)>(chunk_len);
-		memcpy(gps_inject_data_topic.data, &data[written], chunk_len);
-		_gps_inject_data_pub.publish(gps_inject_data_topic);
+		rtcm_corrections_topic.timestamp = hrt_absolute_time();
+		rtcm_corrections_topic.len = static_cast<decltype(rtcm_corrections_topic.len)>(chunk_len);
+		memcpy(rtcm_corrections_topic.data, &data[written], chunk_len);
+		_rtcm_corrections_pub.publish(rtcm_corrections_topic);
 		written += chunk_len;
 	}
 }

src/modules/mavlink/mavlink_receiver.h

@@ -80,7 +80,7 @@
 #include <uORB/topics/gimbal_manager_set_manual_control.h>
 #include <uORB/topics/gimbal_device_information.h>
 #include <uORB/topics/gimbal_device_attitude_status.h>
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_data.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/irlock_report.h>
@@ -282,7 +282,7 @@ private:
 	void update_rx_stats(const mavlink_message_t &message);
 
 	void publish_hil_battery();
-	void publish_gps_inject_data(const uint8_t *data, size_t len);
+	void publish_rtcm_corrections(const uint8_t *data, size_t len);
 
 	px4::atomic_bool 	_should_exit{false};
 	pthread_t		_thread {};
@@ -396,7 +396,7 @@ private:
 	// ORB publications (multi)
 	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::PublicationMulti<aux_global_position_s>		_aux_global_position_pub{ORB_ID(aux_global_position)};
-	uORB::PublicationMulti<gps_inject_data_s>		_gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::PublicationMulti<rtcm_data_s>		_rtcm_corrections_pub{ORB_ID(rtcm_corrections)};
 	uORB::PublicationMulti<input_rc_s>			_rc_pub{ORB_ID(input_rc)};
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_control_input_pub{ORB_ID(manual_control_input)};
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping)};

src/modules/mavlink/streams/GPS_RTCM_DATA.hpp

@@ -1,81 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#ifndef GPS_RTCM_DATA_HPP
-#define GPS_RTCM_DATA_HPP
-
-#include <uORB/topics/gps_inject_data.h>
-
-class MavlinkStreamGPSRTCMData : public MavlinkStream
-{
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRTCMData(mavlink); }
-
-	static constexpr const char *get_name_static() { return "GPS_RTCM_DATA"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_RTCM_DATA; }
-
-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return _gps_inject_data_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
-	}
-
-private:
-	explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data), 0};
-
-	bool send() override
-	{
-		gps_inject_data_s gps_inject_data;
-		bool sent = false;
-
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
-			mavlink_gps_rtcm_data_t msg{};
-
-			msg.len = gps_inject_data.len;
-			msg.flags = gps_inject_data.flags;
-			memcpy(msg.data, gps_inject_data.data, sizeof(msg.data));
-
-			mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
-
-			sent = true;
-		}
-
-		return sent;
-	}
-};
-
-#endif // GPS_RTCM_DATA_HPP