feat: decode canonical location telemetry

2026-08-21 18:19:48 +00:00 · 2026-08-07 01:23:08 +00:00
parent 1c5c887e57
commit 71fd61050f
5 changed files with 331 additions and 0 deletions
@@ -0,0 +1,151 @@
+#include "LocationTelemetryCodec.h"
+
+#include <cstddef>
+#include <cstdint>
+
+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<uint32_t>(bytes[0]) << 24U) |
+           (static_cast<uint32_t>(bytes[1]) << 16U) |
+           (static_cast<uint32_t>(bytes[2]) << 8U) |
+           static_cast<uint32_t>(bytes[3]);
+}
+
+int32_t decodeI32(const uint8_t bytes[4]) {
+    return static_cast<int32_t>(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<uint16_t>(
+        (static_cast<uint16_t>(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