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.

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>

msg/CMakeLists.txt

@@ -116,7 +116,6 @@ set(msg_files
 	GpioOut.msg
 	GpioRequest.msg
 	GpsDump.msg
-	GpsInjectData.msg
 	Gripper.msg
 	HealthReport.msg
 	HeaterStatus.msg
@@ -192,6 +191,8 @@ set(msg_files
 	RoverSteeringSetpoint.msg
 	RoverThrottleSetpoint.msg
 	Rpm.msg
+	RtcmCorrections.msg
+	RtcmMovingBaseline.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
-uint8[300] data                 # data to write to GPS device (RTCM message)
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-uint8 MAX_INSTANCES = 2

msg/RtcmCorrections.msg

@@ -0,0 +1,11 @@
+uint64 timestamp		# time since system start (microseconds)
+
+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 correction data (from a fixed base, or another GPS)
+
+uint8 ORB_QUEUE_LENGTH = 8
+
+uint8 MAX_INSTANCES = 4

msg/RtcmMovingBaseline.msg

@@ -0,0 +1,11 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint32 device_id                # unique device ID of the moving-base GPS that produced this RTCM
+
+uint16 len                      # length of data
+uint8 flags                     # LSB: 1=fragmented
+uint8[300] data                 # RTCM 4072 (or equivalent) moving-baseline data for the rover GPS
+
+uint8 ORB_QUEUE_LENGTH = 16
+
+uint8 MAX_INSTANCES = 2

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_corrections.h>
 #include <uORB/topics/sensor_gps.h>
 
 #include "util.h"
@@ -1621,27 +1621,22 @@ int SeptentrioDriver::set_baudrate(uint32_t baud)
 	}
 }
 
-void SeptentrioDriver::handle_inject_data_topic()
+template <typename T, uint8_t N>
+void SeptentrioDriver::drain_rtcm_subscriptions(uORB::SubscriptionMultiArray<T, N> &subs, uint8_t &selected_instance)
 {
-	// 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;
+	T 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 < subs.size(); instance++) {
+			if (subs[instance].advertised() && subs[instance].copy(&msg)) {
+				if (msg.device_id != get_device_id() && now < msg.timestamp + 5_s) {
+					already_copied = true;
+					selected_instance = instance;
+					break;
 				}
 			}
 		}
@@ -1649,36 +1644,37 @@ void SeptentrioDriver::handle_inject_data_topic()
 
 	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;
+	const size_t max_num_injections = T::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.
-				*/
+			// Prevent injection of data from self
+			if (msg.device_id != get_device_id()) {
 				write(msg.data, msg.len);
-
 				++_current_interval_rtcm_injections;
 				_last_rtcm_injection_time = hrt_absolute_time();
 			}
 		}
 
