diff --git a/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.cpp b/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.cpp new file mode 100644 index 00000000..7e2aced6 --- /dev/null +++ b/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.cpp @@ -0,0 +1,151 @@ +#include "LocationTelemetryCodec.h" + +#include +#include + +namespace Telemetry { +namespace { + +class Cursor { +public: + Cursor(const uint8_t* data, std::size_t size) : data_(data), size_(size) {} + + bool readByte(uint8_t& value) { + if (position_ >= size_) return false; + value = data_[position_++]; + return true; + } + + bool readUnsigned(uint64_t& value) { + uint8_t marker = 0; + if (!readByte(marker)) return false; + if (marker <= 0x7f) { + value = marker; + return true; + } + + std::size_t width = 0; + switch (marker) { + case 0xcc: width = 1; break; + case 0xcd: width = 2; break; + case 0xce: width = 4; break; + case 0xcf: width = 8; break; + default: return false; + } + if (size_ - position_ < width) return false; + + value = 0; + for (std::size_t index = 0; index < width; ++index) { + value = (value << 8U) | data_[position_++]; + } + return true; + } + + bool readMapSize(std::size_t& count) { + uint8_t marker = 0; + if (!readByte(marker) || (marker & 0xf0U) != 0x80U) return false; + count = marker & 0x0fU; + return true; + } + + bool readArraySize(std::size_t& count) { + uint8_t marker = 0; + if (!readByte(marker) || (marker & 0xf0U) != 0x90U) return false; + count = marker & 0x0fU; + return true; + } + + bool readBinary(uint8_t* destination, std::size_t required) { + uint8_t marker = 0; + uint8_t encoded_size = 0; + if (!readByte(marker) || marker != 0xc4U || !readByte(encoded_size)) return false; + if (encoded_size != required || size_ - position_ < required) return false; + for (std::size_t index = 0; index < required; ++index) { + destination[index] = data_[position_++]; + } + return true; + } + + bool atEnd() const { return position_ == size_; } + +private: + const uint8_t* data_; + std::size_t size_; + std::size_t position_ = 0; +}; + +uint32_t decodeU32(const uint8_t bytes[4]) { + return (static_cast(bytes[0]) << 24U) | + (static_cast(bytes[1]) << 16U) | + (static_cast(bytes[2]) << 8U) | + static_cast(bytes[3]); +} + +int32_t decodeI32(const uint8_t bytes[4]) { + return static_cast(decodeU32(bytes)); +} + +bool readLocation(Cursor& cursor, LocationTelemetry& location) { + std::size_t count = 0; + if (!cursor.readArraySize(count) || count != 7) return false; + + uint8_t word[4]{}; + uint8_t half[2]{}; + uint64_t timestamp = 0; + + if (!cursor.readBinary(word, sizeof(word))) return false; + location.latitude_e6 = decodeI32(word); + if (!cursor.readBinary(word, sizeof(word))) return false; + location.longitude_e6 = decodeI32(word); + if (!cursor.readBinary(word, sizeof(word))) return false; + location.altitude_cm = decodeI32(word); + if (!cursor.readBinary(word, sizeof(word))) return false; + location.speed_cms = decodeU32(word); + if (!cursor.readBinary(word, sizeof(word))) return false; + location.bearing_cdeg = decodeI32(word); + if (!cursor.readBinary(half, sizeof(half))) return false; + location.accuracy_cm = static_cast( + (static_cast(half[0]) << 8U) | half[1]); + if (!cursor.readUnsigned(timestamp)) return false; + location.timestamp_seconds = timestamp; + return true; +} + +} // namespace + +DecodeResult decodeLocationTelemetry( + const uint8_t* data, + std::size_t size, + LocationTelemetry& output) { + if (data == nullptr || size == 0) return DecodeResult::INVALID_ARGUMENT; + + Cursor cursor(data, size); + std::size_t map_size = 0; + if (!cursor.readMapSize(map_size)) return DecodeResult::MALFORMED; + + LocationTelemetry candidate{}; + bool has_location = false; + for (std::size_t index = 0; index < map_size; ++index) { + uint64_t key = 0; + if (!cursor.readUnsigned(key)) return DecodeResult::MALFORMED; + if (key == SID_TIME) { + if (!cursor.readUnsigned(candidate.sensor_timestamp_seconds)) { + return DecodeResult::MALFORMED; + } + } else if (key == SID_LOCATION) { + if (has_location || !readLocation(cursor, candidate)) { + return DecodeResult::MALFORMED; + } + has_location = true; + } else { + return DecodeResult::MALFORMED; + } + } + + if (!cursor.atEnd()) return DecodeResult::MALFORMED; + if (!has_location) return DecodeResult::MISSING_LOCATION; + output = candidate; + return DecodeResult::OK; +} + +} // namespace Telemetry diff --git a/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.h b/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.h new file mode 100644 index 00000000..13b70a62 --- /dev/null +++ b/lib/tdeck_ui/Telemetry/LocationTelemetryCodec.h @@ -0,0 +1,40 @@ +#ifndef PYXIS_TELEMETRY_LOCATION_TELEMETRY_CODEC_H +#define PYXIS_TELEMETRY_LOCATION_TELEMETRY_CODEC_H + +#include +#include + +namespace Telemetry { + +constexpr uint8_t FIELD_TELEMETRY = 0x02; +constexpr uint8_t FIELD_ICON_APPEARANCE = 0x04; +constexpr uint8_t FIELD_CUSTOM_META = 0xFD; +constexpr uint8_t SID_TIME = 0x01; +constexpr uint8_t SID_LOCATION = 0x02; + +struct LocationTelemetry { + int32_t latitude_e6 = 0; + int32_t longitude_e6 = 0; + int32_t altitude_cm = 0; + uint32_t speed_cms = 0; + int32_t bearing_cdeg = 0; + uint16_t accuracy_cm = 0; + uint64_t timestamp_seconds = 0; + uint64_t sensor_timestamp_seconds = 0; +}; + +enum class DecodeResult : uint8_t { + OK, + INVALID_ARGUMENT, + MALFORMED, + MISSING_LOCATION, +}; + +DecodeResult decodeLocationTelemetry( + const uint8_t* data, + std::size_t size, + LocationTelemetry& output); + +} // namespace Telemetry + +#endif // PYXIS_TELEMETRY_LOCATION_TELEMETRY_CODEC_H diff --git a/tests/native/native_test.py b/tests/native/native_test.py new file mode 100644 index 00000000..992d9706 --- /dev/null +++ b/tests/native/native_test.py @@ -0,0 +1,60 @@ +"""Shared compile/run support for portable C++ production-code tests.""" + +from __future__ import annotations + +import os +from pathlib import Path +import shutil +import subprocess + +import pytest + + +def find_cxx() -> str: + cxx = shutil.which("clang++") or shutil.which("g++") + if not cxx: + pytest.skip("no C++ compiler found") + return cxx + + +def compile_and_run( + tmp_path: Path, + *, + name: str, + sources: list[Path], + include_dirs: list[Path], + sanitize: bool = False, + timeout: int = 30, +) -> subprocess.CompletedProcess[str]: + """Compile strict C++17 sources and execute the resulting test binary.""" + binary = tmp_path / name + cmd = [ + find_cxx(), + "-std=c++17", + "-Wall", + "-Wextra", + "-Werror", + "-pedantic", + *[f"-I{path}" for path in include_dirs], + *[str(path) for path in sources], + "-o", + str(binary), + ] + if sanitize: + cmd[1:1] = [ + "-fsanitize=address,undefined", + "-fno-omit-frame-pointer", + ] + + compiled = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout) + assert compiled.returncode == 0, compiled.stdout + compiled.stderr + + env = os.environ.copy() + if sanitize: + env.setdefault("ASAN_OPTIONS", "detect_leaks=1:halt_on_error=1") + env.setdefault("UBSAN_OPTIONS", "halt_on_error=1:print_stacktrace=1") + ran = subprocess.run( + [str(binary)], capture_output=True, text=True, timeout=timeout, env=env + ) + assert ran.returncode == 0, ran.stdout + ran.stderr + return ran diff --git a/tests/native/test_location_telemetry_codec.cpp b/tests/native/test_location_telemetry_codec.cpp new file mode 100644 index 00000000..5f0e6a8c --- /dev/null +++ b/tests/native/test_location_telemetry_codec.cpp @@ -0,0 +1,54 @@ +#include +#include +#include + +#include "Telemetry/LocationTelemetryCodec.h" + +namespace { + +int failures = 0; + +#define CHECK(expr) \ + do { \ + if (!(expr)) { \ + ++failures; \ + std::cerr << "FAIL line " << __LINE__ << ": " #expr << '\n'; \ + } \ + } while (false) + +void decodes_canonical_sideband_location() { + // MessagePack: {SID_TIME: 1700000000, SID_LOCATION: [fixed-width fields...]} + const uint8_t packed[] = { + 0x82, 0x01, 0xce, 0x65, 0x53, 0xf1, 0x00, 0x02, 0x97, + 0xc4, 0x04, 0x02, 0x40, 0x66, 0x34, // 37.774900 degrees + 0xc4, 0x04, 0xf8, 0xb4, 0x07, 0x38, // -122.419400 degrees + 0xc4, 0x04, 0x00, 0x00, 0x06, 0x40, // 16.00 m + 0xc4, 0x04, 0x00, 0x00, 0x00, 0x00, // 0.00 m/s + 0xc4, 0x04, 0x00, 0x00, 0x10, 0x68, // 42.00 degrees + 0xc4, 0x02, 0x01, 0x5e, // 3.50 m + 0xce, 0x65, 0x53, 0xf1, 0x00, + }; + + Telemetry::LocationTelemetry output{}; + const auto result = Telemetry::decodeLocationTelemetry( + packed, sizeof(packed), output); + + CHECK(result == Telemetry::DecodeResult::OK); + CHECK(output.latitude_e6 == 37774900); + CHECK(output.longitude_e6 == -122419400); + CHECK(output.altitude_cm == 1600); + CHECK(output.speed_cms == 0); + CHECK(output.bearing_cdeg == 4200); + CHECK(output.accuracy_cm == 350); + CHECK(output.timestamp_seconds == 1700000000ULL); +} + +} // namespace + +int main() { + decodes_canonical_sideband_location(); + if (failures == 0) { + std::cout << "location telemetry codec: 7 passed, 0 failed\n"; + } + return failures == 0 ? EXIT_SUCCESS : EXIT_FAILURE; +} diff --git a/tests/native/test_location_telemetry_codec.py b/tests/native/test_location_telemetry_codec.py new file mode 100644 index 00000000..baa97c29 --- /dev/null +++ b/tests/native/test_location_telemetry_codec.py @@ -0,0 +1,26 @@ +"""Compile and execute the portable location telemetry codec tests.""" + +from pathlib import Path + +from native_test import compile_and_run + +HERE = Path(__file__).resolve().parent +PYXIS_ROOT = HERE.parent.parent + + +def test_location_telemetry_codec(tmp_path): + ran = compile_and_run( + tmp_path, + name="test_location_telemetry_codec", + sources=[ + HERE / "test_location_telemetry_codec.cpp", + PYXIS_ROOT + / "lib" + / "tdeck_ui" + / "Telemetry" + / "LocationTelemetryCodec.cpp", + ], + include_dirs=[PYXIS_ROOT / "lib" / "tdeck_ui"], + sanitize=True, + ) + assert "location telemetry codec: 7 passed, 0 failed" in ran.stdout