Subscribe to all intances of gps_inject_data and mirror uavcan rtcm pub mirror gps driver

src/drivers/uavcan/sensors/gnss.cpp

@@ -467,29 +467,61 @@ void UavcanGnssBridge::update()
 // to work.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
+	// 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 ((hrt_absolute_time() - _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;
+					}
+				}
+			}
+		}
+	}
+
+	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 6 packets thus guarantees, that at least a full injection
+	// Looking at 8 packets thus guarantees, that at least a full injection
 	// data set is evaluated.
-	static constexpr size_t MAX_NUM_INJECTIONS = gps_inject_data_s::ORB_QUEUE_LENGTH;;
-
+	// 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;
-	gps_inject_data_s gps_inject_data;
 
-	while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) {
-		// 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(gps_inject_data.data, gps_inject_data.len);
+	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);
+			}
+
+			_last_rtcm_injection_time = hrt_absolute_time();
 		}
 
-		if (_publish_moving_baseline_data) {
-			PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
-		}
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
 
-		num_injections++;
-	}
+	} while (updated && num_injections < max_num_injections);
 }
 
 bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len)

src/drivers/uavcan/sensors/gnss.hpp

@@ -45,6 +45,7 @@
 #pragma once
 
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/gps_inject_data.h>
@@ -123,7 +124,9 @@ private:
 	float		_last_gnss_auxiliary_hdop{0.0f};
 	float		_last_gnss_auxiliary_vdop{0.0f};
 
-	uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)};
+	uORB::SubscriptionMultiArray<gps_inject_data_s, gps_inject_data_s::MAX_INSTANCES> _orb_inject_data_sub{ORB_ID::gps_inject_data};
+	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
 
 	bool _system_clock_set{false};  ///< Have we set the system clock at least once from GNSS data?