diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 22e377402e..f732ddbef1 100644 --- a/src/lib/CMakeLists.txt +++ b/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) diff --git a/src/lib/gnss/CMakeLists.txt b/src/lib/gnss/CMakeLists.txt new file mode 100644 index 0000000000..a577c75b92 --- /dev/null +++ b/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() diff --git a/src/lib/gnss/rtcm.cpp b/src/lib/gnss/rtcm.cpp new file mode 100644 index 0000000000..d20b07ce8a --- /dev/null +++ b/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 + +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(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(_buffer[frame_len - 3]) << 16) | + (static_cast(_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 diff --git a/src/lib/gnss/rtcm.h b/src/lib/gnss/rtcm.h new file mode 100644 index 0000000000..660682fdc5 --- /dev/null +++ b/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 +#include + +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(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(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 diff --git a/src/lib/gnss/test/CMakeLists.txt b/src/lib/gnss/test/CMakeLists.txt new file mode 100644 index 0000000000..e693e1f015 --- /dev/null +++ b/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) diff --git a/src/lib/gnss/test/README.md b/src/lib/gnss/test/README.md new file mode 100644 index 0000000000..ff7da7a73e --- /dev/null +++ b/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 | diff --git a/src/lib/gnss/test/RtcmBustedSenderTest.cpp b/src/lib/gnss/test/RtcmBustedSenderTest.cpp new file mode 100644 index 0000000000..edb7ad5463 --- /dev/null +++ b/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 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 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 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(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); +} diff --git a/src/lib/gnss/test/RtcmCrcTest.cpp b/src/lib/gnss/test/RtcmCrcTest.cpp new file mode 100644 index 0000000000..e4a2b9eb82 --- /dev/null +++ b/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); +} diff --git a/src/lib/gnss/test/RtcmParserTest.cpp b/src/lib/gnss/test/RtcmParserTest.cpp new file mode 100644 index 0000000000..6652412c83 --- /dev/null +++ b/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 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 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 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); +} diff --git a/src/lib/gnss/test/RtcmStressTest.cpp b/src/lib/gnss/test/RtcmStressTest.cpp new file mode 100644 index 0000000000..0b115c8612 --- /dev/null +++ b/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(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(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 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(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 garbage(10 + (i % 50), 0xAA); + parser.addData(garbage.data(), garbage.size()); + + // Add valid frame + auto frame = buildValidFrame(1005, {static_cast(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); +} diff --git a/src/lib/gnss/test/RtcmTestCommon.hpp b/src/lib/gnss/test/RtcmTestCommon.hpp new file mode 100644 index 0000000000..f2012d353e --- /dev/null +++ b/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 +#include "rtcm.h" +#include +#include +#include + +using namespace gnss; + +class RtcmTest : public ::testing::Test +{ +protected: + Rtcm3Parser parser; + + // Helper to build a valid RTCM3 frame with proper CRC + std::vector buildValidFrame(uint16_t msg_type, const std::vector &payload_data) + { + // Payload = 12-bit message type + rest of payload_data + // Message type is stored in first 12 bits of payload + std::vector 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 buildRawFrame(const std::vector &payload) + { + std::vector 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 buildBadCrcFrame(const std::vector &payload) + { + auto frame = buildRawFrame(payload); + // Corrupt the CRC + frame[frame.size() - 1] ^= 0xFF; + return frame; + } +};