Merge branch 'main' into pr-dual-atmos

* 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>

msg/EstimatorStatus.msg

@@ -16,6 +16,7 @@ uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position
 uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8	# 8 : maximum allowed horizontal speed fail - requires stationary vehicle
 uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9	# 9 : maximum allowed vertical velocity discrepancy fail
 uint8 GPS_CHECK_FAIL_SPOOFED = 10		# 10 : GPS signal is spoofed
+uint8 GPS_CHECK_FAIL_JAMMED = 11		# 11 : GPS signal is jammed
 
 uint64 control_mode_flags	# Bitmask to indicate EKF logic state
 uint8 CS_TILT_ALIGN = 0		# 0 - true if the filter tilt alignment is complete

msg/GpsDump.msg

@@ -2,9 +2,13 @@
 
 uint64 timestamp # time since system start (microseconds)
 
+uint8 INSTANCE_MAIN = 0
+uint8 INSTANCE_SECONDARY = 1
+
 uint8 instance   # Instance of GNSS receiver
+uint32 device_id
 uint8 len        # length of data, MSB bit set = message to the gps device,
                  # clear = message from the device
 uint8[79] data   # data to write to the log
 
-uint8 ORB_QUEUE_LENGTH = 8
+uint8 ORB_QUEUE_LENGTH = 16

src/drivers/gnss/septentrio/septentrio.cpp

@@ -1728,7 +1728,8 @@ void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
 void SeptentrioDriver::dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction)
 {
 	gps_dump_s *dump_data = data_direction == DataDirection::FromReceiver ? _message_data_from_receiver : _message_data_to_receiver;
-	dump_data->instance = _instance == Instance::Main ? 0 : 1;
+	dump_data->instance = _instance == Instance::Main ? gps_dump_s::INSTANCE_MAIN : gps_dump_s::INSTANCE_SECONDARY;
+	dump_data->device_id = get_device_id();
 
 	while (len > 0) {
 		size_t write_len = len;

src/drivers/gps/gps.cpp

@@ -696,6 +696,7 @@ void GPS::dumpGpsData(uint8_t *data, size_t len, gps_dump_comm_mode_t mode, bool
 	}
 
 	dump_data->instance = (uint8_t)_instance;
+	dump_data->device_id = get_device_id();
 
 	while (len > 0) {
 		size_t write_len = len;

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;
+	}
+};

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -272,6 +272,22 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			_gnss_spoofed = false;
 		}
 
+		if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) {
+			if (!_gnss_jammed) {
+				_gnss_jammed = true;
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal jammed\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_warning_jamming"), {events::Log::Alert, events::LogInternal::Info},
+					     "GNSS signal jammed");
+			}
+
+		} else {
+			_gnss_jammed = false;
+		}
+
 		if (!context.isArmed() && ekf_gps_check_fail) {
 			NavModesMessageFail required_modes;
 			events::Log log_level;
@@ -435,6 +451,18 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 							    events::ID("check_estimator_gps_spoofed"),
 							    log_level, "GPS signal spoofed");
 
+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_JAMMED)) {
+				message = "Preflight%s: GPS signal jammed";
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * Can be configured with <param>EKF2_GPS_CHECK</param> and <param>COM_ARM_WO_GPS</param>.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_modes, health_component_t::gps,
+							    events::ID("check_estimator_gps_jammed"),
+							    log_level, "GPS signal jammed");
+
 			} else {
 				if (!ekf_gps_fusion) {
 					// Likely cause unknown

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -103,6 +103,7 @@ private:
 
 	bool _gps_was_fused{false};
 	bool _gnss_spoofed{false};
+	bool _gnss_jammed{false};
 
 	bool _nav_failure_imminent_warned{false};
 

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp

@@ -91,6 +91,7 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
 	_check_fail_status.flags.sacc = (gnss.sacc > 10.f);
 
 	_check_fail_status.flags.spoofed = gnss.spoofed;
+	_check_fail_status.flags.jammed = gnss.jammed;
 
 	bool passed = true;
 
@@ -99,7 +100,8 @@ bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
 		(_check_fail_status.flags.hacc    && isCheckEnabled(GnssChecksMask::kHacc)) ||
 		(_check_fail_status.flags.vacc    && isCheckEnabled(GnssChecksMask::kVacc)) ||
 		(_check_fail_status.flags.sacc    && isCheckEnabled(GnssChecksMask::kSacc)) ||
-		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
+		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed)) ||
+		(_check_fail_status.flags.jammed  && isCheckEnabled(GnssChecksMask::kJammed))
 	) {
 		passed = false;
 	}
