lib: gnss: add RTCM parsing library (#26093)

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

* lib: gnss: rtcm: use rtcm3_payload_length()

* lib: gnss: rtcm: set header year

* lib: gnss: rtcm: add units tests

* Update src/lib/gnss/rtcm.h

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update src/lib/gnss/CMakeLists.txt

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* Update src/lib/gnss/rtcm.h

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

* remove mention of reset()

* lib: gnss: rtcm: more effecient preamble search

---------

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>

src/lib/CMakeLists.txt

@@ -50,6 +50,7 @@ add_subdirectory(dataman_client EXCLUDE_FROM_ALL)
 add_subdirectory(drivers EXCLUDE_FROM_ALL)
 add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL)
 add_subdirectory(geo EXCLUDE_FROM_ALL)
+add_subdirectory(gnss EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
 add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)

src/lib/gnss/CMakeLists.txt

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(gnss rtcm.cpp)
+
+target_include_directories(gnss PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+if(BUILD_TESTING)
+	add_subdirectory(test)
+endif()

src/lib/gnss/rtcm.cpp

@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rtcm.cpp
+ *
+ * RTCM3 protocol parsing implementation.
+ */
+
+#include "rtcm.h"
+#include <cstring>
+
+namespace gnss
+{
+
+uint32_t rtcm3_crc24q(const uint8_t *data, size_t len)
+{
+	uint32_t crc = 0;
+
+	for (size_t i = 0; i < len; i++) {
+		crc ^= static_cast<uint32_t>(data[i]) << 16;
+
+		for (int j = 0; j < 8; j++) {
+			crc <<= 1;
+
+			if (crc & 0x1000000) {
+				crc ^= RTCM3_CRC24Q_POLY;
+			}
+		}
+	}
+
+	return crc & 0xFFFFFF;
+}
+
+size_t Rtcm3Parser::addData(const uint8_t *data, size_t len)
+{
+	size_t space_available = BUFFER_SIZE - _buffer_len;
+	size_t to_copy = (len < space_available) ? len : space_available;
+
+	if (to_copy > 0) {
+		memcpy(&_buffer[_buffer_len], data, to_copy);
+		_buffer_len += to_copy;
+	}
+
+	return to_copy;
+}
+
+const uint8_t *Rtcm3Parser::getNextMessage(size_t *out_len)
+{
+	while (_buffer_len > 0) {
+		int to_drop = 0;
+
+		// Find preamble
+		for (size_t i = 0; i < _buffer_len; i++) {
+			if (_buffer[i] == RTCM3_PREAMBLE) {
+				break;
+			}
+
+			to_drop++;
+		}
+
+		// Drop everything not being the preamble
+		if (to_drop > 0) {
+			_bytes_discarded += to_drop;
+			discardBytes(to_drop);
+		}
+
+		// Need at least header to check length
+		if (_buffer_len < RTCM3_HEADER_LEN) {
+			return nullptr;
+		}
+
+		size_t payload_len = rtcm3_payload_length(_buffer);
+
+		if (payload_len > RTCM3_MAX_PAYLOAD_LEN) {
+			// Invalid length - not a valid frame, discard preamble
+			_bytes_discarded++;
+			discardBytes(1);
+			continue;
+		}
+
+		size_t frame_len = RTCM3_HEADER_LEN + payload_len + RTCM3_CRC_LEN;
+
+		// Check if we have the complete frame
+		if (_buffer_len < frame_len) {
+			return nullptr;
+		}
+
+		uint32_t calculated_crc = rtcm3_crc24q(_buffer, RTCM3_HEADER_LEN + payload_len);
+		uint32_t received_crc = (static_cast<uint32_t>(_buffer[frame_len - 3]) << 16) |
+					(static_cast<uint32_t>(_buffer[frame_len - 2]) << 8) |
+					_buffer[frame_len - 1];
+
+		if (calculated_crc != received_crc) {
+			_crc_errors++;
+			discardBytes(1);
+			continue;
+		}
+
+		*out_len = frame_len;
+		return _buffer;
+	}
+
+	return nullptr;
+}
+
+void Rtcm3Parser::consumeMessage(size_t len)
+{
+	discardBytes(len);
+	_messages_parsed++;
+	_total_frame_bytes += len;
+}
+
+void Rtcm3Parser::discardBytes(size_t count)
+{
+	if (count >= _buffer_len) {
+		_buffer_len = 0;
+
+	} else {
+		memmove(_buffer, &_buffer[count], _buffer_len - count);
+		_buffer_len -= count;
+	}
+}
+
+} // namespace gnss

src/lib/gnss/rtcm.h

@@ -0,0 +1,202 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rtcm.h
+ *
+ * RTCM3 protocol definitions and parsing utilities.
+ *
+ * RTCM3 frame structure:
+ *   Byte 0:     Preamble (0xD3)
+ *   Byte 1-2:   6 reserved bits (0) + 10-bit payload length
+ *   Byte 3..N:  Payload (0-1023 bytes)
+ *   Last 3:     CRC-24Q checksum
+ *
+ * Total frame size: 3 (header) + payload_length + 3 (CRC) = 6 + payload_length
+ * Maximum frame size: 6 + 1023 = 1029 bytes
+ */
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+namespace gnss
+{
+
+// RTCM3 protocol constants
+static constexpr uint8_t  RTCM3_PREAMBLE        = 0xD3;
+static constexpr size_t   RTCM3_HEADER_LEN      = 3;   // Preamble + 2 bytes reserved/length
+static constexpr size_t   RTCM3_CRC_LEN         = 3;
+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;
+
+/**
+ * Calculate CRC-24Q checksum for RTCM3 messages.
+ *
+ * @param data   Pointer to data buffer
+ * @param len    Length of data
+ * @return       24-bit CRC value
+ */
+uint32_t rtcm3_crc24q(const uint8_t *data, size_t len);
+
+/**
+ * Extract RTCM3 message type from a frame buffer.
+ * The message type is the first 12 bits of the payload.
+ *
+ * @param frame  Pointer to complete RTCM3 frame (starting with preamble)
+ * @return       Message type ID (0 if frame is too short)
+ */
+inline uint16_t rtcm3_message_type(const uint8_t *frame)
+{
+	// Message type is first 12 bits of payload (bytes 3-4)
+	return (static_cast<uint16_t>(frame[3]) << 4) | (frame[4] >> 4);
+}
+
+/**
+ * Extract payload length from RTCM3 frame header.
+ *
+ * @param frame  Pointer to at least 3 bytes of RTCM3 frame header
+ * @return       Payload length (0-1023)
+ */
+inline size_t rtcm3_payload_length(const uint8_t *frame)
+{
+	return ((static_cast<size_t>(frame[1]) & 0x03) << 8) | frame[2];
+}
+
+/**
+ * RTCM3 parser statistics.
+ */
+struct Rtcm3ParserStats {
+	uint32_t messages_parsed;   ///< Messages successfully parsed and consumed
+	uint32_t crc_errors;        ///< Messages with CRC failures
+	uint32_t bytes_discarded;   ///< Bytes discarded while searching for valid frames
+	uint32_t total_frame_bytes; ///< Total bytes in successfully parsed frames
+};
+
+/**
+ * RTCM3 frame parser for detecting message boundaries in a byte stream.
+ *
+ * This parser is designed for scenarios where RTCM data arrives in arbitrary
+ * chunks (e.g., from uORB messages, serial ports) and needs to be reassembled
+ * into complete, CRC-validated RTCM messages.
+ *
+ * Usage:
+ *   Rtcm3Parser parser;
+ *   parser.addData(chunk1, len1);
+ *   parser.addData(chunk2, len2);
+ *
+ *   size_t frame_len;
+ *   const uint8_t *frame;
+ *   while ((frame = parser.getNextMessage(&frame_len)) != nullptr) {
+ *       // Process complete RTCM message
+ *       uint16_t msg_type = rtcm3_message_type(frame);
+ *       // ... use the frame ...
+ *       parser.consumeMessage(frame_len);
+ *   }
+ */
+class Rtcm3Parser
+{
+public:
+	// Buffer size: enough for 2 max-size messages to handle overlap
+	static constexpr size_t BUFFER_SIZE = RTCM3_MAX_FRAME_LEN * 2;
+
+	Rtcm3Parser() = default;
+
+	/**
+	 * Add data to the parser buffer.
+	 *
+	 * @param data Pointer to incoming data
+	 * @param len  Number of bytes to add
+	 * @return     Number of bytes actually added (may be less if buffer is full)
+	 */
+	size_t addData(const uint8_t *data, size_t len);
+
+	/**
+	 * Get a pointer to the next complete RTCM3 message without consuming it.
+	 *
+	 * Returns a pointer directly into the parser's internal buffer where the
+	 * valid frame starts. Invalid bytes at the buffer start are discarded
+	 * during the search. The returned pointer remains valid until the next
+	 * call to addData() or consumeMessage().
+	 *
+	 * After processing the message, call consumeMessage() to remove it from
+	 * the buffer.
+	 *
+	 * @param out_len    Set to the total frame length
+	 * @return           Pointer to the frame in internal buffer, or nullptr
+	 *                   if no complete valid frame is available
+	 */
+	const uint8_t *getNextMessage(size_t *out_len);
+
+	/**
+	 * Consume (remove) the next message from the buffer.
+	 *
+	 * Call this after successfully processing a message obtained via
+	 * getNextMessage(). The length should match what getNextMessage returned.
+	 *
+	 * @param len   Number of bytes to remove from the buffer
+	 */
+	void consumeMessage(size_t len);
+
+	/**
+	 * Get the number of bytes currently buffered.
+	 */
+	size_t bufferedBytes() const { return _buffer_len; }
+
+	/**
+	 * Get parser statistics.
+	 */
+	Rtcm3ParserStats getStats() const
+	{
+		return {_messages_parsed, _crc_errors, _bytes_discarded, _total_frame_bytes};
+	}
+
+private:
+	/**
+	 * Remove bytes from the beginning of the buffer.
+	 */
+	void discardBytes(size_t count);
+
+	uint8_t  _buffer[BUFFER_SIZE] {};
+	size_t   _buffer_len {0};
+
+	// Statistics
+	uint32_t _messages_parsed {0};
+	uint32_t _crc_errors {0};
+	uint32_t _bytes_discarded {0};
+	uint32_t _total_frame_bytes {0};
+};
+
+} // namespace gnss

src/lib/gnss/test/CMakeLists.txt

@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_unit_gtest(SRC RtcmCrcTest.cpp LINKLIBS gnss)
+px4_add_unit_gtest(SRC RtcmParserTest.cpp LINKLIBS gnss)
+px4_add_unit_gtest(SRC RtcmBustedSenderTest.cpp LINKLIBS gnss)
+px4_add_unit_gtest(SRC RtcmStressTest.cpp LINKLIBS gnss)

src/lib/gnss/test/README.md

@@ -0,0 +1,57 @@
+# RTCM Library Tests
+
+Unit tests for the RTCM3 parser library.
+
+## Running Tests
+
+```bash
+# Build and test
+make tests TESTFILTER=Rtcm
+```
+
+## Test Descriptions
+
+### RtcmCrcTest.cpp (2 tests)
+
+| Test | Description |
+|------|-------------|
+| `CRC24Q_EmptyData` | Verifies CRC function handles empty/null input without crashing |
+| `CRC24Q_KnownValue` | Verifies CRC produces a valid 24-bit value |
+
+### RtcmParserTest.cpp (11 tests)
+
+| Test | Description |
+|------|-------------|
+| `PayloadLength_MasksReservedBits` | Verifies reserved bits are ignored when extracting length |
+| `MessageType_Extraction` | Verifies 12-bit message type is correctly extracted from payload |
+| `SingleMinimalFrame` | Parses smallest valid frame (2-byte payload) |
+| `SingleMaximalFrame` | Parses largest valid frame (1023-byte payload) |
+| `MultipleFramesInSequence` | Parses 3 back-to-back valid frames |
+| `FrameArrivesInChunks` | Parses a frame arriving one byte at a time |
+| `GarbageBeforeValidFrame` | Skips 50 bytes of garbage, then parses valid frame |
+| `ValidFrameBadFrameValid` | Verifies recovery after CRC error between valid frames |
+| `BufferOverflowProtection` | Verifies buffer rejects data beyond capacity |
+| `EmptyPayloadFrame` | Parses frame with 0-byte payload |
+| `GetNextOnEmptyBuffer` | Verifies empty buffer returns nullptr |
+
+### RtcmBustedSenderTest.cpp (5 tests)
+
+Tests for handling completely invalid data streams.
+
+| Test | Description |
+|------|-------------|
+| `BustedSender_AllZeros` | 1000 bytes of 0x00 - all discarded |
+| `BustedSender_AllPreambles` | 500 bytes of 0xD3 - parser waits for incomplete frame |
+| `BustedSender_RandomNoise` | 500 random bytes - most discarded or cause CRC errors |
+| `BustedSender_ValidHeaderBadCrc` | Well-formed frame with wrong CRC |
+| `BustedSender_RepeatedBadCrcFrames` | 100 consecutive bad-CRC frames |
+
+### RtcmStressTest.cpp (3 tests)
+
+Performance and stress tests.
+
+| Test | Description |
+|------|-------------|
+| `Stress_ManySmallValidFrames` | 1000 valid frames - verifies no data loss |
+| `Stress_LongGarbageStreamThenValid` | 10KB garbage then 1 valid frame - exercises `discardBytes()` |
+| `Stress_InterleavedGarbageAndValid` | 100 cycles of garbage + valid frame - simulates flaky sender |

src/lib/gnss/test/RtcmBustedSenderTest.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RtcmTestCommon.hpp"
+
+TEST_F(RtcmTest, BustedSender_AllZeros)
+{
+	std::vector<uint8_t> garbage(1000, 0x00);
+	parser.addData(garbage.data(), garbage.size());
+
+	size_t len;
+	EXPECT_EQ(parser.getNextMessage(&len), nullptr);
+	EXPECT_EQ(parser.getStats().bytes_discarded, 1000u);
+	EXPECT_EQ(parser.bufferedBytes(), 0u);
+}
+
+TEST_F(RtcmTest, BustedSender_AllPreambles)
+{
+	// All preamble bytes (0xD3). The parser sees each 0xD3 as a potential frame start.
+	// Length extracted from 0xD3,0xD3 is ((0x03)<<8)|0xD3 = 979 bytes.
+	// Parser waits for full frame which never comes.
+	std::vector<uint8_t> preambles(500, RTCM3_PREAMBLE);
+	parser.addData(preambles.data(), preambles.size());
+
+	size_t len;
+	// Parser should be stuck waiting for more data (incomplete frame)
+	EXPECT_EQ(parser.getNextMessage(&len), nullptr);
+	// Buffer retains data waiting for frame completion
+	EXPECT_GT(parser.bufferedBytes(), 0u);
+}
+
+TEST_F(RtcmTest, BustedSender_RandomNoise)
+{
+	std::mt19937 rng(12345);
+	uint8_t noise[500];
+
+	for (size_t i = 0; i < sizeof(noise); i++) {
+		noise[i] = rng() & 0xFF;
+	}
+
+	parser.addData(noise, sizeof(noise));
+
+	size_t len;
+
+	// Consume any messages that might accidentally form (unlikely but possible)
+	while (parser.getNextMessage(&len) != nullptr) {
+		parser.consumeMessage(len);
+	}
+
+	// Most data should be discarded
+	auto stats = parser.getStats();
+	EXPECT_GT(stats.bytes_discarded + stats.crc_errors, 0u);
+}
+
+TEST_F(RtcmTest, BustedSender_ValidHeaderBadCrc)
+{
+	// Frame with valid header but bad CRC
+	std::vector<uint8_t> frame = {
+		RTCM3_PREAMBLE, 0x00, 0x04,  // Header: 4 byte payload
+		0x01, 0x02, 0x03, 0x04,      // Payload
+		0xDE, 0xAD, 0xBE             // Bad CRC
+	};
+
+	parser.addData(frame.data(), frame.size());
+
+	size_t len;
+	EXPECT_EQ(parser.getNextMessage(&len), nullptr);
+	EXPECT_EQ(parser.getStats().crc_errors, 1u);
+}
+
+TEST_F(RtcmTest, BustedSender_RepeatedBadCrcFrames)
+{
+	for (int i = 0; i < 100; i++) {
+		auto bad = buildBadCrcFrame({static_cast<uint8_t>(i), 0x00});
+		parser.addData(bad.data(), bad.size());
+	}
+
+	size_t len;
+
+	while (parser.getNextMessage(&len) != nullptr) {
+		parser.consumeMessage(len);
+	}
+
+	auto stats = parser.getStats();
+	EXPECT_EQ(stats.messages_parsed, 0u);
+	// At least 100 CRC errors (may be more due to re-scanning after failures)
+	EXPECT_GE(stats.crc_errors, 100u);
+}

src/lib/gnss/test/RtcmCrcTest.cpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RtcmTestCommon.hpp"
+
+TEST_F(RtcmTest, CRC24Q_EmptyData)
+{
+	uint32_t crc = rtcm3_crc24q(nullptr, 0);
+	EXPECT_EQ(crc, 0u);
+}
+
+TEST_F(RtcmTest, CRC24Q_KnownValue)
+{
+	// Test with known RTCM data
+	uint8_t data[] = {0xD3, 0x00, 0x04, 0x01, 0x02, 0x03, 0x04};
+	uint32_t crc = rtcm3_crc24q(data, sizeof(data));
+	// CRC should be a 24-bit value
+	EXPECT_EQ(crc & 0xFF000000, 0u);
+	EXPECT_NE(crc, 0u);
+}

src/lib/gnss/test/RtcmParserTest.cpp

@@ -0,0 +1,208 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RtcmParserTest.cpp
+ *
+ * Tests for RTCM3 parser with valid and partially valid input.
+ * Consolidates: GoodSender, PartialSender, Helper, Buffer, Stats, EdgeCase tests.
+ */
+
+#include "RtcmTestCommon.hpp"
+
+// =============================================================================
+// Helper function tests
+// =============================================================================
+
+TEST_F(RtcmTest, PayloadLength_MasksReservedBits)
+{
+	// Reserved bits set to 1, length should still be extracted correctly
+	uint8_t frame[] = {RTCM3_PREAMBLE, 0xFF, 0xFF};  // Reserved=0x3F, Length=1023
+	EXPECT_EQ(rtcm3_payload_length(frame), 1023u);
+}
+
+TEST_F(RtcmTest, MessageType_Extraction)
+{
+	auto frame = buildValidFrame(1005, {});
+	EXPECT_EQ(rtcm3_message_type(frame.data()), 1005u);
+}
+
+// =============================================================================
+// Good sender tests - valid frames, happy path
+// =============================================================================
+
+TEST_F(RtcmTest, SingleMinimalFrame)
+{
+	auto frame = buildRawFrame({0x00, 0x00});
+	parser.addData(frame.data(), frame.size());
+
+	size_t len = 0;
+	const uint8_t *msg = parser.getNextMessage(&len);
+
+	ASSERT_NE(msg, nullptr);
+	EXPECT_EQ(len, frame.size());
+
+	parser.consumeMessage(len);
+	auto stats = parser.getStats();
+	EXPECT_EQ(stats.messages_parsed, 1u);
+	EXPECT_EQ(stats.crc_errors, 0u);
+}
+
+TEST_F(RtcmTest, SingleMaximalFrame)
+{
+	std::vector<uint8_t> payload(RTCM3_MAX_PAYLOAD_LEN, 0xAA);
+	auto frame = buildRawFrame(payload);
+	parser.addData(frame.data(), frame.size());
+
+	size_t len = 0;
+	const uint8_t *msg = parser.getNextMessage(&len);
+
+	ASSERT_NE(msg, nullptr);
+	EXPECT_EQ(len, RTCM3_MAX_FRAME_LEN);
+}
+
+TEST_F(RtcmTest, MultipleFramesInSequence)
+{
+	auto frame1 = buildValidFrame(1005, {0x01, 0x02, 0x03});
+	auto frame2 = buildValidFrame(1077, {0x04, 0x05, 0x06, 0x07});
+	auto frame3 = buildValidFrame(1087, {0x08});
+
+	parser.addData(frame1.data(), frame1.size());
+	parser.addData(frame2.data(), frame2.size());
+	parser.addData(frame3.data(), frame3.size());
+
+	size_t len;
+	int count = 0;
+
+	while (parser.getNextMessage(&len) != nullptr) {
+		parser.consumeMessage(len);
+		count++;
+	}
+
+	EXPECT_EQ(count, 3);
+}
+
+TEST_F(RtcmTest, FrameArrivesInChunks)
+{
+	auto frame = buildValidFrame(1005, {0x01, 0x02, 0x03, 0x04, 0x05});
+
+	// Feed one byte at a time
+	for (size_t i = 0; i < frame.size() - 1; i++) {
+		parser.addData(&frame[i], 1);
+		size_t len;
+		EXPECT_EQ(parser.getNextMessage(&len), nullptr);
+	}
+
+	parser.addData(&frame[frame.size() - 1], 1);
+
+	size_t len;
+	ASSERT_NE(parser.getNextMessage(&len), nullptr);
+	EXPECT_EQ(len, frame.size());
+}
+
+// =============================================================================
+// Partially good sender tests - mix of valid and invalid
+// =============================================================================
+
+TEST_F(RtcmTest, GarbageBeforeValidFrame)
+{
+	std::vector<uint8_t> data;
+
+	// 50 bytes of garbage
+	for (int i = 0; i < 50; i++) {
+		data.push_back(0x00 + i);
+	}
+
+	auto frame = buildValidFrame(1005, {0x01, 0x02});
+	data.insert(data.end(), frame.begin(), frame.end());
+
+	parser.addData(data.data(), data.size());
+
+	size_t len;
+	const uint8_t *msg = parser.getNextMessage(&len);
+	ASSERT_NE(msg, nullptr);
+	EXPECT_EQ(rtcm3_message_type(msg), 1005u);
+
+	parser.consumeMessage(len);
+	EXPECT_EQ(parser.getStats().bytes_discarded, 50u);
+}
+
+TEST_F(RtcmTest, ValidFrameBadFrameValid)
+{
+	auto frame1 = buildValidFrame(1005, {0x01});
+	auto bad_frame = buildBadCrcFrame({0x02, 0x03});
+	auto frame2 = buildValidFrame(1077, {0x04});
+
+	parser.addData(frame1.data(), frame1.size());
+	parser.addData(bad_frame.data(), bad_frame.size());
+	parser.addData(frame2.data(), frame2.size());
+
+	size_t len;
+	int valid_count = 0;
+
+	while (parser.getNextMessage(&len) != nullptr) {
+		parser.consumeMessage(len);
+		valid_count++;
+	}
+
+	EXPECT_EQ(valid_count, 2);
+	EXPECT_EQ(parser.getStats().crc_errors, 1u);
+}
+
+// =============================================================================
+// Buffer and edge case tests
+// =============================================================================
+
+TEST_F(RtcmTest, BufferOverflowProtection)
+{
+	std::vector<uint8_t> large_data(Rtcm3Parser::BUFFER_SIZE + 1000, 0xAA);
+	size_t added = parser.addData(large_data.data(), large_data.size());
+
+	EXPECT_EQ(added, Rtcm3Parser::BUFFER_SIZE);
+}
+
+TEST_F(RtcmTest, EmptyPayloadFrame)
+{
+	auto frame = buildRawFrame({});
+	parser.addData(frame.data(), frame.size());
+
+	size_t len;
+	ASSERT_NE(parser.getNextMessage(&len), nullptr);
+	EXPECT_EQ(len, RTCM3_HEADER_LEN + RTCM3_CRC_LEN);
+}
+
+TEST_F(RtcmTest, GetNextOnEmptyBuffer)
+{
+	size_t len;
+	EXPECT_EQ(parser.getNextMessage(&len), nullptr);
+}

src/lib/gnss/test/RtcmStressTest.cpp

@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RtcmTestCommon.hpp"
+
+TEST_F(RtcmTest, Stress_ManySmallValidFrames)
+{
+	// Simulate a good sender sending many small messages
+	// Process as we go to avoid buffer overflow
+	const int num_frames = 1000;
+	int count = 0;
+
+	for (int i = 0; i < num_frames; i++) {
+		auto frame = buildValidFrame(1005 + (i % 100), {static_cast<uint8_t>(i & 0xFF)});
+		parser.addData(frame.data(), frame.size());
+
+		// Consume available messages after each add
+		size_t len;
+
+		while (parser.getNextMessage(&len) != nullptr) {
+			parser.consumeMessage(len);
+			count++;
+		}
+	}
+
+	EXPECT_EQ(count, num_frames);
+	auto stats = parser.getStats();
+	EXPECT_EQ(stats.messages_parsed, static_cast<uint32_t>(num_frames));
+	EXPECT_EQ(stats.bytes_discarded, 0u);
+	EXPECT_EQ(stats.crc_errors, 0u);
+}
+
+TEST_F(RtcmTest, Stress_LongGarbageStreamThenValid)
+{
+	// This tests the pathological case Alex mentioned: lots of invalid data
+	// causing many memmove operations during preamble search
+	const size_t garbage_size = 10000;
+	std::vector<uint8_t> garbage(garbage_size);
+	std::mt19937 rng(99);
+
+	for (auto &byte : garbage) {
+		// Avoid preamble to ensure maximum discards
+		byte = (rng() % 0xD2) + 1;  // 0x01 to 0xD2, never 0xD3
+	}
+
+	// Add in chunks to simulate streaming
+	size_t offset = 0;
+
+	while (offset < garbage.size()) {
+		size_t chunk = std::min<size_t>(256, garbage.size() - offset);
+		parser.addData(&garbage[offset], chunk);
+
+		// Try to parse (forces discarding)
+		size_t len;
+		parser.getNextMessage(&len);
+		offset += chunk;
+	}
+
+	// Now add a valid frame
+	auto valid = buildValidFrame(1005, {0x01, 0x02, 0x03});
+	parser.addData(valid.data(), valid.size());
+
+	size_t len;
+	const uint8_t *msg = parser.getNextMessage(&len);
+	ASSERT_NE(msg, nullptr);
+
+	parser.consumeMessage(len);
+	auto stats = parser.getStats();
+	EXPECT_EQ(stats.messages_parsed, 1u);
+	EXPECT_EQ(stats.bytes_discarded, garbage_size);
+}
+
+TEST_F(RtcmTest, Stress_InterleavedGarbageAndValid)
+{
+	// Simulate a flaky sender that mixes garbage with valid frames
+	const int num_valid = 100;
+	int parsed = 0;
+
+	for (int i = 0; i < num_valid; i++) {
+		// Add some garbage (variable amount)
+		std::vector<uint8_t> garbage(10 + (i % 50), 0xAA);
+		parser.addData(garbage.data(), garbage.size());
+
+		// Add valid frame
+		auto frame = buildValidFrame(1005, {static_cast<uint8_t>(i)});
+		parser.addData(frame.data(), frame.size());
+
+		// Parse what we can
+		size_t len;
+
+		while (parser.getNextMessage(&len) != nullptr) {
+			parser.consumeMessage(len);
+			parsed++;
+		}
+	}
+
+	EXPECT_EQ(parsed, num_valid);
+}

src/lib/gnss/test/RtcmTestCommon.hpp

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <gtest/gtest.h>
+#include "rtcm.h"
+#include <vector>
+#include <cstring>
+#include <random>
+
+using namespace gnss;
+
+class RtcmTest : public ::testing::Test
+{
+protected:
+	Rtcm3Parser parser;
+
+	// 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)
+	{
+		// Payload = 12-bit message type + rest of payload_data
+		// Message type is stored in first 12 bits of payload
+		std::vector<uint8_t> payload;
+		payload.push_back((msg_type >> 4) & 0xFF);
+		payload.push_back(((msg_type & 0x0F) << 4) | (payload_data.empty() ? 0 : (payload_data[0] >> 4)));
+
+		for (size_t i = 0; i < payload_data.size(); i++) {
+			uint8_t current = (payload_data[i] << 4) & 0xF0;
+
+			if (i + 1 < payload_data.size()) {
+				current |= (payload_data[i + 1] >> 4) & 0x0F;
+			}
+
+			payload.push_back(current);
+		}
+
+		return buildRawFrame(payload);
+	}
+
+	// Helper to build a frame with raw payload bytes (no message type encoding)
+	std::vector<uint8_t> buildRawFrame(const std::vector<uint8_t> &payload)
+	{
+		std::vector<uint8_t> frame;
+		size_t payload_len = payload.size();
+
+		// Header: preamble + length
+		frame.push_back(RTCM3_PREAMBLE);
+		frame.push_back((payload_len >> 8) & 0x03);  // Upper 2 bits of length (reserved bits = 0)
+		frame.push_back(payload_len & 0xFF);         // Lower 8 bits of length
+
+		// Payload
+		frame.insert(frame.end(), payload.begin(), payload.end());
+
+		// CRC
+		uint32_t crc = rtcm3_crc24q(frame.data(), frame.size());
+		frame.push_back((crc >> 16) & 0xFF);
+		frame.push_back((crc >> 8) & 0xFF);
+		frame.push_back(crc & 0xFF);
+
+		return frame;
+	}
+
+	// Helper to build a frame with bad CRC
+	std::vector<uint8_t> buildBadCrcFrame(const std::vector<uint8_t> &payload)
+	{
+		auto frame = buildRawFrame(payload);
+		// Corrupt the CRC
+		frame[frame.size() - 1] ^= 0xFF;
+		return frame;
+	}
+};