GPS RTCM selection fixes (#21666)

* uavcan rtcm set max num injections
* Subscribe to all instances of gps_inject_data and mirror uavcan rtcm pub mirror gps driver
* Minor logic refactor in gps and uavcan gps inject data

src/drivers/gps/gps.cpp

@@ -60,6 +60,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
@@ -203,7 +204,7 @@ private:
 
 	const Instance 			_instance;
 
-	uORB::Subscription		     _orb_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};
 	uORB::Publication<gps_inject_data_s> _gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::Publication<gps_dump_s>	     _dump_communication_pub{ORB_ID(gps_dump)};
 	gps_dump_s			     *_dump_to_device{nullptr};
@@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic()
 	// 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 (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) {
-			if (_orb_inject_data_sub.ChangeInstance(i)) {
-				if (_orb_inject_data_sub.copy(&msg)) {
+		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 = i;
+						_selected_rtcm_instance = instance;
 						break;
 					}
 				}
@@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic()
 		}
 	}
 
-	// Reset instance in case we didn't actually want to switch
-	_orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance);
-
 	bool updated = already_copied;
 
 	// Limit maximum number of GPS injections to 8 since usually
@@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic()
 			}
 		}
 
-		updated = _orb_inject_data_sub.update(&msg);
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
 
 	} while (updated && num_injections < max_num_injections);
 }

src/drivers/uavcan/sensors/gnss.cpp

@@ -467,29 +467,61 @@ void UavcanGnssBridge::update()
 // to work.
 void UavcanGnssBridge::handleInjectDataTopic()
 {
-	// Limit maximum number of GPS injections to 6 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
-	// data set is evaluated.
-	static constexpr size_t MAX_NUM_INJECTIONS = 6;
+	// 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;
 
-	size_t num_injections = 0;
-	gps_inject_data_s gps_inject_data;
+	// 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) {
 
-	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);
+		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_moving_baseline_data) {
-			PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len);
-		}
-
-		num_injections++;
 	}
+
+	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.
+	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);
+			}
+
+			_last_rtcm_injection_time = hrt_absolute_time();
+		}
+
+		updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg);
+
+	} 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?