-		updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg);
-
+		updated = subs[selected_instance].update(&msg);
 	} while (updated && num_injections < max_num_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_subscriptions(_rtcm_corrections_sub, _selected_rtcm_instance);
+
+	// Moving-baseline RTCM is only consumed by the rover (Main instance) in a moving-base setup.
+	if (_receiver_setup == ReceiverSetup::MovingBase && _instance == Instance::Main) {
+		drain_rtcm_subscriptions(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance);
+	}
+}
+
 void SeptentrioDriver::publish()
 {
 	_sensor_gps.device_id = get_device_id();
@@ -1702,35 +1698,25 @@ bool SeptentrioDriver::first_gps_uorb_message_created() const
 
 void SeptentrioDriver::publish_rtcm_corrections(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_moving_baseline_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) / sizeof(moving_baseline.data[0]);
+	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

@@ -55,7 +55,8 @@
 #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_corrections.h>
+#include <uORB/topics/rtcm_moving_baseline.h>
 #include <drivers/drv_hrt.h>
 #include <lib/drivers/device/Device.hpp>
 #include <lib/parameters/param.h>
@@ -525,6 +526,12 @@ private:
 	 */
 	void handle_inject_data_topic();
 
+	/**
+	 * @brief Drain an RTCM source (rtcm_corrections or rtcm_moving_baseline) and write to the receiver.
+	 */
+	template <typename T, uint8_t N>
+	void drain_rtcm_subscriptions(uORB::SubscriptionMultiArray<T, N> &subs, uint8_t &selected_instance);
+
 	/**
 	 * @brief Send data to the receiver, such as RTCM injections.
 	 *
@@ -716,6 +723,7 @@ private:
 	char                                   _port[20] {};                                                 ///< The path of the used serial device
 	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
+	uint8_t                                _selected_moving_baseline_instance {0};                       ///< uORB instance used for moving-baseline RTCM input
 	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
@@ -755,9 +763,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_moving_baseline_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_corrections_s, rtcm_corrections_s::MAX_INSTANCES> _rtcm_corrections_sub {ORB_ID::rtcm_corrections}; ///< uORB subscription for external RTCM corrections
+	uORB::SubscriptionMultiArray<rtcm_moving_baseline_s, rtcm_moving_baseline_s::MAX_INSTANCES> _rtcm_moving_baseline_sub {ORB_ID::rtcm_moving_baseline}; ///< uORB subscription for moving-baseline RTCM input
 
 	// Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate...
 	hrt_abstime _current_interval_start_time {0};      ///< Start time of the current update measurement interval in us

src/drivers/gps/gps.cpp

@@ -64,7 +64,8 @@
 #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_corrections.h>
+#include <uORB/topics/rtcm_moving_baseline.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_relative.h>
 
@@ -215,11 +216,14 @@ private:
 	unsigned			_rate_reading{0}; 				///< reading rate in B/s
 	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
+	uint8_t				_selected_moving_baseline_instance{0};		///< uorb instance used for moving-baseline RTCM input
 
 	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_corrections_s, rtcm_corrections_s::MAX_INSTANCES> _rtcm_corrections_sub{ORB_ID::rtcm_corrections};
+	uORB::SubscriptionMultiArray<rtcm_moving_baseline_s, rtcm_moving_baseline_s::MAX_INSTANCES> _rtcm_moving_baseline_sub{ORB_ID::rtcm_moving_baseline};
+	uORB::PublicationMulti<rtcm_corrections_s> _rtcm_corrections_pub{ORB_ID(rtcm_corrections)};
+	uORB::Publication<rtcm_moving_baseline_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};
@@ -274,6 +278,13 @@ private:
 	 */
 	void handleInjectDataTopic();
 
+	/**
+	 * Drain an RTCM source (rtcm_corrections or rtcm_moving_baseline) into the RTCM parser.
+	 * Selects an active instance if the current one goes stale.
+	 */
+	template <typename T, uint8_t N>
+	void drainRtcmSubscriptions(uORB::SubscriptionMultiArray<T, N> &subs, uint8_t &selected_instance);
+
 	/**
 	 * send data to the device, such as an RTCM stream
 	 * @param data
@@ -554,38 +565,26 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 	return ret;
 }
 
-void GPS::handleInjectDataTopic()
+template <typename T, uint8_t N>
+void GPS::drainRtcmSubscriptions(uORB::SubscriptionMultiArray<T, N> &subs, uint8_t &selected_instance)
 {
-	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.
 	bool already_copied = false;
-	gps_inject_data_s msg;
+	T 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 < subs.size(); instance++) {
+			if (subs[instance].advertised() && subs[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_instance = instance;
+					break;
 				}
 			}
 		}
@@ -593,13 +592,8 @@ void GPS::handleInjectDataTopic()
 
 	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;
+	// Limit maximum number of injections per call so a burst can't starve the driver loop.
+	const size_t max_num_injections = T::ORB_QUEUE_LENGTH;
 	size_t num_injections = 0;
 
 	do {
@@ -608,7 +602,6 @@ void GPS::handleInjectDataTopic()
 
 			// 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) {
@@ -619,19 +612,29 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		auto &gps_inject_data_sub = _orb_inject_data_sub[_selected_rtcm_instance];
+		auto &sub = subs[selected_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) {
-			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());
-			}
+		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 < max_num_injections);
+}
+
+void GPS::handleInjectDataTopic()
+{
+	if (!_helper->shouldInjectRTCM()) {
+		return;
+	}
+
+	// Fixed-base RTCM corrections (MAVLink GPS_RTCM_DATA, UAVCAN RTCMStream).
+	drainRtcmSubscriptions(_rtcm_corrections_sub, _selected_rtcm_instance);
+
+	// Moving-baseline RTCM from a peer GPS (UBX RTCM 4072 etc).
+	drainRtcmSubscriptions(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance);
 
 	// Now inject all complete RTCM frames from the parser buffer
 	size_t frame_len = {};
@@ -1382,35 +1385,45 @@ GPS::publishSatelliteInfo()
 void
 GPS::publishRTCMCorrections(uint8_t *data, size_t len)
 {
-	gps_inject_data_s gps_inject_data{};
+	// 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.
+	const bool moving_base = _helper && _helper->isMovingBase();
 
-	gps_inject_data.timestamp = hrt_absolute_time();
-	gps_inject_data.device_id = get_device_id();
+	rtcm_corrections_s corrections{};
+	rtcm_moving_baseline_s moving_baseline{};
 
-	size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0]));
+	const hrt_abstime now = hrt_absolute_time();
+	const uint32_t device_id = get_device_id();
 
-	if (len > capacity) {
-		gps_inject_data.flags = 1; //LSB: 1=fragmented
+	corrections.timestamp = now;
+	corrections.device_id = device_id;
+	moving_baseline.timestamp = now;
+	moving_baseline.device_id = device_id;
 
-	} else {
-		gps_inject_data.flags = 0;
-	}
+	const size_t capacity = sizeof(corrections.data) / sizeof(corrections.data[0]);
+	const uint8_t flags = (len > capacity) ? 1 : 0; // LSB: 1=fragmented
+	corrections.flags = flags;
+	moving_baseline.flags = flags;
 
 	size_t written = 0;
 
 	while (written < len) {
 
-		gps_inject_data.len = len - written;
+		const size_t chunk = math::min(len - written, capacity);
 
-		if (gps_inject_data.len > capacity) {
-			gps_inject_data.len = capacity;
+		if (moving_base) {
+			moving_baseline.len = chunk;
+			memcpy(moving_baseline.data, &data[written], chunk);
+			_rtcm_moving_baseline_pub.publish(moving_baseline);
+
+		} else {
+			corrections.len = chunk;
+			memcpy(corrections.data, &data[written], chunk);
+			_rtcm_corrections_pub.publish(corrections);
 		}
 
-		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;
+		written += chunk;
 	}
 }
 

src/drivers/uavcan/sensors/gnss.cpp

@@ -564,14 +564,65 @@ 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 a uORB RTCM source and forwards each message to `forward(data, len)`.
+// Handles stale-link switchover across instances via `last_injection_time`.
+template <typename T, uint8_t N, typename Forward>
+static void drain_rtcm_to_can(uORB::SubscriptionMultiArray<T, N> &subs,
+			      uint8_t &selected_instance,
+			      hrt_abstime &last_injection_time,
+			      unsigned &rate_message_count,
+			      Forward forward)
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	bool already_copied = false;
+	T msg;
+
+	if (now > last_injection_time + 5_s) {
+		for (int instance = 0; instance < subs.size(); instance++) {
+			if (subs[instance].advertised() && subs[instance].copy(&msg)) {
+				if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
+					already_copied = true;
+					selected_instance = instance;
+					break;
+				}
+			}
+		}
+	}
+
+	bool updated = already_copied;
+
+	const size_t max_num_injections = T::ORB_QUEUE_LENGTH;
+	size_t num_injections = 0;
+
+	do {
+		if (updated) {
+			num_injections++;
+			forward(msg.data, msg.len);
+			++rate_message_count;
+			last_injection_time = hrt_absolute_time();
+		}
+
+		auto &sub = subs[selected_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 < max_num_injections);
+}
+
+// This drains rtcm_corrections (fixed-base RTCM from GCS/CAN) to uavcan::RTCMStream,
+// and rtcm_moving_baseline (moving-base RTCM 4072 from a peer GPS) to
+// ardupilot::MovingBaselineData. Each topic maps to its own CAN publisher — the
+// two streams are kept separate so fixed-base corrections are not mirrored onto
+// the moving-baseline CAN message.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
-	hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
 
 	// measure RTCM update rate every 5 seconds
 	if (now > _last_rate_measurement + 5_s) {
@@ -581,72 +632,17 @@ 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) {
+		drain_rtcm_to_can(_rtcm_corrections_sub, _selected_rtcm_instance,
+				  _last_rtcm_injection_time, _rtcm_injection_rate_message_count,
+		[this](const uint8_t *data, size_t len) { PublishRTCMStream(data, len); });
 	}
 
-	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) {
+		drain_rtcm_to_can(_rtcm_moving_baseline_sub, _selected_moving_baseline_instance,
+				  _last_moving_baseline_injection_time, _rtcm_injection_rate_message_count,
+		[this](const uint8_t *data, size_t len) { PublishMovingBaselineData(data, len); });
+	}
 }
 
 bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)

src/drivers/uavcan/sensors/gnss.hpp

@@ -48,7 +48,8 @@
 #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_corrections.h>
+#include <uORB/topics/rtcm_moving_baseline.h>
 #include <uORB/topics/gps_dump.h>
 
 #include <uavcan/uavcan.hpp>
@@ -143,9 +144,12 @@ 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_corrections_s, rtcm_corrections_s::MAX_INSTANCES> _rtcm_corrections_sub{ORB_ID::rtcm_corrections};
+	uORB::SubscriptionMultiArray<rtcm_moving_baseline_s, rtcm_moving_baseline_s::MAX_INSTANCES> _rtcm_moving_baseline_sub{ORB_ID::rtcm_moving_baseline};
 	hrt_abstime		_last_rtcm_injection_time{0};	///< time of last rtcm injection
+	hrt_abstime		_last_moving_baseline_injection_time{0};	///< time of last moving-baseline injection
 	uint8_t			_selected_rtcm_instance{0};	///< uorb instance that is being used for RTCM corrections
+	uint8_t			_selected_moving_baseline_instance{0};	///< uorb instance used for moving-baseline input
 
 	uORB::Publication<gps_dump_s> _gps_dump_pub{ORB_ID(gps_dump)}; // For PPK
 

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_moving_baseline.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,46 +70,38 @@ public:
 
 	void BroadcastAnyUpdates() override
 	{
-		// 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 = {};
 
 		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
-			union device::Device::DeviceId device_id;
-			device_id.devid = gps_inject_data.device_id;
-
-			if (device_id.devid_s.bus_type == device::Device::DeviceBusType::DeviceBusType_UAVCAN) {
-				continue;
-			}
-
 			ardupilot::gnss::MovingBaselineData mbd = {};
 
 			const size_t capacity = mbd.data.capacity();
 			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_moving_baseline.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,13 +83,13 @@ private:
 	{
 		// Don't republish a message from ourselves
 		if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) {
-			gps_inject_data_s gps_inject_data{};
+			rtcm_moving_baseline_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;
 
@@ -97,12 +97,12 @@ private:
 			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_moving_baseline_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_corrections.h>
 
 namespace uavcannode
 {
@@ -75,7 +75,7 @@ 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);
+		       _rtcm_corrections_pub.get_topic()->o_name);
 	}
 
 private:
@@ -83,13 +83,13 @@ private:
 	{
 		// Don't republish a message from ourselves
 		if (msg.getSrcNodeID().get() != getNode().getNodeID().get()) {
-			gps_inject_data_s gps_inject_data{};
+			rtcm_corrections_s corrections{};
 
-			gps_inject_data.len = msg.data.size();
+			corrections.len = msg.data.size();
 
-			memcpy(gps_inject_data.data, &msg.data[0], gps_inject_data.len);
+			memcpy(corrections.data, &msg.data[0], corrections.len);
 
-			gps_inject_data.timestamp = hrt_absolute_time();
+			corrections.timestamp = hrt_absolute_time();
 
 			union device::Device::DeviceId device_id;
 
@@ -97,12 +97,12 @@ private:
 			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;
+			corrections.device_id = device_id.devid;
 
-			_gps_inject_data_pub.publish(gps_inject_data);
+			_rtcm_corrections_pub.publish(corrections);
 		}
 	}
 
-	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
+	uORB::PublicationMulti<rtcm_corrections_s> _rtcm_corrections_pub{ORB_ID(rtcm_corrections)};
 };
 } // namespace uavcannode

src/modules/mavlink/mavlink_receiver.cpp

@@ -81,7 +81,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();
@@ -2736,18 +2736,18 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 	mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
 	mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
 
-	gps_inject_data_s gps_inject_data_topic{};
+	rtcm_corrections_s rtcm_corrections_topic{};
 
-	gps_inject_data_topic.timestamp = hrt_absolute_time();
+	rtcm_corrections_topic.timestamp = hrt_absolute_time();
 
-	gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data),
-					      (int)sizeof(uint8_t) * gps_rtcm_data_msg.len);
-	gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
-	memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
-	       math::min((int)sizeof(gps_inject_data_topic.data), (int)sizeof(uint8_t) * gps_inject_data_topic.len));
+	rtcm_corrections_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data),
+					       (int)sizeof(uint8_t) * gps_rtcm_data_msg.len);
+	rtcm_corrections_topic.flags = gps_rtcm_data_msg.flags;
+	memcpy(rtcm_corrections_topic.data, gps_rtcm_data_msg.data,
+	       math::min((int)sizeof(rtcm_corrections_topic.data), (int)sizeof(uint8_t) * rtcm_corrections_topic.len));
 
-	gps_inject_data_topic.timestamp = hrt_absolute_time();
-	_gps_inject_data_pub.publish(gps_inject_data_topic);
+	rtcm_corrections_topic.timestamp = hrt_absolute_time();
+	_rtcm_corrections_pub.publish(rtcm_corrections_topic);
 }
 
 void

src/modules/mavlink/mavlink_receiver.h

@@ -79,7 +79,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_corrections.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/input_rc.h>
 #include <uORB/topics/irlock_report.h>
@@ -360,7 +360,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_corrections_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

@@ -34,7 +34,7 @@
 #ifndef GPS_RTCM_DATA_HPP
 #define GPS_RTCM_DATA_HPP
 
-#include <uORB/topics/gps_inject_data.h>
+#include <uORB/topics/rtcm_corrections.h>
 
 class MavlinkStreamGPSRTCMData : public MavlinkStream
 {
@@ -49,25 +49,25 @@ public:
 
 	unsigned get_size() override
 	{
-		return _gps_inject_data_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _rtcm_corrections_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};
+	uORB::Subscription _rtcm_corrections_sub{ORB_ID(rtcm_corrections), 0};
 
 	bool send() override
 	{
-		gps_inject_data_s gps_inject_data;
+		rtcm_corrections_s corrections;
 		bool sent = false;
 
-		while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
+		while ((_mavlink->get_free_tx_buf() >= get_size()) && _rtcm_corrections_sub.update(&corrections)) {
 			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));
+			msg.len = corrections.len;
+			msg.flags = corrections.flags;
+			memcpy(msg.data, corrections.data, sizeof(msg.data));
 
 			mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
 

src/modules/zenoh/Kconfig.topics

@@ -337,10 +337,6 @@ menu "Zenoh publishers/subscribers"
 		bool "gps_dump"
 		default n
 
-	config ZENOH_PUBSUB_GPS_INJECT_DATA
-		bool "gps_inject_data"
-		default n
-
 	config ZENOH_PUBSUB_GRIPPER
 		bool "gripper"
 		default n
@@ -665,6 +661,14 @@ menu "Zenoh publishers/subscribers"
 		bool "rpm"
 		default n
 
+	config ZENOH_PUBSUB_RTCM_CORRECTIONS
+		bool "rtcm_corrections"
+		default n
+
+	config ZENOH_PUBSUB_RTCM_MOVING_BASELINE
+		bool "rtcm_moving_baseline"
+		default n
+
 	config ZENOH_PUBSUB_RTL_STATUS
 		bool "rtl_status"
 		default n
@@ -1034,7 +1038,6 @@ config ZENOH_PUBSUB_ALL_SELECTION
 	select ZENOH_PUBSUB_GPIO_OUT
 	select ZENOH_PUBSUB_GPIO_REQUEST
 	select ZENOH_PUBSUB_GPS_DUMP
-	select ZENOH_PUBSUB_GPS_INJECT_DATA
 	select ZENOH_PUBSUB_GRIPPER
 	select ZENOH_PUBSUB_HEALTH_REPORT
 	select ZENOH_PUBSUB_HEATER_STATUS
@@ -1116,6 +1119,8 @@ config ZENOH_PUBSUB_ALL_SELECTION
 	select ZENOH_PUBSUB_ROVER_STEERING_SETPOINT
 	select ZENOH_PUBSUB_ROVER_THROTTLE_SETPOINT
 	select ZENOH_PUBSUB_RPM
+	select ZENOH_PUBSUB_RTCM_CORRECTIONS
+	select ZENOH_PUBSUB_RTCM_MOVING_BASELINE
 	select ZENOH_PUBSUB_RTL_STATUS
 	select ZENOH_PUBSUB_RTL_TIME_ESTIMATE
 	select ZENOH_PUBSUB_SATELLITE_INFO