gps: fix RTCM injection and enable MSM7 for PPK (#26095)

* serial: add txSpaceAvailable function

* serial: txSpaceAvailable and bytesAvailable fixups

* msg: GpsDump: increase queue from 8 to 16 and replace instance with device_id

* lib: gnss: add RTCM parsing library. Generated by Claude Code.

* gps: fix RTCM injection to use inject-before-read pattern as before. Add RTCM parser to frame-align injection. Drain GpsInjectData uORB queue into RTCM parser buffer and then inject. Add GPS_UBX_PPK parameter to enable MSM7 output from the GPS module (rather than the default of MSM4) which is required for PPK workflows.

* gps: replace PX4_WARN with perf counters

src/drivers/gps/CMakeLists.txt

@@ -55,4 +55,5 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		git_gps_devices
+		gnss
 	)

src/drivers/gps/devices

@@ -1 +1 @@
-Subproject commit caf5158061bd10e79c9f042abb62c86bc6f3e7a7
+Subproject commit 991f771a8e192ccde3feb84fad3fb4fa3a3ef7bb

src/drivers/gps/gps.cpp

@@ -50,6 +50,7 @@
 #include <drivers/drv_sensor.h>
 #include <lib/drivers/device/Device.hpp>
 #include <lib/parameters/param.h>
+#include <lib/perf/perf_counter.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <px4_platform_common/atomic.h>
@@ -67,6 +68,8 @@
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/sensor_gnss_relative.h>
 
+#include <lib/gnss/rtcm.h>
+
 #ifndef CONSTRAINED_FLASH
 # include "devices/src/ashtech.h"
 # include "devices/src/emlid_reach.h"
@@ -215,6 +218,11 @@ private:
 	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{};
+
+	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")};
+
 	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.
 
@@ -264,7 +272,7 @@ private:
 	 * @param data
 	 * @param len
 	 */
-	inline bool injectData(uint8_t *data, size_t len);
+	inline bool injectData(const uint8_t *data, size_t len);
 
 	/**
 	 * set the Baudrate
@@ -285,7 +293,7 @@ private:
 	 * @param mode calling source
 	 * @param msg_to_gps_device if true, this is a message sent to the gps device, otherwise it's from the device
 	 */
-	void dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device);
+	void dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device);
 
 	void initializeCommunicationDump();
 
@@ -383,6 +391,9 @@ GPS::~GPS()
 		} while (_secondary_instance.load() && i < 100);
 	}
 
+	perf_free(_uart_tx_buffer_full_perf);
+	perf_free(_rtcm_buffer_full_perf);
+
 	delete _sat_info;
 	delete _dump_to_device;
 	delete _dump_from_device;
@@ -472,28 +483,16 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout)
 	const int max_timeout = 50;
 	int timeout_adjusted = math::min(max_timeout, timeout);
 
+	handleInjectDataTopic();
+
 	if (_interface == GPSHelper::Interface::UART) {
-
-		const ssize_t read_at_least = math::min(character_count, buf_length);
-
-		// handle injection data before read if caught up
-		if (_uart.bytesAvailable() < read_at_least) {
-			handleInjectDataTopic();
-		}
-
-		ret = _uart.readAtLeast(buf, buf_length, read_at_least, timeout_adjusted);
-
-		if (ret > 0) {
-			_num_bytes_read += ret;
-		}
+		ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted);
 
 // SPI is only supported on LInux
 #if defined(__PX4_LINUX)
 
 	} else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) {
 
-		handleInjectDataTopic();
-
 		//Poll only for the SPI data. In the same thread we also need to handle orb messages,
 		//so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the
 		//two pollings use different underlying mechanisms (at least under posix), which makes this
@@ -583,6 +582,7 @@ void GPS::handleInjectDataTopic()
 	// 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;
 
@@ -592,13 +592,13 @@ void GPS::handleInjectDataTopic()
 
 			// Prevent injection of data from self
 			if (msg.device_id != get_device_id()) {
-				/* 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.
-				*/
-				injectData(msg.data, msg.len);
+				// Add data to the RTCM parser buffer for frame reassembly
+				size_t added = _rtcm_parser.addData(msg.data, msg.len);
+
+				if (added < msg.len) {
+					perf_count(_rtcm_buffer_full_perf);
+				}
 
-				++_rtcm_injection_rate_message_count;
 				_last_rtcm_injection_time = hrt_absolute_time();
 			}
 		}
