Support two RTCM links with the same corrections

Add selected_rtcm_instance to sensor_gps message to track loss of rtcm links

Publish _rate_rtcm_injection

msg/gps_inject_data.msg

@@ -7,3 +7,5 @@ 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/sensor_gps.msg

@@ -46,4 +46,7 @@ float32 heading			# heading angle of XYZ body frame rel to NED. Set to NaN if no
 float32 heading_offset		# heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI])
 float32 heading_accuracy	# heading accuracy (rad, [0, 2PI])
 
+float32 rtcm_injection_rate	# RTCM message injection rate Hz
+uint8 selected_rtcm_instance	# uorb instance that is being used for RTCM corrections
+
 # TOPICS sensor_gps vehicle_gps_position

src/drivers/gps/gps.cpp

@@ -78,9 +78,11 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */
 
+using namespace time_literals;
+
 #define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
 #define TIMEOUT_5HZ		500		//!< Timeout time in mS,  200 mS (5Hz) + 300 mS delta for error
-#define RATE_MEASUREMENT_PERIOD 5000000
+#define RATE_MEASUREMENT_PERIOD 5_s
 
 enum class gps_driver_mode_t {
 	None = 0,
@@ -191,6 +193,8 @@ private:
 	unsigned			_last_rate_rtcm_injection_count{0};		///< counter for number of RTCM messages
 	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
+	uint8_t				_selected_rtcm_instance{0};			///< uorb instance that is being used for RTCM corrections
 
 	const Instance 			_instance;
 
@@ -511,6 +515,24 @@ void GPS::handleInjectDataTopic()
 		return;
 	}
 
+	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) {
+		_last_rtcm_injection_time = hrt_absolute_time();
+
+		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)) {
+					if ((hrt_absolute_time() - msg.timestamp) < 5_s) {
+						_selected_rtcm_instance = i;
+						break;
+					}
+				}
+			}
+		}
+	}
+
 	bool updated = false;
 
 	// Limit maximum number of GPS injections to 6 since usually
@@ -525,7 +547,6 @@ void GPS::handleInjectDataTopic()
 		updated = _orb_inject_data_sub.updated();
 
 		if (updated) {
-			gps_inject_data_s msg;
 
 			if (_orb_inject_data_sub.copy(&msg)) {
 
@@ -538,6 +559,7 @@ void GPS::handleInjectDataTopic()
 					injectData(msg.data, msg.len);
 
 					++_last_rate_rtcm_injection_count;
+					_last_rtcm_injection_time = hrt_absolute_time();
 				}
 			}
 		}
@@ -1143,6 +1165,9 @@ GPS::publish()
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
 		_report_gps_pos.device_id = get_device_id();
 
+		_report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance;
+		_report_gps_pos.rtcm_injection_rate = _rate_rtcm_injection;
+
 		_report_gps_pos_pub.publish(_report_gps_pos);
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.

src/modules/mavlink/mavlink_receiver.cpp

@@ -2603,6 +2603,8 @@ MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
 
 	gps_inject_data_s gps_inject_data_topic{};
 
+	gps_inject_data_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;

src/modules/mavlink/mavlink_receiver.h

@@ -330,6 +330,7 @@ private:
 
 	// ORB publications (multi)
 	uORB::PublicationMulti<distance_sensor_s>		_distance_sensor_pub{ORB_ID(distance_sensor)};
+	uORB::PublicationMulti<gps_inject_data_s>		_gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	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)};
@@ -339,7 +340,6 @@ private:
 	uORB::PublicationMulti<sensor_optical_flow_s>         _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
 
 	// ORB publications (queue length > 1)
-	uORB::Publication<gps_inject_data_s>     _gps_inject_data_pub{ORB_ID(gps_inject_data)};
 	uORB::Publication<transponder_report_s>  _transponder_report_pub{ORB_ID(transponder_report)};
 	uORB::Publication<vehicle_command_s>     _cmd_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _cmd_ack_pub{ORB_ID(vehicle_command_ack)};