feat(mavlink): reassemble GPS_RTCM_DATA before GPS injection (#27084)

* mavlink: reassemble GPS_RTCM_DATA before GPS injection * Apply minor comment requested changes * Simplification: remove _completed_sequence asymetric protection * Handle RTCM payload length which is an exact multiple of 180 * update docs * lib gnss: new GpsRtcmMessageFragmenter to send RTCM via GPS_RTCM_DATA.hpp * fix clang * Remove RTCM fragmenter * update docs * Compatibility fallback for older QGroundControl builds that omit the final zero-length fragment * mavlink receiver, remove while loop to avoid dead lock * docs(update): Subedit Co-authored-by: Hamish Willee <hamishwillee@gmail.com> * docs(docs): format --------- Co-authored-by: jonas <jonas.perolini@rigi.tech> Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
2026-05-24 21:09:41 +08:00 · 2026-05-21 20:23:50 +02:00
parent e1befdea29
commit 80557b32ee
9 changed files with 911 additions and 12 deletions
--- a/docs/en/advanced/rtk_gps.md
+++ b/docs/en/advanced/rtk_gps.md
@@ -15,7 +15,7 @@ It relies on a single reference station to provide real-time corrections, which
 Two RTK GNSS modules and a datalink are required to setup RTK with PX4.
 The fixed-position ground-based GPS unit is called the _Base_ and the in-air unit is called the _Rover_.
 The Base unit connects to _QGroundControl_ (via USB) and uses the datalink to stream RTCM corrections to the vehicle (using the MAVLink [GPS_RTCM_DATA](https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA) message).
-On the autopilot, the MAVLink packets are unpacked and sent to the Rover unit, where they are processed to get the RTK solution.
+On the autopilot, `GPS_RTCM_DATA` packets are reassembled according to the MAVLink fragment and sequence fields before the RTCM byte stream is forwarded to the Rover unit, where it is processed to get the RTK solution.

 The datalink should typically be able to handle an uplink rate of 300 bytes per second (see the [Uplink Datarate](#uplink-datarate) section below for more information).

@@ -32,7 +32,7 @@ Make sure to select the correct variant.

 The PX4 GPS stack automatically sets up the GPS modules to send and receive the correct messages over the UART or USB, depending on where the module is connected (to _QGroundControl_ or the autopilot).

-As soon as the autopilot receives `GPS_RTCM_DATA` MAVLink messages, it automatically forwards the RTCM data to the attached GPS module over existing data channels (a dedicated channel for correction data is not required).
+As soon as the autopilot receives `GPS_RTCM_DATA` MAVLink messages, it reassembles fragmented packets when needed and then forwards the RTCM data to the attached GPS module over existing data channels (a dedicated channel for correction data is not required).

 ::: info
 The u-blox U-Center RTK module configuration tool is not needed/used!
@@ -43,6 +43,24 @@ Both _QGroundControl_ and the autopilot firmware share the same [PX4 GPS driver
 In practice, this means that support for new protocols and/or messages only need to be added to one place.
 :::

+### GPS_RTCM_DATA handling
+
+If you are sending RTCM corrections to PX4 yourself, follow the MAVLink [`GPS_RTCM_DATA`](https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA) definition:
+
+- Each MAVLink packet carries up to 180 bytes of RTCM data.
+- If the RTCM payload exceeds 180 bytes, split it across up to 4 packets using the Fragment ID and Sequence ID (encoded in `GPS_RTCM_DATA.flags`).
+  Every packet except the last one must be filled to its maximum 180-byte capacity; only the final packet may be partially filled.
+- PX4 reassembles fragmented packets according to the MAVLink rules and supports out-of-order delivery for one in-progress fragmented message at a time.
+- A fragmented message is considered complete when either 4 fragments with the same Sequence ID have been received, or when you receive a partial fragment and you have already recieved all the fully-packed fragments that precede it (by Fragment ID) in the current sequence.
+- If the RTCM payload length is an exact multiple of 180 bytes and uses fewer than 4 fragments, the sender must still send a final zero-length fragment to mark completion. A 720-byte payload (all 4 fragments full) is complete after the last fragment is received.
+- As a compatibility fallback for older QGroundControl builds that omit that final zero-length fragment, PX4 also flushes a buffered RTCM message to the GNSS when a `GPS_RTCM_DATA` message with a different Sequence ID arrives, but only if the buffered fragments are a gap-free run of full 180-byte fragments starting at fragment 0.
+
+Current limitations:
+
+- PX4 keeps only one in-progress fragmented `GPS_RTCM_DATA` message at a time. A packet with a different `sequence_id` starts a new buffer.
+- Stale partial state is dropped after 1 second if the rest of the fragments do not arrive.
+- The legacy exact-multiple compatibility fallback only works if another `sequence_id` arrives before that 1 second timeout. Otherwise the buffered partial message is dropped.
+
 ### RTCM messages

 QGroundControl configures the RTK base station to output the following RTCM3.2 frames, each with 1 Hz, unless otherwise stated:
--- a/msg/GpsInjectData.msg
+++ b/msg/GpsInjectData.msg
@@ -3,8 +3,8 @@ 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 flags                     # LSB: 1=fragmented across multiple uORB publications
+uint8[300] data                 # data chunk to write to GPS device (RTCM message)

 uint8 ORB_QUEUE_LENGTH = 8

--- a/src/lib/gnss/rtcm.cpp
+++ b/src/lib/gnss/rtcm.cpp
@@ -62,6 +62,262 @@ uint32_t rtcm3_crc24q(const uint8_t *data, size_t len)
 	return crc & 0xFFFFFF;
 }

+const uint8_t *GpsRtcmMessageAssembler::addPacket(uint8_t flags, const uint8_t *data, size_t len, uint64_t timestamp,
+		size_t &out_len)
+{
+	out_len = 0;
+
+	if (data == nullptr) {
+		reset();
+		return nullptr;
+	}
+
+	// Timeout, clear stale partial state.
+	if (_active_sequence.active && (_active_sequence.timestamp != 0)
+	    && (timestamp > _active_sequence.timestamp + GPS_RTCM_FRAGMENT_TIMEOUT_US)) {
+		resetActiveState();
+	}
+
+	if (!gps_rtcm_is_fragmented(flags)) {
+		// Compatibility fallback for older QGroundControl builds
+		// that omit the final zero-length fragment
+		const size_t legacy_message_len = assembleLegacyExactMultipleMessage(_assembled_message);
+
+		// New fragment is invalid, return previous message if valid
+		if (len > sizeof(_assembled_message)) {
+			reset();
+			out_len = legacy_message_len;
+			return (legacy_message_len > 0) ? _assembled_message : nullptr;
+		}
+
+		if (legacy_message_len > 0) {
+			// Return the older buffered message first. Queue this new
+			// unfragmented packet as deferred output.
+			memcpy(_deferred_message, data, len);
+			_deferred_message_len = len;
+			_deferred_message_valid = true;
+			resetActiveState();
+			out_len = legacy_message_len;
+			return _assembled_message;
+		}
+
+		reset();
+		memcpy(_assembled_message, data, len);
+		out_len = len;
+
+		return _assembled_message;
+	}
+
+	if (len > GPS_RTCM_MAX_FRAGMENT_LEN) {
+		resetActiveState();
+		return nullptr;
+	}
+
+	const uint8_t sequence_id = gps_rtcm_sequence_id(flags);
+	const uint8_t fragment_id = gps_rtcm_fragment_id(flags);
+	const bool is_non_full_fragment = len < GPS_RTCM_MAX_FRAGMENT_LEN;
+	const uint8_t *sequence_change_message = nullptr;
+	size_t sequence_change_message_len = 0;
+
+	if (!_active_sequence.active || sequence_id != _active_sequence.sequence_id) {
+		if (_active_sequence.active) {
+			// Compatibility fallback for older QGroundControl builds
+			// that omit the final zero-length fragment
+			sequence_change_message_len = assembleLegacyExactMultipleMessage(_assembled_message);
+
+			if (sequence_change_message_len > 0) {
+				// Do not return here: this same packet also starts the new
+				// sequence, so it still needs to be validated and buffered.
+				sequence_change_message = _assembled_message;
+			}
+		}
+
+		resetActiveState();
+		_active_sequence.active = true;
+		_active_sequence.sequence_id = sequence_id;
+	}
+
+	const bool last_fragment_known = (_active_sequence.last_fragment_id >= 0);
+
+	// Once a non-full final fragment is known, any later fragment index is invalid.
+	if (last_fragment_known && (fragment_id > _active_sequence.last_fragment_id)) {
+		resetActiveState();
+		out_len = sequence_change_message_len;
+		return sequence_change_message;
+	}
+
+	// A non-full fragment before the known end would contradict the current
+	// end-of-message boundary. That can only come from malformed sender state.
+	if (last_fragment_known && (fragment_id < _active_sequence.last_fragment_id) && is_non_full_fragment) {
+		resetActiveState();
+		out_len = sequence_change_message_len;
+		return sequence_change_message;
+	}
+
+	// The first fragment with a non-full payload defines the completion
+	// boundary. A higher buffered fragment would contradict that boundary.
+	if (is_non_full_fragment && hasFragmentAfter(fragment_id)) {
+		resetActiveState();
+		out_len = sequence_change_message_len;
+		return sequence_change_message;
+	}
+
+	FragmentSlot &fragment = _fragments[fragment_id];
+
+	if (fragment.present && ((fragment.len != len) || (memcmp(fragment.data, data, len) != 0))) {
+		// Reusing a slot with different bytes means the current buffer no longer
+		// matches the incoming fragments. Drop the old partial state and treat
+		// this packet as the start of a new buffer for the same sequence ID.
+		resetActiveState();
+		_active_sequence.active = true;
+		_active_sequence.sequence_id = sequence_id;
+	}
+
+	memcpy(fragment.data, data, len);
+	fragment.len = len;
+	fragment.present = true;
+	_active_sequence.timestamp = timestamp;
+
+	// The buffer is complete when either all 4 fragments are present, or when
+	// the first fragment with a non-full payload has been received and every
+	// lower fragment ID is already present. Recording fragment 3 as the
+	// boundary covers the "all 4 fragments" case. A non-full fragment 0 also
+	// completes immediately, although a compliant sender would normally send
+	// that packet as unfragmented instead of setting the fragmented bit.
+	if (is_non_full_fragment || (fragment_id == GPS_RTCM_MAX_FRAGMENTS - 1)) {
+		_active_sequence.last_fragment_id = fragment_id;
+	}
+
+	if (!isComplete()) {
+		out_len = sequence_change_message_len;
+		return sequence_change_message;
+	}
+
+	size_t assembled_len = 0;
+	uint8_t *destination = (sequence_change_message != nullptr) ? _deferred_message : _assembled_message;
+
+	for (size_t i = 0; i <= lastFragmentIndex(); i++) {
+		const FragmentSlot &slot = _fragments[i];
+		memcpy(&destination[assembled_len], slot.data, slot.len);
+		assembled_len += slot.len;
+	}
+
+	resetActiveState();
+
+	if (sequence_change_message != nullptr) {
+		_deferred_message_len = assembled_len;
+		_deferred_message_valid = true;
+		out_len = sequence_change_message_len;
+		return sequence_change_message;
+	}
+
+	out_len = assembled_len;
+	return _assembled_message;
+}
+
+const uint8_t *GpsRtcmMessageAssembler::takeDeferredMessage(size_t &out_len)
+{
+	out_len = 0;
+
+	if (!_deferred_message_valid) {
+		return nullptr;
+	}
+
+	_deferred_message_valid = false;
+	out_len = _deferred_message_len;
+	return _deferred_message;
+}
+
+void GpsRtcmMessageAssembler::reset()
+{
+	resetActiveState();
+	_deferred_message_len = 0;
+	_deferred_message_valid = false;
+}
+
+void GpsRtcmMessageAssembler::resetActiveState()
+{
+	for (FragmentSlot &fragment : _fragments) {
+		fragment.present = false;
+		fragment.len = 0;
+	}
+
+	_active_sequence = {};
+}
+
+bool GpsRtcmMessageAssembler::hasFragmentAfter(uint8_t fragment_id) const
+{
+	for (size_t i = fragment_id + 1; i < GPS_RTCM_MAX_FRAGMENTS; i++) {
+		if (_fragments[i].present) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+size_t GpsRtcmMessageAssembler::assembleLegacyExactMultipleMessage(uint8_t *destination) const
+{
+	size_t full_fragments = 0;
+
+	// Extension to MAVLink rule: accept legacy senders that omit the required
+	// zero-length terminator for exact multiples of 180 bytes, but only when
+	// every buffered fragment forms a gap-free full-size prefix starting at
+	// fragment 0.
+	for (; full_fragments < GPS_RTCM_MAX_FRAGMENTS; full_fragments++) {
+		if (!_fragments[full_fragments].present) {
+			break;
+		}
+
+		if (_fragments[full_fragments].len != GPS_RTCM_MAX_FRAGMENT_LEN) {
+			break;
+		}
+	}
+
+	// Reject buffers like [180, _, 180]: only flush when every buffered
+	// fragment belongs to one contiguous full-size run starting at fragment 0.
+	if ((full_fragments == 0) || hasFragmentAfter(full_fragments - 1)) {
+		return 0;
+	}
+
+	size_t assembled_len = 0;
+
+	for (size_t i = 0; i < full_fragments; i++) {
+		const FragmentSlot &slot = _fragments[i];
+		memcpy(&destination[assembled_len], slot.data, slot.len);
+		assembled_len += slot.len;
+	}
+
+	return assembled_len;
+}
+
+bool GpsRtcmMessageAssembler::isComplete() const
+{
+	for (size_t i = 0; i <= lastFragmentIndex(); i++) {
+		if (!_fragments[i].present) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+size_t GpsRtcmMessageAssembler::lastFragmentIndex() const
+{
+	// Per the MAVLink GPS_RTCM_DATA rule, a buffer is complete once either all
+	// 4 fragments are present, or the first fragment with a non-full payload
+	// has been received and every lower fragment ID is present. Until such a
+	// fragment arrives, the receiver must assume that all 4 fragment slots may
+	// still be used. If the RTCM payload length is an exact multiple of 180
+	// bytes and uses fewer than 4 fragments, the sender must still send a
+	// final zero-length fragment to mark completion. Sequence rollover above
+	// intentionally accepts older senders that omit that terminator, but only
+	// for gap-free full-size fragment runs starting at fragment 0 while the old
+	// sequence is still buffered.
+	return (_active_sequence.last_fragment_id >= 0) ? static_cast<size_t>(_active_sequence.last_fragment_id) :
+	       (GPS_RTCM_MAX_FRAGMENTS - 1);
+}
+
 size_t Rtcm3Parser::addData(const uint8_t *data, size_t len)
 {
 	size_t space_available = BUFFER_SIZE - _buffer_len;
--- a/src/lib/gnss/rtcm.h
+++ b/src/lib/gnss/rtcm.h
@@ -62,6 +62,28 @@ static constexpr size_t   RTCM3_MAX_PAYLOAD_LEN = 1023;
 static constexpr size_t   RTCM3_MAX_FRAME_LEN   = RTCM3_HEADER_LEN + RTCM3_MAX_PAYLOAD_LEN + RTCM3_CRC_LEN; // 1029
 static constexpr uint32_t RTCM3_CRC24Q_POLY     = 0x1864CFB;

+// MAVLink GPS_RTCM_DATA fragmentation constants.
+// Spec: https://mavlink.io/en/messages/common.html#GPS_RTCM_DATA
+//
+// flags bit layout:
+//   bit 0:   fragmented flag
+//   bits 1-2: fragment ID
+//   bits 3-7: sequence ID
+//
+// Fragmented payloads must only be flushed once the complete buffer has been
+// reconstructed. A buffer is complete once either all 4 fragments are present,
+// or the first fragment with a non-full payload has been received and every
+// lower fragment ID is present.
+static constexpr size_t   GPS_RTCM_MAX_FRAGMENT_LEN         = 180;
+static constexpr size_t   GPS_RTCM_MAX_FRAGMENTS            = 4;
+static constexpr size_t   GPS_RTCM_MAX_MESSAGE_LEN          = GPS_RTCM_MAX_FRAGMENT_LEN * GPS_RTCM_MAX_FRAGMENTS;
+static constexpr uint64_t GPS_RTCM_FRAGMENT_TIMEOUT_US      = 1000000;
+static constexpr uint8_t  GPS_RTCM_FLAG_FRAGMENTED          = 1 << 0;
+static constexpr uint8_t  GPS_RTCM_FLAG_FRAGMENT_ID_SHIFT   = 1;
+static constexpr uint8_t  GPS_RTCM_FLAG_FRAGMENT_ID_MASK    = 0x03;
+static constexpr uint8_t  GPS_RTCM_FLAG_SEQUENCE_ID_SHIFT   = 3;
+static constexpr uint8_t  GPS_RTCM_FLAG_SEQUENCE_ID_MASK    = 0x1f;
+
 /**
 * Calculate CRC-24Q checksum for RTCM3 messages.
 *
@@ -95,6 +117,114 @@ inline size_t rtcm3_payload_length(const uint8_t *frame)
 	return ((static_cast<size_t>(frame[1]) & 0x03) << 8) | frame[2];
 }

+inline bool gps_rtcm_is_fragmented(uint8_t flags)
+{
+	return (flags & GPS_RTCM_FLAG_FRAGMENTED) != 0;
+}
+
+inline uint8_t gps_rtcm_fragment_id(uint8_t flags)
+{
+	return (flags >> GPS_RTCM_FLAG_FRAGMENT_ID_SHIFT) & GPS_RTCM_FLAG_FRAGMENT_ID_MASK;
+}
+
+inline uint8_t gps_rtcm_sequence_id(uint8_t flags)
+{
+	return (flags >> GPS_RTCM_FLAG_SEQUENCE_ID_SHIFT) & GPS_RTCM_FLAG_SEQUENCE_ID_MASK;
+}
+
+class GpsRtcmMessageAssembler
+{
+public:
+	GpsRtcmMessageAssembler() = default;
+
+	/**
+	 * Add a MAVLink GPS_RTCM_DATA packet.
+	 *
+	 * Fragmented packets are buffered until the autopilot can reconstruct the
+	 * complete payload. The fragment ID selects the slot within the 4-fragment
+	 * buffer, while the sequence ID prevents fragments from different buffers
+	 * from being mixed together. A fragmented payload is complete once either
+	 * all 4 fragments are present, or the first fragment with a non-full
+	 * payload has been received and every lower fragment ID is present. If the
+	 * RTCM payload length is an exact multiple of 180 bytes and uses fewer than
+	 * 4 fragments, the sender must still send a final zero-length fragment to
+	 * mark completion.
+	 *
+	 * As a compatibility fallback for older senders that omit
+	 * that terminator, PX4 also flushes a buffered message when a different
+	 * sequence ID arrives, but only if the buffered fragments are a gap-free
+	 * run of full 180-byte fragments starting at fragment 0 (the start of the
+	 * RTCM message). This fallback only works while the older sequence remains
+	 * buffered (up to the 1 second fragment timeout).
+	 *
+	 * If processing a packet completes two messages, addPacket() returns the older one
+	 * and queues the newer one for takeDeferredMessage(). The returned pointer
+	 * is valid until the next call to addPacket().
+	 *
+	 * @param flags       MAVLink GPS_RTCM_DATA flags
+	 * @param data        Packet payload
+	 * @param len         Packet payload length
+	 * @param timestamp   Packet arrival timestamp in microseconds
+	 * @param out_len     Set to the reassembled message length, or 0 if incomplete/invalid
+	 * @return            Pointer to a complete message, or nullptr if the message is incomplete or invalid
+	 */
+	const uint8_t *addPacket(uint8_t flags, const uint8_t *data, size_t len, uint64_t timestamp, size_t &out_len);
+
+	/**
+	 * Retrieve the single deferred message queued by addPacket().
+	 *
+	 * This is only needed when processing one input packet completes two RTCM
+	 * messages, for example when a legacy exact-multiple sequence is flushed on
+	 * sequence rollover and the same packet also completes the new sequence.
+	 *
+	 * @param out_len     Set to the queued message length, or 0 if none is queued
+	 * @return            Pointer to the queued message, or nullptr if none is queued
+	 */
+	const uint8_t *takeDeferredMessage(size_t &out_len);
+
+	void reset();
+
+private:
+	struct FragmentSlot {
+		uint8_t data[GPS_RTCM_MAX_FRAGMENT_LEN] {};
+		size_t len {0};
+		bool present {false};
+	};
+
+	struct SequenceState {
+		uint8_t sequence_id {0};
+		int8_t last_fragment_id {-1};
+		uint64_t timestamp {0};
+		bool active {false};
+	};
+
+	void resetActiveState();
+	bool hasFragmentAfter(uint8_t fragment_id) const;
+
+	/**
+	 * Compatibility fallback for older QGroundControl builds that omit the
+	 * final zero-length fragment: assemble the buffered message only if the
+	 * buffered fragments are a gap-free run of full 180-byte fragments
+	 * starting at fragment 0.
+	 *
+	 * @param destination Buffer that receives the assembled message bytes
+	 * @return Length of the assembled message, or 0 if the fallback does not apply
+	 *
+	 */
+	size_t assembleLegacyExactMultipleMessage(uint8_t *destination) const;
+
+	bool isComplete() const;
+	size_t lastFragmentIndex() const;
+
+	FragmentSlot _fragments[GPS_RTCM_MAX_FRAGMENTS] {};
+	uint8_t _assembled_message[GPS_RTCM_MAX_MESSAGE_LEN] {};
+	uint8_t _deferred_message[GPS_RTCM_MAX_MESSAGE_LEN] {};
+	size_t _deferred_message_len {0};
+	bool _deferred_message_valid {false};
+	// State for the sequence that is currently being assembled.
+	SequenceState _active_sequence {};
+};
+
 /**
 * RTCM3 parser statistics.
 */
--- a/src/lib/gnss/test/RtcmParserTest.cpp
+++ b/src/lib/gnss/test/RtcmParserTest.cpp
@@ -206,3 +206,449 @@ TEST_F(RtcmTest, GetNextOnEmptyBuffer)
 	size_t len;
 	EXPECT_EQ(parser.getNextMessage(&len), nullptr);
 }
+
+// =============================================================================
+// GPS_RTCM_DATA fragmentation tests
+// =============================================================================
+
+// WHAT: Verify that a normal unfragmented payload is passed through unchanged.
+// WHY: The assembler must ignore sequence bits when the fragmented flag is not set.
+TEST_F(RtcmTest, GpsRtcmAssemblerReturnsUnfragmentedPayload)
+{
+	// GIVEN: A small payload that fits in one packet.
+	const std::vector<uint8_t> payload {0x01, 0x02, 0x03, 0x04};
+	size_t len = 0;
+
+	// WHEN: The payload is added without the fragmented flag.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(false, 0, 9), payload.data(), payload.size(), 10,
+				 len);
+
+	// THEN: The assembler returns the full payload unchanged.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that the unfragmented path accepts the largest payload the helper stores.
+// WHY: This keeps the direct helper's explicit size guard covered even though
+// MAVLink GPS_RTCM_DATA packets top out at 180 payload bytes.
+TEST_F(RtcmTest, GpsRtcmAssemblerAcceptsMaxSizeUnfragmentedPayload)
+{
+	// GIVEN: A payload that is exactly as large as the assembler's internal
+	// message buffer. This exceeds what MAVLink can carry in one packet, but the
+	// direct helper still needs a checked upper bound.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_MESSAGE_LEN);
+	std::iota(payload.begin(), payload.end(), 0);
+	size_t len = 0;
+
+	// WHEN: The payload is added as a single unfragmented packet.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(false), payload.data(), payload.size(), 10, len);
+
+	// THEN: The assembler accepts it and returns the original bytes unchanged.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that oversized fragmented packets are rejected.
+// WHY: GPS_RTCM_DATA only allows up to 180 bytes per fragment, so larger packets must be dropped.
+TEST_F(RtcmTest, GpsRtcmAssemblerRejectsOversizedFragment)
+{
+	// GIVEN: A fragmented packet that exceeds the protocol fragment size limit.
+	std::vector<uint8_t> oversized_fragment(GPS_RTCM_MAX_FRAGMENT_LEN + 1, 0x5a);
+	size_t len = 123;
+
+	// WHEN: The oversized fragment is passed to the assembler.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 6), oversized_fragment.data(),
+				 oversized_fragment.size(), 10, len);
+
+	// THEN: The fragment is rejected and no payload is returned.
+	EXPECT_EQ(message, nullptr);
+	EXPECT_EQ(len, 0u);
+}
+
+// WHAT: Verify that fragments can arrive out of order and still be reassembled correctly.
+// WHY: The sequence/fragment handling exists to allow for unreliable delivery order.
+TEST_F(RtcmTest, GpsRtcmAssemblerReordersFragments)
+{
+	// GIVEN: A payload that spans three fragments with a non-full final fragment.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN * 2 + 25);
+	std::iota(payload.begin(), payload.end(), 0);
+	const uint8_t flags0 = buildGpsRtcmFlags(true, 0, 7);
+	const uint8_t flags1 = buildGpsRtcmFlags(true, 1, 7);
+	const uint8_t flags2 = buildGpsRtcmFlags(true, 2, 7);
+
+	size_t len = 0;
+
+	// WHEN: The later fragments arrive before fragment 0.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(flags1, &payload[GPS_RTCM_MAX_FRAGMENT_LEN], GPS_RTCM_MAX_FRAGMENT_LEN, 10, len),
+		  nullptr);
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(flags2, &payload[GPS_RTCM_MAX_FRAGMENT_LEN * 2], 25, 20, len), nullptr);
+
+	// WHEN: Fragment 0 finally arrives.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(flags0, payload.data(), GPS_RTCM_MAX_FRAGMENT_LEN, 30, len);
+
+	// THEN: The assembler emits the full payload in the correct order.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that a duplicate fragment with identical content is handled.
+// WHY: Retransmissions should not reset a healthy buffer or corrupt the assembled payload.
+TEST_F(RtcmTest, GpsRtcmAssemblerIgnoresDuplicateMatchingFragment)
+{
+	// GIVEN: A payload that spans one full fragment and one non-full fragment.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN + 12);
+	std::iota(payload.begin(), payload.end(), 0);
+	const uint8_t flags0 = buildGpsRtcmFlags(true, 0, 5);
+	const uint8_t flags1 = buildGpsRtcmFlags(true, 1, 5);
+	size_t len = 0;
+
+	// WHEN: Fragment 0 is received twice with the same content.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(flags0, payload.data(), GPS_RTCM_MAX_FRAGMENT_LEN, 10, len), nullptr);
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(flags0, payload.data(), GPS_RTCM_MAX_FRAGMENT_LEN, 11, len), nullptr);
+
+	// WHEN: The non-full final fragment arrives.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(flags1, &payload[GPS_RTCM_MAX_FRAGMENT_LEN], 12, 20, len);
+
+	// THEN: The assembler still returns the expected full payload once.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+struct GpsRtcmSequenceChangeCase {
+	const char *name;
+	std::vector<uint8_t> old_fragment_ids;
+	size_t expected_flushed_fragments;
+};
+
+class GpsRtcmSequenceChangeTest : public RtcmTest, public ::testing::WithParamInterface<GpsRtcmSequenceChangeCase>
+{
+};
+
+// WHAT: Verify that legacy exact-multiple fragmented messages are sent
+// when every buffered fragment is a full 180-byte prefix starting at fragment 0.
+// WHY: This keeps the receiver compatible with older QGC senders while still
+// rejecting partial buffers that have a missing fragment in the middle.
+TEST_P(GpsRtcmSequenceChangeTest, GpsRtcmAssemblerFlushesOnlyGapFreeFullPrefixOnSequenceChange)
+{
+	const GpsRtcmSequenceChangeCase &param = GetParam();
+	const uint8_t old_sequence_id = 9;
+	const uint8_t new_sequence_id = 10;
+	size_t len = 0;
+	uint64_t timestamp = 10;
+	std::vector<std::vector<uint8_t>> old_fragments(GPS_RTCM_MAX_FRAGMENTS);
+
+	// GIVEN: A buffered old sequence with full-sized fragments as described by
+	// the parameter set, followed by a new sequence that starts with fragment 0.
+	for (const uint8_t fragment_id : param.old_fragment_ids) {
+		old_fragments[fragment_id].resize(GPS_RTCM_MAX_FRAGMENT_LEN);
+		std::iota(old_fragments[fragment_id].begin(), old_fragments[fragment_id].end(),
+			  static_cast<uint8_t>(0x20 + fragment_id * 0x10));
+		EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, fragment_id, old_sequence_id),
+						       old_fragments[fragment_id].data(), old_fragments[fragment_id].size(), timestamp, len),
+			  nullptr);
+		EXPECT_EQ(len, 0u);
+		timestamp += 10;
+	}
+
+	std::vector<uint8_t> expected_old_message;
+
+	for (size_t fragment_id = 0; fragment_id < param.expected_flushed_fragments; fragment_id++) {
+		expected_old_message.insert(expected_old_message.end(), old_fragments[fragment_id].begin(), old_fragments[fragment_id].end());
+	}
+
+	std::vector<uint8_t> new_fragment0(GPS_RTCM_MAX_FRAGMENT_LEN);
+	std::iota(new_fragment0.begin(), new_fragment0.end(), static_cast<uint8_t>(0x70));
+
+	// WHEN: A packet for the next sequence arrives.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, new_sequence_id), new_fragment0.data(),
+				 new_fragment0.size(), timestamp, len);
+
+	// THEN: Only a gap-free run of full 180-byte fragments from fragment 0 is
+	// flushed. Any gap in the buffered old sequence emits nothing.
+	if (expected_old_message.empty()) {
+		EXPECT_EQ(message, nullptr);
+		EXPECT_EQ(len, 0u);
+
+	} else {
+		ASSERT_NE(message, nullptr);
+		EXPECT_EQ(len, expected_old_message.size());
+		EXPECT_EQ(memcmp(message, expected_old_message.data(), expected_old_message.size()), 0);
+	}
+
+	std::vector<uint8_t> new_fragment1(7);
+	std::iota(new_fragment1.begin(), new_fragment1.end(), static_cast<uint8_t>(0x90));
+	message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, new_sequence_id), new_fragment1.data(), new_fragment1.size(),
+					       timestamp + 10, len);
+
+	std::vector<uint8_t> expected_new_message = new_fragment0;
+	expected_new_message.insert(expected_new_message.end(), new_fragment1.begin(), new_fragment1.end());
+
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, expected_new_message.size());
+	EXPECT_EQ(memcmp(message, expected_new_message.data(), expected_new_message.size()), 0);
+}
+
+INSTANTIATE_TEST_SUITE_P(RolloverFlushCases, GpsRtcmSequenceChangeTest,
+			 ::testing::Values(
+				 GpsRtcmSequenceChangeCase {"Single180ByteFragment", {0}, 1},
+				 GpsRtcmSequenceChangeCase {"Two180ByteFragments", {0, 1}, 2},
+				 GpsRtcmSequenceChangeCase {"Three180ByteFragments", {0, 1, 2}, 3},
+				 GpsRtcmSequenceChangeCase {"MissingFragmentZero", {1, 2}, 0},
+				 GpsRtcmSequenceChangeCase {"GapAfterFragmentZero", {0, 2}, 0},
+				 GpsRtcmSequenceChangeCase {"GapBeforeFragmentThree", {0, 1, 3}, 0}
+			 ),
+			 [](const ::testing::TestParamInfo<GpsRtcmSequenceChangeCase> &test_param_info)
+{
+	return test_param_info.param.name;
+});
+
+// WHAT: Verify that the fragmented sequence-change path can emit both the
+// older buffered message and a new single-fragment fragmented message.
+// WHY: A non-full fragment 0 is already a complete fragmented message. This
+// only happens if the sender sets the fragmented bit on a packet that would
+// normally be sent unfragmented, but PX4 still needs to handle it safely.
+TEST_F(RtcmTest, GpsRtcmAssemblerQueuesSingleFragmentMessageAfterSequenceChangeFlush)
+{
+	// GIVEN: An old exact-180-byte fragmented message and a new sequence whose
+	// fragment 0 is non-full, so it is also the final fragment even though a
+	// compliant sender would usually send it as unfragmented.
+	std::vector<uint8_t> old_payload(GPS_RTCM_MAX_FRAGMENT_LEN);
+	std::iota(old_payload.begin(), old_payload.end(), 0);
+	std::vector<uint8_t> new_payload(23);
+	std::iota(new_payload.begin(), new_payload.end(), static_cast<uint8_t>(0x80));
+	size_t len = 0;
+
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 9), old_payload.data(),
+					       old_payload.size(), 10, len), nullptr);
+
+	// WHEN: A new sequence starts with a non-full fragment 0, which completes
+	// the new message immediately while also flushing the old sequence.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 10), new_payload.data(),
+				 new_payload.size(), 20, len);
+
+	// THEN: addPacket() returns the older flushed message first.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, old_payload.size());
+	EXPECT_EQ(memcmp(message, old_payload.data(), old_payload.size()), 0);
+
+	message = gps_rtcm_assembler.takeDeferredMessage(len);
+
+	// THEN: The new short complete message is queued and can be drained
+	// immediately afterwards.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, new_payload.size());
+	EXPECT_EQ(memcmp(message, new_payload.data(), new_payload.size()), 0);
+	EXPECT_EQ(gps_rtcm_assembler.takeDeferredMessage(len), nullptr);
+	EXPECT_EQ(len, 0u);
+}
+
+// WHAT: Verify that the unfragmented path can emit both the older buffered
+// message and the new unfragmented packet from one call.
+// WHY: This covers the separate unfragmented branch, which uses the same
+// compatibility fallback but does not go through fragmented reassembly.
+TEST_F(RtcmTest, GpsRtcmAssemblerQueuesSecondUnfragmentedMessageAfterLegacyFlush)
+{
+	// GIVEN: An old exact-180-byte fragmented message followed by a new
+	// unfragmented packet.
+	std::vector<uint8_t> old_payload(GPS_RTCM_MAX_FRAGMENT_LEN);
+	std::iota(old_payload.begin(), old_payload.end(), 0);
+	std::vector<uint8_t> new_payload(23);
+	std::iota(new_payload.begin(), new_payload.end(), static_cast<uint8_t>(0xa0));
+	size_t len = 0;
+
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 11), old_payload.data(),
+					       old_payload.size(), 10, len), nullptr);
+
+	// WHEN: The next packet arrives without the fragmented flag.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(false, 0, 12), new_payload.data(),
+				 new_payload.size(), 20, len);
+
+	// THEN: addPacket() returns the older flushed message first from
+	// _assembled_message; the new unfragmented payload is deferred.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, old_payload.size());
+	EXPECT_EQ(memcmp(message, old_payload.data(), old_payload.size()), 0);
+
+	message = gps_rtcm_assembler.takeDeferredMessage(len);
+
+	// THEN: The new unfragmented payload is queued and can be drained
+	// immediately afterwards.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, new_payload.size());
+	EXPECT_EQ(memcmp(message, new_payload.data(), new_payload.size()), 0);
+	EXPECT_EQ(gps_rtcm_assembler.takeDeferredMessage(len), nullptr);
+	EXPECT_EQ(len, 0u);
+}
+
+// WHAT: Verify that a spec-compliant sender still completes on the short
+// terminal fragment before any sequence rollover logic is needed.
+// WHY: The receiver-side sequence-change fallback must not change the normal
+// completion path for a standard [180, short] fragmented payload.
+TEST_F(RtcmTest, GpsRtcmAssemblerCompletesOnShortTerminalBeforeSequenceChange)
+{
+	// GIVEN: A payload with one full fragment and one short terminal fragment.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN + 23);
+	std::iota(payload.begin(), payload.end(), 0);
+	size_t len = 0;
+
+	// WHEN: The fragmented payload arrives in order and completes on the short
+	// final fragment.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 3), payload.data(),
+					       GPS_RTCM_MAX_FRAGMENT_LEN, 10, len), nullptr);
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 3),
+				 &payload[GPS_RTCM_MAX_FRAGMENT_LEN], 23, 20, len);
+
+	// THEN: The message completes immediately, without waiting for a later
+	// sequence change.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that four full-sized fragments complete a message explicitly.
+// WHY: The exact 720-byte case must not rely on an accidental fallback path.
+TEST_F(RtcmTest, GpsRtcmAssemblerCompletesOnFourthFullFragment)
+{
+	// GIVEN: A payload that uses all four 180-byte fragment slots.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_MESSAGE_LEN);
+	std::iota(payload.begin(), payload.end(), 0);
+
+	size_t len = 0;
+
+	// WHEN: The first three full fragments arrive.
+	for (uint8_t fragment = 0; fragment < GPS_RTCM_MAX_FRAGMENTS - 1; fragment++) {
+		EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, fragment, 3),
+						       &payload[fragment * GPS_RTCM_MAX_FRAGMENT_LEN], GPS_RTCM_MAX_FRAGMENT_LEN, 10 + fragment, len),
+			  nullptr);
+	}
+
+	// WHEN: The fourth full fragment arrives.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 3, 3),
+				 &payload[3 * GPS_RTCM_MAX_FRAGMENT_LEN], GPS_RTCM_MAX_FRAGMENT_LEN, 20, len);
+
+	// THEN: The assembler emits the completed 720-byte payload.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that an exact multiple of 180 bytes below 720 bytes can be
+// completed with an empty terminating fragment.
+// WHY: Per the MAVLink completion rule, the receiver only knows the message is
+// shorter than 4 fragments after the first non-full fragment arrives.
+TEST_F(RtcmTest, GpsRtcmAssemblerCompletesOnZeroLengthTerminatingFragment)
+{
+	// GIVEN: A 360-byte payload split into two full fragments.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN * 2);
+	std::iota(payload.begin(), payload.end(), 0);
+
+	size_t len = 0;
+
+	// WHEN: The two full fragments arrive.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 11), payload.data(),
+					       GPS_RTCM_MAX_FRAGMENT_LEN, 10, len), nullptr);
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 11),
+					       payload.data() + GPS_RTCM_MAX_FRAGMENT_LEN, GPS_RTCM_MAX_FRAGMENT_LEN, 20, len), nullptr);
+
+	// WHEN: An empty fragment marks the end of the MAVLink fragmented payload.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 2, 11),
+				 payload.data() + payload.size(), 0, 30, len);
+
+	// THEN: The assembler returns the original 360-byte payload.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that a stale partial buffer is dropped after the timeout.
+// WHY: This prevents an old fragment from being mixed into a later message with the same sequence ID.
+TEST_F(RtcmTest, GpsRtcmAssemblerDropsStalePartialMessage)
+{
+	// GIVEN: A payload that spans one full fragment and one non-full fragment.
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN + 15, 0x44);
+
+	size_t len = 0;
+
+	// WHEN: Fragment 0 is followed by a late fragment 1 after the timeout window.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 4), payload.data(), GPS_RTCM_MAX_FRAGMENT_LEN, 10,
+					       len), nullptr);
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 4),
+					       &payload[GPS_RTCM_MAX_FRAGMENT_LEN], 15, 10 + GPS_RTCM_FRAGMENT_TIMEOUT_US + 1, len),
+		  nullptr);
+
+	// WHEN: The matching fragment 0 arrives after the timeout reset.
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 4), payload.data(),
+				 GPS_RTCM_MAX_FRAGMENT_LEN, 10 + GPS_RTCM_FRAGMENT_TIMEOUT_US + 2, len);
+
+	// THEN: The fresh out-of-order pair is reconstructed without reusing the stale pre-timeout state.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that a conflicting fragment slot restarts assembly from the
+// newest packet.
+// WHY: The sequence ID only identifies one active buffer. If a slot is reused
+// with different bytes, the old partial state is no longer trustworthy.
+TEST_F(RtcmTest, GpsRtcmAssemblerRestartsOnFragmentContentMismatch)
+{
+	// GIVEN: Two different payloads that reuse the same fragment slot under the
+	// same sequence ID.
+	std::vector<uint8_t> old_fragment(GPS_RTCM_MAX_FRAGMENT_LEN, 0x55);
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN * 2 + 12);
+	std::iota(payload.begin(), payload.end(), 0);
+	size_t len = 0;
+
+	// WHEN: Fragment 1 is first buffered with old content, then replaced with
+	// new content for the same sequence ID.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 9), old_fragment.data(),
+					       old_fragment.size(), 10, len), nullptr);
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 9),
+					       &payload[GPS_RTCM_MAX_FRAGMENT_LEN], GPS_RTCM_MAX_FRAGMENT_LEN, 20, len), nullptr);
+
+	// WHEN: The remaining fragments for the new buffer arrive.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 9), payload.data(),
+					       GPS_RTCM_MAX_FRAGMENT_LEN, 30, len), nullptr);
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 2, 9),
+				 &payload[GPS_RTCM_MAX_FRAGMENT_LEN * 2], 12, 40, len);
+
+	// THEN: The assembler returns the newer payload, not the abandoned partial buffer.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
+
+// WHAT: Verify that a non-full fragment cannot appear before an already buffered higher fragment.
+// WHY: Once a fragment claims to be the first non-full fragment, any stored
+// higher fragment makes the buffered message internally inconsistent and the
+// buffer must be dropped.
+TEST_F(RtcmTest, GpsRtcmAssemblerRejectsNonFullFragmentBeforeBufferedHigherFragment)
+{
+	// GIVEN: An out-of-order higher fragment and a later valid two-fragment message with the same sequence ID.
+	std::vector<uint8_t> garbage_fragment(GPS_RTCM_MAX_FRAGMENT_LEN, 0x77);
+	std::vector<uint8_t> payload(GPS_RTCM_MAX_FRAGMENT_LEN + 16, 0x88);
+	size_t len = 0;
+
+	// WHEN: A higher fragment is buffered first.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 2, 10), garbage_fragment.data(),
+					       garbage_fragment.size(), 10, len), nullptr);
+
+	// WHEN: A non-full lower fragment arrives and would incorrectly claim the message already ended.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 10), &payload[GPS_RTCM_MAX_FRAGMENT_LEN], 16, 20,
+					       len), nullptr);
+
+	// WHEN: A clean message is sent afterwards with the same sequence ID.
+	EXPECT_EQ(gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 0, 10), payload.data(),
+					       GPS_RTCM_MAX_FRAGMENT_LEN, 30, len), nullptr);
+	const uint8_t *message = gps_rtcm_assembler.addPacket(buildGpsRtcmFlags(true, 1, 10),
+				 &payload[GPS_RTCM_MAX_FRAGMENT_LEN], 16, 40, len);
+
+	// THEN: The inconsistent partial buffer was discarded and the clean message reconstructs normally.
+	ASSERT_NE(message, nullptr);
+	EXPECT_EQ(len, payload.size());
+	EXPECT_EQ(memcmp(message, payload.data(), payload.size()), 0);
+}
--- a/src/lib/gnss/test/RtcmTestCommon.hpp
+++ b/src/lib/gnss/test/RtcmTestCommon.hpp
@@ -37,6 +37,7 @@
 #include "rtcm.h"
 #include <vector>
 #include <cstring>