@@ -616,9 +616,30 @@ void GPS::handleInjectDataTopic()
 		}
 
 	} while (updated && num_injections < max_num_injections);
+
+	// Now inject all complete RTCM frames from the parser buffer
+	size_t frame_len = {};
+	const uint8_t *frame_ptr = {};
+
+	while ((frame_ptr = _rtcm_parser.getNextMessage(&frame_len)) != nullptr) {
+		// Check TX buffer space before writing
+		if (_interface == GPSHelper::Interface::UART) {
+			ssize_t tx_available = _uart.txSpaceAvailable();
+
+			if ((ssize_t)frame_len > tx_available) {
+				// TX buffer full, stop and let it drain - frames stay in parser buffer
+				perf_count(_uart_tx_buffer_full_perf);
+				break;
+			}
+		}
+
+		injectData(frame_ptr, frame_len);
+		_rtcm_parser.consumeMessage(frame_len);
+		_rtcm_injection_rate_message_count++;
+	}
 }
 
-bool GPS::injectData(uint8_t *data, size_t len)
+bool GPS::injectData(const uint8_t *data, size_t len)
 {
 	dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true);
 
@@ -687,7 +708,7 @@ void GPS::initializeCommunicationDump()
 	_dump_communication_mode = (gps_dump_comm_mode_t)param_dump_comm;
 }
 
-void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device)
+void GPS::dumpGpsData(const uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool msg_to_gps_device)
 {
 	gps_dump_s *dump_data  = msg_to_gps_device ? _dump_to_device : _dump_from_device;
 
@@ -798,6 +819,13 @@ GPS::run()
 		param_get(handle, &f9p_uart2_baudrate);
 	}
 
+	handle = param_find("GPS_UBX_PPK");
+	int32_t ppk_output = 0;
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &ppk_output);
+	}
+
 	int32_t gnssSystemsParam = static_cast<int32_t>(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS);
 
 	if (_instance == Instance::Main) {
@@ -879,11 +907,21 @@ GPS::run()
 			_mode = gps_driver_mode_t::UBX;
 
 		/* FALLTHROUGH */
-		case gps_driver_mode_t::UBX:
-			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info,
-						   gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
-			set_device_type(DRV_GPS_DEVTYPE_UBX);
-			break;
+		case gps_driver_mode_t::UBX: {
+				GPSDriverUBX::Settings settings = {
+					.dynamic_model = (uint8_t)gps_ubx_dynmodel,
+					.heading_offset = heading_offset,
+					.uart2_baudrate = f9p_uart2_baudrate,
+					.ppk_output = ppk_output > 0,
+					.mode = ubx_mode,
+				};
+
+				_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_sensor_gps, _p_report_sat_info, settings);
+
+				set_device_type(DRV_GPS_DEVTYPE_UBX);
+				break;
+			}
+
 #ifndef CONSTRAINED_FLASH
 
 		case gps_driver_mode_t::MTK:
@@ -1021,6 +1059,8 @@ GPS::run()
 				healthy_timeout += TIMEOUT_DUMP_ADD;
 			}
 
+			PX4_INFO("GPS device configured @ %u baud", _baudrate);
+
 			while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) {
 
 				if (helper_ret & 1) {
@@ -1208,6 +1248,9 @@ GPS::print_status()
 		print_message(ORB_ID(sensor_gps), _sensor_gps);
 	}
 
+	perf_print_counter(_uart_tx_buffer_full_perf);
+	perf_print_counter(_rtcm_buffer_full_perf);
+
 	if (_instance == Instance::Main && _secondary_instance.load()) {
 		GPS *secondary_instance = _secondary_instance.load();
 		secondary_instance->print_status();

src/drivers/gps/params.c

@@ -144,6 +144,15 @@ PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
  */
 PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
 
+/**
+ * Enable MSM7 message output for PPK workflow.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_PPK, 0);
+
 /**
  * Wipes the flash config of UBX modules.
  *