@@ -126,6 +128,7 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 	_check_fail_status.flags.sacc = (gnss.sacc > _params.ekf2_req_sacc);
 
 	_check_fail_status.flags.spoofed = gnss.spoofed;
+	_check_fail_status.flags.jammed = gnss.jammed;
 
 	runOnGroundGnssChecks(gnss);
 
@@ -153,7 +156,8 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 		(_check_fail_status.flags.vdrift  && isCheckEnabled(GnssChecksMask::kVdrift)) ||
 		(_check_fail_status.flags.hspeed  && isCheckEnabled(GnssChecksMask::kHspd)) ||
 		(_check_fail_status.flags.vspeed  && isCheckEnabled(GnssChecksMask::kVspd)) ||
-		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
+		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed)) ||
+		(_check_fail_status.flags.jammed  && isCheckEnabled(GnssChecksMask::kJammed))
 	) {
 		passed = false;
 	}

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp

@@ -63,6 +63,7 @@ public:
 			uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground)
 			uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive
 			uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed
+			uint16_t jammed : 1; ///< 11 - true if the GNSS data is jammed
 		} flags;
 		uint16_t value;
 	};
@@ -108,7 +109,8 @@ private:
 		kHspd    = (1 << 7),
 		kVspd    = (1 << 8),
 		kSpoofed = (1 << 9),
-		kFix     = (1 << 10)
+		kFix     = (1 << 10),
+		kJammed  = (1 << 11)
 	};
 
 	bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }

src/modules/ekf2/EKF/common.h

@@ -202,6 +202,7 @@ struct gnssSample {
 	float       yaw_acc{};    ///< 1-std yaw error (rad)
 	float       yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET
 	bool        spoofed{};    ///< true if GNSS data is spoofed
+	bool        jammed{};     ///< true if GNSS data is jammed
 };
 
 struct magSample {

src/modules/ekf2/EKF2.cpp

@@ -2473,6 +2473,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			.yaw_acc = vehicle_gps_position.heading_accuracy,
 			.yaw_offset = vehicle_gps_position.heading_offset,
 			.spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_DETECTED,
+			.jammed = vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_DETECTED,
 		};
 
 		_ekf.setGpsData(gnss_sample, pps_compensation);

src/modules/ekf2/params_gnss.yaml

@@ -128,9 +128,10 @@ parameters:
         8: Vertical speed offset (EKF2_REQ_VDRIFT)
         9: Spoofing
         10: GPS fix type (EKF2_REQ_FIX)
+        11: Jamming
       default: 2047
       min: 0
-      max: 2047
+      max: 4095
     EKF2_REQ_EPH:
       description:
         short: Required EPH to use GPS

src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp

@@ -109,9 +109,10 @@ void GZMixingInterfaceESC::motorSpeedCallback(const gz::msgs::Actuators &actuato
 	pthread_mutex_lock(&_node_mutex);
 
 	esc_status_s esc_status{};
-	esc_status.esc_count = actuators.velocity_size();
+	// Limit to max supported ESCs while allowing for a larger number of system actuators
+	esc_status.esc_count = math::min(actuators.velocity_size(), static_cast<int>(esc_status_s::CONNECTED_ESC_MAX));
 
-	for (int i = 0; i < actuators.velocity_size(); i++) {
+	for (int i = 0; i < esc_status.esc_count; i++) {
 		esc_status.esc[i].timestamp = hrt_absolute_time();
 		esc_status.esc[i].esc_rpm = actuators.velocity(i);
 		esc_status.esc_online_flags |= 1 << i;

src/modules/simulation/gz_bridge/module.yaml

@@ -1,3 +1,4 @@
+__max_num_motors: &max_num_motors 12
 __max_num_servos: &max_num_servos 8
 __max_num_tilts: &max_num_tilts 4
 
@@ -18,7 +19,7 @@ actuator_output:
         min: { min: 0, max: 1000, default: 0 }
         max: { min: 0, max: 1000, default: 1000 }
         failsafe: { min: 0, max: 1000 }
-      num_channels: *max_num_servos
+      num_channels: *max_num_motors
     - param_prefix: SIM_GZ_SV
       group_label: 'Servos'
       channel_label: 'Servo'