+#include <numeric>
 #include <random>

 using namespace gnss;
@@ -45,6 +46,15 @@ class RtcmTest : public ::testing::Test
 {
 protected:
 	Rtcm3Parser parser;
+	GpsRtcmMessageAssembler gps_rtcm_assembler;
+
+	uint8_t buildGpsRtcmFlags(bool fragmented, uint8_t fragment_id = 0, uint8_t sequence_id = 0)
+	{
+		uint8_t flags = fragmented ? GPS_RTCM_FLAG_FRAGMENTED : 0;
+		flags |= (fragment_id & GPS_RTCM_FLAG_FRAGMENT_ID_MASK) << GPS_RTCM_FLAG_FRAGMENT_ID_SHIFT;
+		flags |= (sequence_id & GPS_RTCM_FLAG_SEQUENCE_ID_MASK) << GPS_RTCM_FLAG_SEQUENCE_ID_SHIFT;
+		return flags;
+	}

 	// Helper to build a valid RTCM3 frame with proper CRC
 	std::vector<uint8_t> buildValidFrame(uint16_t msg_type, const std::vector<uint8_t> &payload_data)
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -136,6 +136,7 @@ px4_add_module(
 		drivers_gyroscope
 		drivers_magnetometer
 		conversion
+		gnss
 		sensor_calibration
 		geo
 		mavlink_c
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -2784,18 +2784,53 @@ 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);

+	// Drop packets with an invalid payload length
+	if (gps_rtcm_data_msg.len > sizeof(gps_rtcm_data_msg.data)) {
+		return;
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+	const size_t packet_len = static_cast<size_t>(gps_rtcm_data_msg.len);
+	size_t message_len = 0;
+
+	const uint8_t *message = _gps_rtcm_message_assembler.addPacket(gps_rtcm_data_msg.flags, gps_rtcm_data_msg.data,
+				 packet_len, now, message_len);
+
+	if (message != nullptr) {
+		publish_gps_inject_data(message, message_len);
+
+		// addPacket() can queue at most one deferred message.
+		const uint8_t *deferred_message = _gps_rtcm_message_assembler.takeDeferredMessage(message_len);
+
+		if (deferred_message != nullptr) {
+			publish_gps_inject_data(deferred_message, message_len);
+		}
+	}
+}
+
+void
+MavlinkReceiver::publish_gps_inject_data(const uint8_t *data, size_t len)
+{
 	gps_inject_data_s gps_inject_data_topic{};
+	constexpr uint8_t gps_inject_data_flag_fragmented = 1;

-	gps_inject_data_topic.timestamp = hrt_absolute_time();
+	const size_t capacity = sizeof(gps_inject_data_topic.data);
+	// gps_inject_data only carries the transport-level fragmented bit. The
+	// MAVLink fragment/sequence bits are consumed by the assembler above.
+	gps_inject_data_topic.flags = (len > capacity) ? gps_inject_data_flag_fragmented : 0;

-	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));
+	size_t written = 0;

-	gps_inject_data_topic.timestamp = hrt_absolute_time();
-	_gps_inject_data_pub.publish(gps_inject_data_topic);
+	// gps_inject_data transports RTCM in 300-byte uORB chunks, so a fully
+	// reassembled RTCM frame may still require multiple publications.
+	while (written < len) {
+		const size_t chunk_len = math::min(len - written, capacity);
+		gps_inject_data_topic.timestamp = hrt_absolute_time();
+		gps_inject_data_topic.len = static_cast<decltype(gps_inject_data_topic.len)>(chunk_len);
+		memcpy(gps_inject_data_topic.data, &data[written], chunk_len);
+		_gps_inject_data_pub.publish(gps_inject_data_topic);
+		written += chunk_len;
+	}
 }

 void
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -51,6 +51,7 @@
 #include "mavlink_timesync.h"
 #include "tune_publisher.h"

+#include <lib/gnss/rtcm.h>
 #include <geo/geo.h>
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@@ -257,6 +258,7 @@ private:
 	void update_rx_stats(const mavlink_message_t &message);

 	void publish_hil_battery();
+	void publish_gps_inject_data(const uint8_t *data, size_t len);

 	px4::atomic_bool 	_should_exit{false};
 	pthread_t		_thread {};
@@ -369,6 +371,7 @@ private:
 	uORB::PublicationMulti<sensor_baro_s>			_sensor_baro_pub{ORB_ID(sensor_baro)};
 	uORB::PublicationMulti<sensor_gps_s>			_sensor_gps_pub{ORB_ID(sensor_gps)};
 	uORB::PublicationMulti<sensor_optical_flow_s>           _sensor_optical_flow_pub{ORB_ID(sensor_optical_flow)};
+	gnss::GpsRtcmMessageAssembler				_gps_rtcm_message_assembler {};

 	// ORB publications (queue length > 1)
 	uORB::Publication<transponder_report_s>  _transponder_report_pub{ORB_ID(transponder_report)};