Cleanup ubloxd message parsing (#20664)

* use kaitai to generate parsers

* add scons flag to generate parsers

* add outer layer ephemeris parsing

* create ephemeris packet at the right time

* parse subframe 1 and 2

* add more fields

* add generated files to release

* fix signs

* no cast

* add unordered map include

* add exception handling

* small cleanup

* parse iono data

* cleanup

SConstruct

@@ -13,6 +13,10 @@ AddOption('--test',
           action='store_true',
           help='build test files')
 
+AddOption('--kaitai',
+          action='store_true',
+          help='Regenerate kaitai struct parsers')
+
 AddOption('--asan',
           action='store_true',
           help='turn on ASAN')
@@ -181,6 +185,7 @@ env = Environment(
     "#phonelibs/snpe/include",
     "#phonelibs/nanovg",
     "#phonelibs/qrcode",
+    "#phonelibs",
     "#selfdrive/boardd",
     "#selfdrive/common",
     "#selfdrive/camerad",

phonelibs/SConscript

@@ -2,3 +2,5 @@ Import('env')
 
 env.Library('json11', ['json11/json11.cpp'])
 env.Append(CPPPATH=[Dir('json11')])
+
+env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE'])

phonelibs/kaitai/custom_decoder.h

@@ -0,0 +1,16 @@
+#ifndef KAITAI_CUSTOM_DECODER_H
+#define KAITAI_CUSTOM_DECODER_H
+
+#include <string>
+
+namespace kaitai {
+
+class custom_decoder {
+public:
+    virtual ~custom_decoder() {};
+    virtual std::string decode(std::string src) = 0;
+};
+
+}
+
+#endif

phonelibs/kaitai/exceptions.h

@@ -0,0 +1,189 @@
+#ifndef KAITAI_EXCEPTIONS_H
+#define KAITAI_EXCEPTIONS_H
+
+#include <kaitai/kaitaistream.h>
+
+#include <string>
+#include <stdexcept>
+
+// We need to use "noexcept" in virtual destructor of our exceptions
+// subclasses. Different compilers have different ideas on how to
+// achieve that: C++98 compilers prefer `throw()`, C++11 and later
+// use `noexcept`. We define KS_NOEXCEPT macro for that.
+
+#if __cplusplus >= 201103L || (defined(_MSC_VER) && _MSC_VER >= 1900)
+#define KS_NOEXCEPT noexcept
+#else
+#define KS_NOEXCEPT throw()
+#endif
+
+namespace kaitai {
+
+/**
+ * Common ancestor for all error originating from Kaitai Struct usage.
+ * Stores KSY source path, pointing to an element supposedly guilty of
+ * an error.
+ */
+class kstruct_error: public std::runtime_error {
+public:
+    kstruct_error(const std::string what, const std::string src_path):
+        std::runtime_error(src_path + ": " + what),
+        m_src_path(src_path)
+    {
+    }
+
+    virtual ~kstruct_error() KS_NOEXCEPT {};
+
+protected:
+    const std::string m_src_path;
+};
+
+/**
+ * Error that occurs when default endianness should be decided with
+ * a switch, but nothing matches (although using endianness expression
+ * implies that there should be some positive result).
+ */
+class undecided_endianness_error: public kstruct_error {
+public:
+    undecided_endianness_error(const std::string src_path):
+        kstruct_error("unable to decide on endianness for a type", src_path)
+    {
+    }
+
+    virtual ~undecided_endianness_error() KS_NOEXCEPT {};
+};
+
+/**
+ * Common ancestor for all validation failures. Stores pointer to
+ * KaitaiStream IO object which was involved in an error.
+ */
+class validation_failed_error: public kstruct_error {
+public:
+    validation_failed_error(const std::string what, kstream* io, const std::string src_path):
+        kstruct_error("at pos " + kstream::to_string(static_cast<int>(io->pos())) + ": validation failed: " + what, src_path),
+        m_io(io)
+    {
+    }
+
+// "at pos #{io.pos}: validation failed: #{msg}"
+
+    virtual ~validation_failed_error() KS_NOEXCEPT {};
+
+protected:
+    kstream* m_io;
+};
+
+/**
+ * Signals validation failure: we required "actual" value to be equal to
+ * "expected", but it turned out that it's not.
+ */
+template<typename T>
+class validation_not_equal_error: public validation_failed_error {
+public:
+    validation_not_equal_error<T>(const T& expected, const T& actual, kstream* io, const std::string src_path):
+        validation_failed_error("not equal", io, src_path),
+        m_expected(expected),
+        m_actual(actual)
+    {
+    }
+
+    // "not equal, expected #{expected.inspect}, but got #{actual.inspect}"
+
+    virtual ~validation_not_equal_error<T>() KS_NOEXCEPT {};
+
+protected:
+    const T& m_expected;
+    const T& m_actual;
+};
+
+/**
+ * Signals validation failure: we required "actual" value to be greater
+ * than or equal to "min", but it turned out that it's not.
+ */
+template<typename T>
+class validation_less_than_error: public validation_failed_error {
+public:
+    validation_less_than_error<T>(const T& min, const T& actual, kstream* io, const std::string src_path):
+        validation_failed_error("not in range", io, src_path),
+        m_min(min),
+        m_actual(actual)
+    {
+    }
+
+    // "not in range, min #{min.inspect}, but got #{actual.inspect}"
+
+    virtual ~validation_less_than_error<T>() KS_NOEXCEPT {};
+
+protected:
+    const T& m_min;
+    const T& m_actual;
+};
+
+/**
+ * Signals validation failure: we required "actual" value to be less
+ * than or equal to "max", but it turned out that it's not.
+ */
+template<typename T>
+class validation_greater_than_error: public validation_failed_error {
+public:
+    validation_greater_than_error<T>(const T& max, const T& actual, kstream* io, const std::string src_path):
+        validation_failed_error("not in range", io, src_path),
+        m_max(max),
+        m_actual(actual)
+    {
+    }
+
+    // "not in range, max #{max.inspect}, but got #{actual.inspect}"
+
+    virtual ~validation_greater_than_error<T>() KS_NOEXCEPT {};
+
+protected:
+    const T& m_max;
+    const T& m_actual;
+};
+
+/**
+ * Signals validation failure: we required "actual" value to be from
+ * the list, but it turned out that it's not.
+ */
+template<typename T>
+class validation_not_any_of_error: public validation_failed_error {
+public:
+    validation_not_any_of_error<T>(const T& actual, kstream* io, const std::string src_path):
+        validation_failed_error("not any of the list", io, src_path),
+        m_actual(actual)
+    {
+    }
+
+    // "not any of the list, got #{actual.inspect}"
+
+    virtual ~validation_not_any_of_error<T>() KS_NOEXCEPT {};
+
+protected:
+    const T& m_actual;
+};
+
+/**
+ * Signals validation failure: we required "actual" value to match
+ * the expression, but it turned out that it doesn't.
+ */
+template<typename T>
+class validation_expr_error: public validation_failed_error {
+public:
+    validation_expr_error<T>(const T& actual, kstream* io, const std::string src_path):
+        validation_failed_error("not matching the expression", io, src_path),
+        m_actual(actual)
+    {
+    }
+
+    // "not matching the expression, got #{actual.inspect}"
+
+    virtual ~validation_expr_error<T>() KS_NOEXCEPT {};
+
+protected:
+    const T& m_actual;
+};
+
+}
+
+#endif

phonelibs/kaitai/kaitaistream.cpp

@@ -0,0 +1,689 @@
+#include <kaitai/kaitaistream.h>
+
+#if defined(__APPLE__)
+#include <machine/endian.h>
+#include <libkern/OSByteOrder.h>
+#define bswap_16(x) OSSwapInt16(x)
+#define bswap_32(x) OSSwapInt32(x)
+#define bswap_64(x) OSSwapInt64(x)
+#define __BYTE_ORDER    BYTE_ORDER
+#define __BIG_ENDIAN    BIG_ENDIAN
+#define __LITTLE_ENDIAN LITTLE_ENDIAN
+#elif defined(_MSC_VER) // !__APPLE__
+#include <stdlib.h>
+#define __LITTLE_ENDIAN     1234
+#define __BIG_ENDIAN        4321
+#define __BYTE_ORDER        __LITTLE_ENDIAN
+#define bswap_16(x) _byteswap_ushort(x)
+#define bswap_32(x) _byteswap_ulong(x)
+#define bswap_64(x) _byteswap_uint64(x)
+#else // !__APPLE__ or !_MSC_VER
+#include <endian.h>
+#include <byteswap.h>
+#endif
+
+#include <iostream>
+#include <vector>
+#include <stdexcept>
+
+kaitai::kstream::kstream(std::istream* io) {
+    m_io = io;
+    init();
+}
+
+kaitai::kstream::kstream(std::string& data): m_io_str(data) {
+    m_io = &m_io_str;
+    init();
+}
+
+void kaitai::kstream::init() {
+    exceptions_enable();
+    align_to_byte();
+}
+
+void kaitai::kstream::close() {
+    //  m_io->close();
+}
+
+void kaitai::kstream::exceptions_enable() const {
+    m_io->exceptions(
+        std::istream::eofbit |
+        std::istream::failbit |
+        std::istream::badbit
+    );
+}
+
+// ========================================================================
+// Stream positioning
+// ========================================================================
+
+bool kaitai::kstream::is_eof() const {
+    if (m_bits_left > 0) {
+        return false;
+    }
+    char t;
+    m_io->exceptions(
+        std::istream::badbit
+    );
+    m_io->get(t);
+    if (m_io->eof()) {
+        m_io->clear();
+        exceptions_enable();
+        return true;
+    } else {
+        m_io->unget();
+        exceptions_enable();
+        return false;
+    }
+}
+
+void kaitai::kstream::seek(uint64_t pos) {
+    m_io->seekg(pos);
+}
+
+uint64_t kaitai::kstream::pos() {
+    return m_io->tellg();
+}
+
+uint64_t kaitai::kstream::size() {
+    std::iostream::pos_type cur_pos = m_io->tellg();
+    m_io->seekg(0, std::ios::end);
+    std::iostream::pos_type len = m_io->tellg();
+    m_io->seekg(cur_pos);
+    return len;
+}
+
+// ========================================================================
+// Integer numbers
+// ========================================================================
+
+// ------------------------------------------------------------------------
+// Signed
+// ------------------------------------------------------------------------
+
+int8_t kaitai::kstream::read_s1() {
+    char t;
+    m_io->get(t);
+    return t;
+}
+
+// ........................................................................
+// Big-endian
+// ........................................................................
+
+int16_t kaitai::kstream::read_s2be() {
+    int16_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 2);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_16(t);
+#endif
+    return t;
+}
+
+int32_t kaitai::kstream::read_s4be() {
+    int32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_32(t);
+#endif
+    return t;
+}
+
+int64_t kaitai::kstream::read_s8be() {
+    int64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_64(t);
+#endif
+    return t;
+}
+
+// ........................................................................
+// Little-endian
+// ........................................................................
+
+int16_t kaitai::kstream::read_s2le() {
+    int16_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 2);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_16(t);
+#endif
+    return t;
+}
+
+int32_t kaitai::kstream::read_s4le() {
+    int32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_32(t);
+#endif
+    return t;
+}
+
+int64_t kaitai::kstream::read_s8le() {
+    int64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_64(t);
+#endif
+    return t;
+}
+
+// ------------------------------------------------------------------------
+// Unsigned
+// ------------------------------------------------------------------------
+
+uint8_t kaitai::kstream::read_u1() {
+    char t;
+    m_io->get(t);
+    return t;
+}
+
+// ........................................................................
+// Big-endian
+// ........................................................................
+
+uint16_t kaitai::kstream::read_u2be() {
+    uint16_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 2);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_16(t);
+#endif
+    return t;
+}
+
+uint32_t kaitai::kstream::read_u4be() {
+    uint32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_32(t);
+#endif
+    return t;
+}
+
+uint64_t kaitai::kstream::read_u8be() {
+    uint64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_64(t);
+#endif
+    return t;
+}
+
+// ........................................................................
+// Little-endian
+// ........................................................................
+
+uint16_t kaitai::kstream::read_u2le() {
+    uint16_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 2);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_16(t);
+#endif
+    return t;
+}
+
+uint32_t kaitai::kstream::read_u4le() {
+    uint32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_32(t);
+#endif
+    return t;
+}
+
+uint64_t kaitai::kstream::read_u8le() {
+    uint64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_64(t);
+#endif
+    return t;
+}
+
+// ========================================================================
+// Floating point numbers
+// ========================================================================
+
+// ........................................................................
+// Big-endian
+// ........................................................................
+
+float kaitai::kstream::read_f4be() {
+    uint32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_32(t);
+#endif
+    return reinterpret_cast<float&>(t);
+}
+
+double kaitai::kstream::read_f8be() {
+    uint64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __LITTLE_ENDIAN
+    t = bswap_64(t);
+#endif
+    return reinterpret_cast<double&>(t);
+}
+
+// ........................................................................
+// Little-endian
+// ........................................................................
+
+float kaitai::kstream::read_f4le() {
+    uint32_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 4);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_32(t);
+#endif
+    return reinterpret_cast<float&>(t);
+}
+
+double kaitai::kstream::read_f8le() {
+    uint64_t t;
+    m_io->read(reinterpret_cast<char *>(&t), 8);
+#if __BYTE_ORDER == __BIG_ENDIAN
+    t = bswap_64(t);
+#endif
+    return reinterpret_cast<double&>(t);
+}
+
+// ========================================================================
+// Unaligned bit values
+// ========================================================================
+
+void kaitai::kstream::align_to_byte() {
+    m_bits_left = 0;
+    m_bits = 0;
+}
+
+uint64_t kaitai::kstream::read_bits_int_be(int n) {
+    int bits_needed = n - m_bits_left;
+    if (bits_needed > 0) {
+        // 1 bit  => 1 byte
+        // 8 bits => 1 byte
+        // 9 bits => 2 bytes
+        int bytes_needed = ((bits_needed - 1) / 8) + 1;
+        if (bytes_needed > 8)
+            throw std::runtime_error("read_bits_int: more than 8 bytes requested");
+        char buf[8];
+        m_io->read(buf, bytes_needed);
+        for (int i = 0; i < bytes_needed; i++) {
+            uint8_t b = buf[i];
+            m_bits <<= 8;
+            m_bits |= b;
+            m_bits_left += 8;
+        }
+    }
+
+    // raw mask with required number of 1s, starting from lowest bit
+    uint64_t mask = get_mask_ones(n);
+    // shift mask to align with highest bits available in @bits
+    int shift_bits = m_bits_left - n;
+    mask <<= shift_bits;
+    // derive reading result
+    uint64_t res = (m_bits & mask) >> shift_bits;
+    // clear top bits that we've just read => AND with 1s
+    m_bits_left -= n;
+    mask = get_mask_ones(m_bits_left);
+    m_bits &= mask;
+
+    return res;
+}
+
+// Deprecated, use read_bits_int_be() instead.
+uint64_t kaitai::kstream::read_bits_int(int n) {
+    return read_bits_int_be(n);
+}
+
+uint64_t kaitai::kstream::read_bits_int_le(int n) {
+    int bits_needed = n - m_bits_left;
+    if (bits_needed > 0) {
+        // 1 bit  => 1 byte
+        // 8 bits => 1 byte
+        // 9 bits => 2 bytes
+        int bytes_needed = ((bits_needed - 1) / 8) + 1;
+        if (bytes_needed > 8)
+            throw std::runtime_error("read_bits_int_le: more than 8 bytes requested");
+        char buf[8];
+        m_io->read(buf, bytes_needed);
+        for (int i = 0; i < bytes_needed; i++) {
+            uint8_t b = buf[i];
+            m_bits |= (static_cast<uint64_t>(b) << m_bits_left);
+            m_bits_left += 8;
+        }
+    }
+
+    // raw mask with required number of 1s, starting from lowest bit
+    uint64_t mask = get_mask_ones(n);
+    // derive reading result
+    uint64_t res = m_bits & mask;
+    // remove bottom bits that we've just read by shifting
+    m_bits >>= n;
+    m_bits_left -= n;
+
+    return res;
+}
+
+uint64_t kaitai::kstream::get_mask_ones(int n) {
+    if (n == 64) {
+        return 0xFFFFFFFFFFFFFFFF;
+    } else {
+        return ((uint64_t) 1 << n) - 1;
+    }
+}
+
+// ========================================================================
+// Byte arrays
+// ========================================================================
+
+std::string kaitai::kstream::read_bytes(std::streamsize len) {
+    std::vector<char> result(len);
+
+    // NOTE: streamsize type is signed, negative values are only *supposed* to not be used.
+    // http://en.cppreference.com/w/cpp/io/streamsize
+    if (len < 0) {
+        throw std::runtime_error("read_bytes: requested a negative amount");
+    }
+
+    if (len > 0) {
+        m_io->read(&result[0], len);
+    }
+
+    return std::string(result.begin(), result.end());
+}
+
+std::string kaitai::kstream::read_bytes_full() {
+    std::iostream::pos_type p1 = m_io->tellg();
+    m_io->seekg(0, std::ios::end);
+    std::iostream::pos_type p2 = m_io->tellg();
+    size_t len = p2 - p1;
+
+    // Note: this requires a std::string to be backed with a
+    // contiguous buffer. Officially, it's a only requirement since
+    // C++11 (C++98 and C++03 didn't have this requirement), but all
+    // major implementations had contiguous buffers anyway.
+    std::string result(len, ' ');
+    m_io->seekg(p1);
+    m_io->read(&result[0], len);
+
+    return result;
+}
+
+std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) {
+    std::string result;
+    std::getline(*m_io, result, term);
+    if (m_io->eof()) {
+        // encountered EOF
+        if (eos_error) {
+            throw std::runtime_error("read_bytes_term: encountered EOF");
+        }
+    } else {
+        // encountered terminator
+        if (include)
+            result.push_back(term);
+        if (!consume)
+            m_io->unget();
+    }
+    return result;
+}
+
+std::string kaitai::kstream::ensure_fixed_contents(std::string expected) {
+    std::string actual = read_bytes(expected.length());
+
+    if (actual != expected) {
+        // NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr.
+        throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data");
+    }
+
+    return actual;
+}
+
+std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) {
+    std::size_t new_len = src.length();
+
+    while (new_len > 0 && src[new_len - 1] == pad_byte)
+        new_len--;
+
+    return src.substr(0, new_len);
+}
+
+std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) {
+    std::size_t new_len = 0;
+    std::size_t max_len = src.length();
+
+    while (new_len < max_len && src[new_len] != term)
+        new_len++;
+
+    if (include && new_len < max_len)
+        new_len++;
+
+    return src.substr(0, new_len);
+}
+
+// ========================================================================
+// Byte array processing
+// ========================================================================
+
+std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) {
+    size_t len = data.length();
+    std::string result(len, ' ');
+
+    for (size_t i = 0; i < len; i++)
+        result[i] = data[i] ^ key;
+
+    return result;
+}
+
+std::string kaitai::kstream::process_xor_many(std::string data, std::string key) {
+    size_t len = data.length();
+    size_t kl = key.length();
+    std::string result(len, ' ');
+
+    size_t ki = 0;
+    for (size_t i = 0; i < len; i++) {
+        result[i] = data[i] ^ key[ki];
+        ki++;
+        if (ki >= kl)
+            ki = 0;
+    }
+
+    return result;
+}
+
+std::string kaitai::kstream::process_rotate_left(std::string data, int amount) {
+    size_t len = data.length();
+    std::string result(len, ' ');
+
+    for (size_t i = 0; i < len; i++) {
+        uint8_t bits = data[i];
+        result[i] = (bits << amount) | (bits >> (8 - amount));
+    }
+
+    return result;
+}
+
+#ifdef KS_ZLIB
+#include <zlib.h>
+
+std::string kaitai::kstream::process_zlib(std::string data) {
+    int ret;
+
+    unsigned char *src_ptr = reinterpret_cast<unsigned char*>(&data[0]);
+    std::stringstream dst_strm;
+
+    z_stream strm;
+    strm.zalloc = Z_NULL;
+    strm.zfree = Z_NULL;
+    strm.opaque = Z_NULL;
+
+    ret = inflateInit(&strm);
+    if (ret != Z_OK)
+        throw std::runtime_error("process_zlib: inflateInit error");
+
+    strm.next_in = src_ptr;
+    strm.avail_in = data.length();
+
+    unsigned char outbuffer[ZLIB_BUF_SIZE];
+    std::string outstring;
+
+    // get the decompressed bytes blockwise using repeated calls to inflate
+    do {
+        strm.next_out = reinterpret_cast<Bytef*>(outbuffer);
+        strm.avail_out = sizeof(outbuffer);
+
+        ret = inflate(&strm, 0);
+
+        if (outstring.size() < strm.total_out)
+            outstring.append(reinterpret_cast<char*>(outbuffer), strm.total_out - outstring.size());
+    } while (ret == Z_OK);
+
+    if (ret != Z_STREAM_END) {          // an error occurred that was not EOF
+        std::ostringstream exc_msg;
+        exc_msg << "process_zlib: error #" << ret << "): " << strm.msg;
+        throw std::runtime_error(exc_msg.str());
+    }
+
+    if (inflateEnd(&strm) != Z_OK)
+        throw std::runtime_error("process_zlib: inflateEnd error");
+
+    return outstring;
+}
+#endif
+
+// ========================================================================
+// Misc utility methods
+// ========================================================================
+
+int kaitai::kstream::mod(int a, int b) {
+    if (b <= 0)
+        throw std::invalid_argument("mod: divisor b <= 0");
+    int r = a % b;
+    if (r < 0)
+        r += b;
+    return r;
+}
+
+#include <stdio.h>
+std::string kaitai::kstream::to_string(int val) {
+    // if int is 32 bits, "-2147483648" is the longest string representation
+    //   => 11 chars + zero => 12 chars
+    // if int is 64 bits, "-9223372036854775808" is the longest
+    //   => 20 chars + zero => 21 chars
+    char buf[25];
+    int got_len = snprintf(buf, sizeof(buf), "%d", val);
+
+    // should never happen, but check nonetheless
+    if (got_len > sizeof(buf))
+        throw std::invalid_argument("to_string: integer is longer than string buffer");
+
+    return std::string(buf);
+}
+
+#include <algorithm>
+std::string kaitai::kstream::reverse(std::string val) {
+    std::reverse(val.begin(), val.end());
+
+    return val;
+}
+
+uint8_t kaitai::kstream::byte_array_min(const std::string val) {
+    uint8_t min = 0xff; // UINT8_MAX
+    std::string::const_iterator end = val.end();
+    for (std::string::const_iterator it = val.begin(); it != end; ++it) {
+        uint8_t cur = static_cast<uint8_t>(*it);
+        if (cur < min) {
+            min = cur;
+        }
+    }
+    return min;
+}
+
+uint8_t kaitai::kstream::byte_array_max(const std::string val) {
+    uint8_t max = 0; // UINT8_MIN
+    std::string::const_iterator end = val.end();
+    for (std::string::const_iterator it = val.begin(); it != end; ++it) {
+        uint8_t cur = static_cast<uint8_t>(*it);
+        if (cur > max) {
+            max = cur;
+        }
+    }
+    return max;
+}
+
+// ========================================================================
+// Other internal methods
+// ========================================================================
+
+#ifndef KS_STR_DEFAULT_ENCODING
+#define KS_STR_DEFAULT_ENCODING "UTF-8"
+#endif
+
+#ifdef KS_STR_ENCODING_ICONV
+
+#include <iconv.h>
+#include <cerrno>
+#include <stdexcept>
+
+std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
+    iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str());
+
+    if (cd == (iconv_t) -1) {
+        if (errno == EINVAL) {
+            throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested");
+        } else {
+            throw std::runtime_error("bytes_to_str: error opening iconv");
+        }
+    }
+
+    size_t src_len = src.length();
+    size_t src_left = src_len;
+
+    // Start with a buffer length of double the source length.
+    size_t dst_len = src_len * 2;
+    std::string dst(dst_len, ' ');
+    size_t dst_left = dst_len;
+
+    char *src_ptr = &src[0];
+    char *dst_ptr = &dst[0];
+
+    while (true) {
+        size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left);
+
+        if (res == (size_t) -1) {
+            if (errno == E2BIG) {
+                // dst buffer is not enough to accomodate whole string
+                // enlarge the buffer and try again
+                size_t dst_used = dst_len - dst_left;
+                dst_left += dst_len;
+                dst_len += dst_len;
+                dst.resize(dst_len);
+
+                // dst.resize might have allocated destination buffer in another area
+                // of memory, thus our previous pointer "dst" will be invalid; re-point
+                // it using "dst_used".
+                dst_ptr = &dst[dst_used];
+            } else {
+                throw std::runtime_error("bytes_to_str: iconv error");
+            }
+        } else {
+            // conversion successful
+            dst.resize(dst_len - dst_left);
+            break;
+        }
+    }
+
+    if (iconv_close(cd) != 0) {
+        throw std::runtime_error("bytes_to_str: iconv close error");
+    }
+
+    return dst;
+}
+#elif defined(KS_STR_ENCODING_NONE)
+std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
+    return src;
+}
+#else
+#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE
+#endif

phonelibs/kaitai/kaitaistream.h

@@ -0,0 +1,268 @@
+#ifndef KAITAI_STREAM_H
+#define KAITAI_STREAM_H
+
+// Kaitai Struct runtime API version: x.y.z = 'xxxyyyzzz' decimal
+#define KAITAI_STRUCT_VERSION 9000L
+
+#include <istream>
+#include <sstream>
+#include <stdint.h>
+#include <sys/types.h>
+
+namespace kaitai {
+
+/**
+ * Kaitai Stream class (kaitai::kstream) is an implementation of
+ * <a href="https://doc.kaitai.io/stream_api.html">Kaitai Struct stream API</a>
+ * for C++/STL. It's implemented as a wrapper over generic STL std::istream.
+ *
+ * It provides a wide variety of simple methods to read (parse) binary
+ * representations of primitive types, such as integer and floating
+ * point numbers, byte arrays and strings, and also provides stream
+ * positioning / navigation methods with unified cross-language and
+ * cross-toolkit semantics.
+ *
+ * Typically, end users won't access Kaitai Stream class manually, but would
+ * describe a binary structure format using .ksy language and then would use
+ * Kaitai Struct compiler to generate source code in desired target language.
+ * That code, in turn, would use this class and API to do the actual parsing
+ * job.
+ */
+class kstream {
+public:
+    /**
+     * Constructs new Kaitai Stream object, wrapping a given std::istream.
+     * \param io istream object to use for this Kaitai Stream
+     */
+    kstream(std::istream* io);
+
+    /**
+     * Constructs new Kaitai Stream object, wrapping a given in-memory data
+     * buffer.
+     * \param data data buffer to use for this Kaitai Stream
+     */
+    kstream(std::string& data);
+
+    void close();
+
+    /** @name Stream positioning */
+    //@{
+    /**
+     * Check if stream pointer is at the end of stream. Note that the semantics
+     * are different from traditional STL semantics: one does *not* need to do a
+     * read (which will fail) after the actual end of the stream to trigger EOF
+     * flag, which can be accessed after that read. It is sufficient to just be
+     * at the end of the stream for this method to return true.
+     * \return "true" if we are located at the end of the stream.
+     */
+    bool is_eof() const;
+
+    /**
+     * Set stream pointer to designated position.
+     * \param pos new position (offset in bytes from the beginning of the stream)
+     */
+    void seek(uint64_t pos);
+
+    /**
+     * Get current position of a stream pointer.
+     * \return pointer position, number of bytes from the beginning of the stream
+     */
+    uint64_t pos();
+
+    /**
+     * Get total size of the stream in bytes.
+     * \return size of the stream in bytes
+     */
+    uint64_t size();
+    //@}
+
+    /** @name Integer numbers */
+    //@{
+
+    // ------------------------------------------------------------------------
+    // Signed
+    // ------------------------------------------------------------------------
+
+    int8_t read_s1();
+
+    // ........................................................................
+    // Big-endian
+    // ........................................................................
+
+    int16_t read_s2be();
+    int32_t read_s4be();
+    int64_t read_s8be();
+
+    // ........................................................................
+    // Little-endian
+    // ........................................................................
+
+    int16_t read_s2le();
+    int32_t read_s4le();
+    int64_t read_s8le();
+
+    // ------------------------------------------------------------------------
+    // Unsigned
+    // ------------------------------------------------------------------------
+
+    uint8_t read_u1();
+
+    // ........................................................................
+    // Big-endian
+    // ........................................................................
+
+    uint16_t read_u2be();
+    uint32_t read_u4be();
+    uint64_t read_u8be();
+
+    // ........................................................................
+    // Little-endian
+    // ........................................................................
+
+    uint16_t read_u2le();
+    uint32_t read_u4le();
+    uint64_t read_u8le();
+
+    //@}
+
+    /** @name Floating point numbers */
+    //@{
+
+    // ........................................................................
+    // Big-endian
+    // ........................................................................
+
+    float read_f4be();
+    double read_f8be();
+
+    // ........................................................................
+    // Little-endian
+    // ........................................................................
+
+    float read_f4le();
+    double read_f8le();
+
+    //@}
+
+    /** @name Unaligned bit values */
+    //@{
+
+    void align_to_byte();
+    uint64_t read_bits_int_be(int n);
+    uint64_t read_bits_int(int n);
+    uint64_t read_bits_int_le(int n);
+
+    //@}
+
+    /** @name Byte arrays */
+    //@{
+
+    std::string read_bytes(std::streamsize len);
+    std::string read_bytes_full();
+    std::string read_bytes_term(char term, bool include, bool consume, bool eos_error);
+    std::string ensure_fixed_contents(std::string expected);
+
+    static std::string bytes_strip_right(std::string src, char pad_byte);
+    static std::string bytes_terminate(std::string src, char term, bool include);
+    static std::string bytes_to_str(std::string src, std::string src_enc);
+
+    //@}
+
+    /** @name Byte array processing */
+    //@{
+
+    /**
+     * Performs a XOR processing with given data, XORing every byte of input with a single
+     * given value.
+     * @param data data to process
+     * @param key value to XOR with
+     * @return processed data
+     */
+    static std::string process_xor_one(std::string data, uint8_t key);
+
+    /**
+     * Performs a XOR processing with given data, XORing every byte of input with a key
+     * array, repeating key array many times, if necessary (i.e. if data array is longer
+     * than key array).
+     * @param data data to process
+     * @param key array of bytes to XOR with
+     * @return processed data
+     */
+    static std::string process_xor_many(std::string data, std::string key);
+
+    /**
+     * Performs a circular left rotation shift for a given buffer by a given amount of bits,
+     * using groups of 1 bytes each time. Right circular rotation should be performed
+     * using this procedure with corrected amount.
+     * @param data source data to process
+     * @param amount number of bits to shift by
+     * @return copy of source array with requested shift applied
+     */
+    static std::string process_rotate_left(std::string data, int amount);
+
+#ifdef KS_ZLIB
+    /**
+     * Performs an unpacking ("inflation") of zlib-compressed data with usual zlib headers.
+     * @param data data to unpack
+     * @return unpacked data
+     * @throws IOException
+     */
+    static std::string process_zlib(std::string data);
+#endif
+
+    //@}
+
+    /**
+     * Performs modulo operation between two integers: dividend `a`
+     * and divisor `b`. Divisor `b` is expected to be positive. The
+     * result is always 0 <= x <= b - 1.
+     */
+    static int mod(int a, int b);
+
+    /**
+     * Converts given integer `val` to a decimal string representation.
+     * Should be used in place of std::to_string() (which is available only
+     * since C++11) in older C++ implementations.
+     */
+    static std::string to_string(int val);
+
+    /**
+     * Reverses given string `val`, so that the first character becomes the
+     * last and the last one becomes the first. This should be used to avoid
+     * the need of local variables at the caller.
+     */
+    static std::string reverse(std::string val);
+
+    /**
+     * Finds the minimal byte in a byte array, treating bytes as
+     * unsigned values.
+     * @param val byte array to scan
+     * @return minimal byte in byte array as integer
+     */
+    static uint8_t byte_array_min(const std::string val);
+
+    /**
+     * Finds the maximal byte in a byte array, treating bytes as
+     * unsigned values.
+     * @param val byte array to scan
+     * @return maximal byte in byte array as integer
+     */
+    static uint8_t byte_array_max(const std::string val);
+
+private:
+    std::istream* m_io;
+    std::istringstream m_io_str;
+    int m_bits_left;
+    uint64_t m_bits;
+
+    void init();
+    void exceptions_enable() const;
+
+    static uint64_t get_mask_ones(int n);
+
+    static const int ZLIB_BUF_SIZE = 128 * 1024;
+};
+
+}
+
+#endif

phonelibs/kaitai/kaitaistruct.h

@@ -0,0 +1,20 @@
+#ifndef KAITAI_STRUCT_H
+#define KAITAI_STRUCT_H
+
+#include <kaitai/kaitaistream.h>
+
+namespace kaitai {
+
+class kstruct {
+public:
+    kstruct(kstream *_io) { m__io = _io; }
+    virtual ~kstruct() {}
+protected:
+    kstream *m__io;
+public:
+    kstream *_io() { return m__io; }
+};
+
+}
+
+#endif

release/files_common

@@ -283,6 +283,10 @@ selfdrive/locationd/SConscript
 selfdrive/locationd/ubloxd.cc
 selfdrive/locationd/ublox_msg.cc
 selfdrive/locationd/ublox_msg.h
+selfdrive/locationd/generated/ubx.cpp
+selfdrive/locationd/generated/ubx.h
+selfdrive/locationd/generated/gps.cpp
+selfdrive/locationd/generated/gps.h
 
 selfdrive/locationd/locationd.py
 selfdrive/locationd/paramsd.py
@@ -448,6 +452,9 @@ phonelibs/qpoases/**
 phonelibs/qrcode/*.cc
 phonelibs/qrcode/*.hpp
 
+phonelibs/kaitai/*.h
+phonelibs/kaitai/*.cpp
+
 phonelibs/libyuv/include/**
 phonelibs/libyuv/lib/**
 phonelibs/libyuv/larch64/**

selfdrive/locationd/SConscript

@@ -1,5 +1,12 @@
 Import('env', 'common', 'cereal', 'messaging')
 
-loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread']
+loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
 
-env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc"], LIBS=loc_libs)
+
+if GetOption('kaitai'):
+  generated = Dir('generated').srcnode().abspath
+  cmd = f"kaitai-struct-compiler --target cpp_stl --outdir {generated} $SOURCES"
+  env.Command(['generated/ubx.cpp', 'generated/ubx.h'], 'ubx.ksy', cmd)
+  env.Command(['generated/gps.cpp', 'generated/gps.h'], 'gps.ksy', cmd)
+
+env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs)

selfdrive/locationd/generated/gps.cpp

@@ -0,0 +1,325 @@
+// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
+
+#include "gps.h"
+#include "kaitai/exceptions.h"
+
+gps_t::gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = this;
+    m_tlm = 0;
+    m_how = 0;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::_read() {
+    m_tlm = new tlm_t(m__io, this, m__root);
+    m_how = new how_t(m__io, this, m__root);
+    n_body = true;
+    switch (how()->subframe_id()) {
+    case 1: {
+        n_body = false;
+        m_body = new subframe_1_t(m__io, this, m__root);
+        break;
+    }
+    case 2: {
+        n_body = false;
+        m_body = new subframe_2_t(m__io, this, m__root);
+        break;
+    }
+    case 3: {
+        n_body = false;
+        m_body = new subframe_3_t(m__io, this, m__root);
+        break;
+    }
+    case 4: {
+        n_body = false;
+        m_body = new subframe_4_t(m__io, this, m__root);
+        break;
+    }
+    }
+}
+
+gps_t::~gps_t() {
+    _clean_up();
+}
+
+void gps_t::_clean_up() {
+    if (m_tlm) {
+        delete m_tlm; m_tlm = 0;
+    }
+    if (m_how) {
+        delete m_how; m_how = 0;
+    }
+    if (!n_body) {
+        if (m_body) {
+            delete m_body; m_body = 0;
+        }
+    }
+}
+
+gps_t::subframe_1_t::subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+    f_af_0 = false;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::subframe_1_t::_read() {
+    m_week_no = m__io->read_bits_int_be(10);
+    m_code = m__io->read_bits_int_be(2);
+    m_sv_accuracy = m__io->read_bits_int_be(4);
+    m_sv_health = m__io->read_bits_int_be(6);
+    m_iodc_msb = m__io->read_bits_int_be(2);
+    m_l2_p_data_flag = m__io->read_bits_int_be(1);
+    m_reserved1 = m__io->read_bits_int_be(23);
+    m_reserved2 = m__io->read_bits_int_be(24);
+    m_reserved3 = m__io->read_bits_int_be(24);
+    m_reserved4 = m__io->read_bits_int_be(16);
+    m__io->align_to_byte();
+    m_t_gd = m__io->read_s1();
+    m_iodc_lsb = m__io->read_u1();
+    m_t_oc = m__io->read_u2be();
+    m_af_2 = m__io->read_s1();
+    m_af_1 = m__io->read_s2be();
+    m_af_0_sign = m__io->read_bits_int_be(1);
+    m_af_0_value = m__io->read_bits_int_be(21);
+    m_reserved5 = m__io->read_bits_int_be(2);
+}
+
+gps_t::subframe_1_t::~subframe_1_t() {
+    _clean_up();
+}
+
+void gps_t::subframe_1_t::_clean_up() {
+}
+
+int32_t gps_t::subframe_1_t::af_0() {
+    if (f_af_0)
+        return m_af_0;
+    m_af_0 = ((af_0_sign()) ? ((af_0_value() - (1 << 21))) : (af_0_value()));
+    f_af_0 = true;
+    return m_af_0;
+}
+
+gps_t::subframe_3_t::subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+    f_omega_dot = false;
+    f_idot = false;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::subframe_3_t::_read() {
+    m_c_ic = m__io->read_s2be();
+    m_omega_0 = m__io->read_s4be();
+    m_c_is = m__io->read_s2be();
+    m_i_0 = m__io->read_s4be();
+    m_c_rc = m__io->read_s2be();
+    m_omega = m__io->read_s4be();
+    m_omega_dot_sign = m__io->read_bits_int_be(1);
+    m_omega_dot_value = m__io->read_bits_int_be(23);
+    m__io->align_to_byte();
+    m_iode = m__io->read_u1();
+    m_idot_sign = m__io->read_bits_int_be(1);
+    m_idot_value = m__io->read_bits_int_be(13);
+    m_reserved = m__io->read_bits_int_be(2);
+}
+
+gps_t::subframe_3_t::~subframe_3_t() {
+    _clean_up();
+}
+
+void gps_t::subframe_3_t::_clean_up() {
+}
+
+int32_t gps_t::subframe_3_t::omega_dot() {
+    if (f_omega_dot)
+        return m_omega_dot;
+    m_omega_dot = ((omega_dot_sign()) ? ((omega_dot_value() - (1 << 23))) : (omega_dot_value()));
+    f_omega_dot = true;
+    return m_omega_dot;
+}
+
+int32_t gps_t::subframe_3_t::idot() {
+    if (f_idot)
+        return m_idot;
+    m_idot = ((idot_sign()) ? ((idot_value() - (1 << 13))) : (idot_value()));
+    f_idot = true;
+    return m_idot;
+}
+
+gps_t::subframe_4_t::subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::subframe_4_t::_read() {
+    m_data_id = m__io->read_bits_int_be(2);
+    m_page_id = m__io->read_bits_int_be(6);
+    m__io->align_to_byte();
+    n_body = true;
+    switch (page_id()) {
+    case 56: {
+        n_body = false;
+        m_body = new ionosphere_data_t(m__io, this, m__root);
+        break;
+    }
+    }
+}
+
+gps_t::subframe_4_t::~subframe_4_t() {
+    _clean_up();
+}
+
+void gps_t::subframe_4_t::_clean_up() {
+    if (!n_body) {
+        if (m_body) {
+            delete m_body; m_body = 0;
+        }
+    }
+}
+
+gps_t::subframe_4_t::ionosphere_data_t::ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::subframe_4_t::ionosphere_data_t::_read() {
+    m_a0 = m__io->read_s1();
+    m_a1 = m__io->read_s1();
+    m_a2 = m__io->read_s1();
+    m_a3 = m__io->read_s1();
+    m_b0 = m__io->read_s1();
+    m_b1 = m__io->read_s1();
+    m_b2 = m__io->read_s1();
+    m_b3 = m__io->read_s1();
+}
+
+gps_t::subframe_4_t::ionosphere_data_t::~ionosphere_data_t() {
+    _clean_up();
+}
+
+void gps_t::subframe_4_t::ionosphere_data_t::_clean_up() {
+}
+
+gps_t::how_t::how_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::how_t::_read() {
+    m_tow_count = m__io->read_bits_int_be(17);
+    m_alert = m__io->read_bits_int_be(1);
+    m_anti_spoof = m__io->read_bits_int_be(1);
+    m_subframe_id = m__io->read_bits_int_be(3);
+    m_reserved = m__io->read_bits_int_be(2);
+}
+
+gps_t::how_t::~how_t() {
+    _clean_up();
+}
+
+void gps_t::how_t::_clean_up() {
+}
+
+gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::tlm_t::_read() {
+    m_magic = m__io->read_bytes(1);
+    if (!(magic() == std::string("\x8B", 1))) {
+        throw kaitai::validation_not_equal_error<std::string>(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0"));
+    }
+    m_tlm = m__io->read_bits_int_be(14);
+    m_integrity_status = m__io->read_bits_int_be(1);
+    m_reserved = m__io->read_bits_int_be(1);
+}
+
+gps_t::tlm_t::~tlm_t() {
+    _clean_up();
+}
+
+void gps_t::tlm_t::_clean_up() {
+}
+
+gps_t::subframe_2_t::subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void gps_t::subframe_2_t::_read() {
+    m_iode = m__io->read_u1();
+    m_c_rs = m__io->read_s2be();
+    m_delta_n = m__io->read_s2be();
+    m_m_0 = m__io->read_s4be();
+    m_c_uc = m__io->read_s2be();
+    m_e = m__io->read_s4be();
+    m_c_us = m__io->read_s2be();
+    m_sqrt_a = m__io->read_u4be();
+    m_t_oe = m__io->read_u2be();
+    m_fit_interval_flag = m__io->read_bits_int_be(1);
+    m_aoda = m__io->read_bits_int_be(5);
+    m_reserved = m__io->read_bits_int_be(2);
+}
+
+gps_t::subframe_2_t::~subframe_2_t() {
+    _clean_up();
+}
+
+void gps_t::subframe_2_t::_clean_up() {
+}

selfdrive/locationd/generated/gps.h

@@ -0,0 +1,359 @@
+#ifndef GPS_H_
+#define GPS_H_
+
+// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
+
+#include "kaitai/kaitaistruct.h"
+#include <stdint.h>
+
+#if KAITAI_STRUCT_VERSION < 9000L
+#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
+#endif
+
+class gps_t : public kaitai::kstruct {
+
+public:
+    class subframe_1_t;
+    class subframe_3_t;
+    class subframe_4_t;
+    class how_t;
+    class tlm_t;
+    class subframe_2_t;
+
+    gps_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, gps_t* p__root = 0);
+
+private:
+    void _read();
+    void _clean_up();
+
+public:
+    ~gps_t();
+
+    class subframe_1_t : public kaitai::kstruct {
+
+    public:
+
+        subframe_1_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~subframe_1_t();
+
+    private:
+        bool f_af_0;
+        int32_t m_af_0;
+
+    public:
+        int32_t af_0();
+
+    private:
+        uint64_t m_week_no;
+        uint64_t m_code;
+        uint64_t m_sv_accuracy;
+        uint64_t m_sv_health;
+        uint64_t m_iodc_msb;
+        bool m_l2_p_data_flag;
+        uint64_t m_reserved1;
+        uint64_t m_reserved2;
+        uint64_t m_reserved3;
+        uint64_t m_reserved4;
+        int8_t m_t_gd;
+        uint8_t m_iodc_lsb;
+        uint16_t m_t_oc;
+        int8_t m_af_2;
+        int16_t m_af_1;
+        bool m_af_0_sign;
+        uint64_t m_af_0_value;
+        uint64_t m_reserved5;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        uint64_t week_no() const { return m_week_no; }
+        uint64_t code() const { return m_code; }
+        uint64_t sv_accuracy() const { return m_sv_accuracy; }
+        uint64_t sv_health() const { return m_sv_health; }
+        uint64_t iodc_msb() const { return m_iodc_msb; }
+        bool l2_p_data_flag() const { return m_l2_p_data_flag; }
+        uint64_t reserved1() const { return m_reserved1; }
+        uint64_t reserved2() const { return m_reserved2; }
+        uint64_t reserved3() const { return m_reserved3; }
+        uint64_t reserved4() const { return m_reserved4; }
+        int8_t t_gd() const { return m_t_gd; }
+        uint8_t iodc_lsb() const { return m_iodc_lsb; }
+        uint16_t t_oc() const { return m_t_oc; }
+        int8_t af_2() const { return m_af_2; }
+        int16_t af_1() const { return m_af_1; }
+        bool af_0_sign() const { return m_af_0_sign; }
+        uint64_t af_0_value() const { return m_af_0_value; }
+        uint64_t reserved5() const { return m_reserved5; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+    class subframe_3_t : public kaitai::kstruct {
+
+    public:
+
+        subframe_3_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~subframe_3_t();
+
+    private:
+        bool f_omega_dot;
+        int32_t m_omega_dot;
+
+    public:
+        int32_t omega_dot();
+
+    private:
+        bool f_idot;
+        int32_t m_idot;
+
+    public:
+        int32_t idot();
+
+    private:
+        int16_t m_c_ic;
+        int32_t m_omega_0;
+        int16_t m_c_is;
+        int32_t m_i_0;
+        int16_t m_c_rc;
+        int32_t m_omega;
+        bool m_omega_dot_sign;
+        uint64_t m_omega_dot_value;
+        uint8_t m_iode;
+        bool m_idot_sign;
+        uint64_t m_idot_value;
+        uint64_t m_reserved;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        int16_t c_ic() const { return m_c_ic; }
+        int32_t omega_0() const { return m_omega_0; }
+        int16_t c_is() const { return m_c_is; }
+        int32_t i_0() const { return m_i_0; }
+        int16_t c_rc() const { return m_c_rc; }
+        int32_t omega() const { return m_omega; }
+        bool omega_dot_sign() const { return m_omega_dot_sign; }
+        uint64_t omega_dot_value() const { return m_omega_dot_value; }
+        uint8_t iode() const { return m_iode; }
+        bool idot_sign() const { return m_idot_sign; }
+        uint64_t idot_value() const { return m_idot_value; }
+        uint64_t reserved() const { return m_reserved; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+    class subframe_4_t : public kaitai::kstruct {
+
+    public:
+        class ionosphere_data_t;
+
+        subframe_4_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~subframe_4_t();
+
+        class ionosphere_data_t : public kaitai::kstruct {
+
+        public:
+
+            ionosphere_data_t(kaitai::kstream* p__io, gps_t::subframe_4_t* p__parent = 0, gps_t* p__root = 0);
+
+        private:
+            void _read();
+            void _clean_up();
+
+        public:
+            ~ionosphere_data_t();
+
+        private:
+            int8_t m_a0;
+            int8_t m_a1;
+            int8_t m_a2;
+            int8_t m_a3;
+            int8_t m_b0;
+            int8_t m_b1;
+            int8_t m_b2;
+            int8_t m_b3;
+            gps_t* m__root;
+            gps_t::subframe_4_t* m__parent;
+
+        public:
+            int8_t a0() const { return m_a0; }
+            int8_t a1() const { return m_a1; }
+            int8_t a2() const { return m_a2; }
+            int8_t a3() const { return m_a3; }
+            int8_t b0() const { return m_b0; }
+            int8_t b1() const { return m_b1; }
+            int8_t b2() const { return m_b2; }
+            int8_t b3() const { return m_b3; }
+            gps_t* _root() const { return m__root; }
+            gps_t::subframe_4_t* _parent() const { return m__parent; }
+        };
+
+    private:
+        uint64_t m_data_id;
+        uint64_t m_page_id;
+        ionosphere_data_t* m_body;
+        bool n_body;
+
+    public:
+        bool _is_null_body() { body(); return n_body; };
+
+    private:
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        uint64_t data_id() const { return m_data_id; }
+        uint64_t page_id() const { return m_page_id; }
+        ionosphere_data_t* body() const { return m_body; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+    class how_t : public kaitai::kstruct {
+
+    public:
+
+        how_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~how_t();
+
+    private:
+        uint64_t m_tow_count;
+        bool m_alert;
+        bool m_anti_spoof;
+        uint64_t m_subframe_id;
+        uint64_t m_reserved;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        uint64_t tow_count() const { return m_tow_count; }
+        bool alert() const { return m_alert; }
+        bool anti_spoof() const { return m_anti_spoof; }
+        uint64_t subframe_id() const { return m_subframe_id; }
+        uint64_t reserved() const { return m_reserved; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+    class tlm_t : public kaitai::kstruct {
+
+    public:
+
+        tlm_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~tlm_t();
+
+    private:
+        std::string m_magic;
+        uint64_t m_tlm;
+        bool m_integrity_status;
+        bool m_reserved;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        std::string magic() const { return m_magic; }
+        uint64_t tlm() const { return m_tlm; }
+        bool integrity_status() const { return m_integrity_status; }
+        bool reserved() const { return m_reserved; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+    class subframe_2_t : public kaitai::kstruct {
+
+    public:
+
+        subframe_2_t(kaitai::kstream* p__io, gps_t* p__parent = 0, gps_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~subframe_2_t();
+
+    private:
+        uint8_t m_iode;
+        int16_t m_c_rs;
+        int16_t m_delta_n;
+        int32_t m_m_0;
+        int16_t m_c_uc;
+        int32_t m_e;
+        int16_t m_c_us;
+        uint32_t m_sqrt_a;
+        uint16_t m_t_oe;
+        bool m_fit_interval_flag;
+        uint64_t m_aoda;
+        uint64_t m_reserved;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        uint8_t iode() const { return m_iode; }
+        int16_t c_rs() const { return m_c_rs; }
+        int16_t delta_n() const { return m_delta_n; }
+        int32_t m_0() const { return m_m_0; }
+        int16_t c_uc() const { return m_c_uc; }
+        int32_t e() const { return m_e; }
+        int16_t c_us() const { return m_c_us; }
+        uint32_t sqrt_a() const { return m_sqrt_a; }
+        uint16_t t_oe() const { return m_t_oe; }
+        bool fit_interval_flag() const { return m_fit_interval_flag; }
+        uint64_t aoda() const { return m_aoda; }
+        uint64_t reserved() const { return m_reserved; }
+        gps_t* _root() const { return m__root; }
+        gps_t* _parent() const { return m__parent; }
+    };
+
+private:
+    tlm_t* m_tlm;
+    how_t* m_how;
+    kaitai::kstruct* m_body;
+    bool n_body;
+
+public:
+    bool _is_null_body() { body(); return n_body; };
+
+private:
+    gps_t* m__root;
+    kaitai::kstruct* m__parent;
+
+public:
+    tlm_t* tlm() const { return m_tlm; }
+    how_t* how() const { return m_how; }
+    kaitai::kstruct* body() const { return m_body; }
+    gps_t* _root() const { return m__root; }
+    kaitai::kstruct* _parent() const { return m__parent; }
+};
+
+#endif  // GPS_H_

selfdrive/locationd/generated/ubx.cpp

@@ -0,0 +1,340 @@
+// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
+
+#include "ubx.h"
+#include "kaitai/exceptions.h"
+
+ubx_t::ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = this;
+    f_checksum = false;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::_read() {
+    m_magic = m__io->read_bytes(2);
+    if (!(magic() == std::string("\xB5\x62", 2))) {
+        throw kaitai::validation_not_equal_error<std::string>(std::string("\xB5\x62", 2), magic(), _io(), std::string("/seq/0"));
+    }
+    m_msg_type = m__io->read_u2be();
+    m_length = m__io->read_u2le();
+    n_body = true;
+    switch (msg_type()) {
+    case 2569: {
+        n_body = false;
+        m_body = new mon_hw_t(m__io, this, m__root);
+        break;
+    }
+    case 533: {
+        n_body = false;
+        m_body = new rxm_rawx_t(m__io, this, m__root);
+        break;
+    }
+    case 531: {
+        n_body = false;
+        m_body = new rxm_sfrbx_t(m__io, this, m__root);
+        break;
+    }
+    case 2571: {
+        n_body = false;
+        m_body = new mon_hw2_t(m__io, this, m__root);
+        break;
+    }
+    case 263: {
+        n_body = false;
+        m_body = new nav_pvt_t(m__io, this, m__root);
+        break;
+    }
+    }
+}
+
+ubx_t::~ubx_t() {
+    _clean_up();
+}
+
+void ubx_t::_clean_up() {
+    if (!n_body) {
+        if (m_body) {
+            delete m_body; m_body = 0;
+        }
+    }
+    if (f_checksum) {
+    }
+}
+
+ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+    m_measurements = 0;
+    m__raw_measurements = 0;
+    m__io__raw_measurements = 0;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::rxm_rawx_t::_read() {
+    m_rcv_tow = m__io->read_f8le();
+    m_week = m__io->read_u2le();
+    m_leap_s = m__io->read_s1();
+    m_num_meas = m__io->read_u1();
+    m_rec_stat = m__io->read_u1();
+    m_reserved1 = m__io->read_bytes(3);
+    int l_measurements = num_meas();
+    m__raw_measurements = new std::vector<std::string>();
+    m__raw_measurements->reserve(l_measurements);
+    m__io__raw_measurements = new std::vector<kaitai::kstream*>();
+    m__io__raw_measurements->reserve(l_measurements);
+    m_measurements = new std::vector<meas_t*>();
+    m_measurements->reserve(l_measurements);
+    for (int i = 0; i < l_measurements; i++) {
+        m__raw_measurements->push_back(m__io->read_bytes(32));
+        kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1));
+        m__io__raw_measurements->push_back(io__raw_measurements);
+        m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root));
+    }
+}
+
+ubx_t::rxm_rawx_t::~rxm_rawx_t() {
+    _clean_up();
+}
+
+void ubx_t::rxm_rawx_t::_clean_up() {
+    if (m__raw_measurements) {
+        delete m__raw_measurements; m__raw_measurements = 0;
+    }
+    if (m__io__raw_measurements) {
+        for (std::vector<kaitai::kstream*>::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) {
+            delete *it;
+        }
+        delete m__io__raw_measurements; m__io__raw_measurements = 0;
+    }
+    if (m_measurements) {
+        for (std::vector<meas_t*>::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) {
+            delete *it;
+        }
+        delete m_measurements; m_measurements = 0;
+    }
+}
+
+ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::rxm_rawx_t::meas_t::_read() {
+    m_pr_mes = m__io->read_f8le();
+    m_cp_mes = m__io->read_f8le();
+    m_do_mes = m__io->read_f4le();
+    m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
+    m_sv_id = m__io->read_u1();
+    m_reserved2 = m__io->read_bytes(1);
+    m_freq_id = m__io->read_u1();
+    m_lock_time = m__io->read_u2le();
+    m_cno = m__io->read_u1();
+    m_pr_stdev = m__io->read_u1();
+    m_cp_stdev = m__io->read_u1();
+    m_do_stdev = m__io->read_u1();
+    m_trk_stat = m__io->read_u1();
+    m_reserved3 = m__io->read_bytes(1);
+}
+
+ubx_t::rxm_rawx_t::meas_t::~meas_t() {
+    _clean_up();
+}
+
+void ubx_t::rxm_rawx_t::meas_t::_clean_up() {
+}
+
+ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+    m_body = 0;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::rxm_sfrbx_t::_read() {
+    m_gnss_id = static_cast<ubx_t::gnss_type_t>(m__io->read_u1());
+    m_sv_id = m__io->read_u1();
+    m_reserved1 = m__io->read_bytes(1);
+    m_freq_id = m__io->read_u1();
+    m_num_words = m__io->read_u1();
+    m_reserved2 = m__io->read_bytes(1);
+    m_version = m__io->read_u1();
+    m_reserved3 = m__io->read_bytes(1);
+    int l_body = num_words();
+    m_body = new std::vector<uint32_t>();
+    m_body->reserve(l_body);
+    for (int i = 0; i < l_body; i++) {
+        m_body->push_back(m__io->read_u4le());
+    }
+}
+
+ubx_t::rxm_sfrbx_t::~rxm_sfrbx_t() {
+    _clean_up();
+}
+
+void ubx_t::rxm_sfrbx_t::_clean_up() {
+    if (m_body) {
+        delete m_body; m_body = 0;
+    }
+}
+
+ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::nav_pvt_t::_read() {
+    m_i_tow = m__io->read_u4le();
+    m_year = m__io->read_u2le();
+    m_month = m__io->read_u1();
+    m_day = m__io->read_u1();
+    m_hour = m__io->read_u1();
+    m_min = m__io->read_u1();
+    m_sec = m__io->read_u1();
+    m_valid = m__io->read_u1();
+    m_t_acc = m__io->read_u4le();
+    m_nano = m__io->read_s4le();
+    m_fix_type = m__io->read_u1();
+    m_flags = m__io->read_u1();
+    m_flags2 = m__io->read_u1();
+    m_num_sv = m__io->read_u1();
+    m_lon = m__io->read_s4le();
+    m_lat = m__io->read_s4le();
+    m_height = m__io->read_s4le();
+    m_h_msl = m__io->read_s4le();
+    m_h_acc = m__io->read_u4le();
+    m_v_acc = m__io->read_u4le();
+    m_vel_n = m__io->read_s4le();
+    m_vel_e = m__io->read_s4le();
+    m_vel_d = m__io->read_s4le();
+    m_g_speed = m__io->read_s4le();
+    m_head_mot = m__io->read_s4le();
+    m_s_acc = m__io->read_s4le();
+    m_head_acc = m__io->read_u4le();
+    m_p_dop = m__io->read_u2le();
+    m_flags3 = m__io->read_u1();
+    m_reserved1 = m__io->read_bytes(5);
+    m_head_veh = m__io->read_s4le();
+    m_mag_dec = m__io->read_s2le();
+    m_mag_acc = m__io->read_u2le();
+}
+
+ubx_t::nav_pvt_t::~nav_pvt_t() {
+    _clean_up();
+}
+
+void ubx_t::nav_pvt_t::_clean_up() {
+}
+
+ubx_t::mon_hw2_t::mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::mon_hw2_t::_read() {
+    m_ofs_i = m__io->read_s1();
+    m_mag_i = m__io->read_u1();
+    m_ofs_q = m__io->read_s1();
+    m_mag_q = m__io->read_u1();
+    m_cfg_source = static_cast<ubx_t::mon_hw2_t::config_source_t>(m__io->read_u1());
+    m_reserved1 = m__io->read_bytes(3);
+    m_low_lev_cfg = m__io->read_u4le();
+    m_reserved2 = m__io->read_bytes(8);
+    m_post_status = m__io->read_u4le();
+    m_reserved3 = m__io->read_bytes(4);
+}
+
+ubx_t::mon_hw2_t::~mon_hw2_t() {
+    _clean_up();
+}
+
+void ubx_t::mon_hw2_t::_clean_up() {
+}
+
+ubx_t::mon_hw_t::mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void ubx_t::mon_hw_t::_read() {
+    m_pin_sel = m__io->read_u4le();
+    m_pin_bank = m__io->read_u4le();
+    m_pin_dir = m__io->read_u4le();
+    m_pin_val = m__io->read_u4le();
+    m_noise_per_ms = m__io->read_u2le();
+    m_agc_cnt = m__io->read_u2le();
+    m_a_status = static_cast<ubx_t::mon_hw_t::antenna_status_t>(m__io->read_u1());
+    m_a_power = static_cast<ubx_t::mon_hw_t::antenna_power_t>(m__io->read_u1());
+    m_flags = m__io->read_u1();
+    m_reserved1 = m__io->read_bytes(1);
+    m_used_mask = m__io->read_u4le();
+    m_vp = m__io->read_bytes(17);
+    m_jam_ind = m__io->read_u1();
+    m_reserved2 = m__io->read_bytes(2);
+    m_pin_irq = m__io->read_u4le();
+    m_pull_h = m__io->read_u4le();
+    m_pull_l = m__io->read_u4le();
+}
+
+ubx_t::mon_hw_t::~mon_hw_t() {
+    _clean_up();
+}
+
+void ubx_t::mon_hw_t::_clean_up() {
+}
+
+uint16_t ubx_t::checksum() {
+    if (f_checksum)
+        return m_checksum;
+    std::streampos _pos = m__io->pos();
+    m__io->seek((length() + 6));
+    m_checksum = m__io->read_u2le();
+    m__io->seek(_pos);
+    f_checksum = true;
+    return m_checksum;
+}

selfdrive/locationd/generated/ubx.h

@@ -0,0 +1,410 @@
+#ifndef UBX_H_
+#define UBX_H_
+
+// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
+
+#include "kaitai/kaitaistruct.h"
+#include <stdint.h>
+#include <vector>
+
+#if KAITAI_STRUCT_VERSION < 9000L
+#error "Incompatible Kaitai Struct C++/STL API: version 0.9 or later is required"
+#endif
+
+class ubx_t : public kaitai::kstruct {
+
+public:
+    class rxm_rawx_t;
+    class rxm_sfrbx_t;
+    class nav_pvt_t;
+    class mon_hw2_t;
+    class mon_hw_t;
+
+    enum gnss_type_t {
+        GNSS_TYPE_GPS = 0,
+        GNSS_TYPE_SBAS = 1,
+        GNSS_TYPE_GALILEO = 2,
+        GNSS_TYPE_BEIDOU = 3,
+        GNSS_TYPE_IMES = 4,
+        GNSS_TYPE_QZSS = 5,
+        GNSS_TYPE_GLONASS = 6
+    };
+
+    ubx_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, ubx_t* p__root = 0);
+
+private:
+    void _read();
+    void _clean_up();
+
+public:
+    ~ubx_t();
+
+    class rxm_rawx_t : public kaitai::kstruct {
+
+    public:
+        class meas_t;
+
+        rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~rxm_rawx_t();
+
+        class meas_t : public kaitai::kstruct {
+
+        public:
+
+            meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0);
+
+        private:
+            void _read();
+            void _clean_up();
+
+        public:
+            ~meas_t();
+
+        private:
+            double m_pr_mes;
+            double m_cp_mes;
+            float m_do_mes;
+            gnss_type_t m_gnss_id;
+            uint8_t m_sv_id;
+            std::string m_reserved2;
+            uint8_t m_freq_id;
+            uint16_t m_lock_time;
+            uint8_t m_cno;
+            uint8_t m_pr_stdev;
+            uint8_t m_cp_stdev;
+            uint8_t m_do_stdev;
+            uint8_t m_trk_stat;
+            std::string m_reserved3;
+            ubx_t* m__root;
+            ubx_t::rxm_rawx_t* m__parent;
+
+        public:
+            double pr_mes() const { return m_pr_mes; }
+            double cp_mes() const { return m_cp_mes; }
+            float do_mes() const { return m_do_mes; }
+            gnss_type_t gnss_id() const { return m_gnss_id; }
+            uint8_t sv_id() const { return m_sv_id; }
+            std::string reserved2() const { return m_reserved2; }
+            uint8_t freq_id() const { return m_freq_id; }
+            uint16_t lock_time() const { return m_lock_time; }
+            uint8_t cno() const { return m_cno; }
+            uint8_t pr_stdev() const { return m_pr_stdev; }
+            uint8_t cp_stdev() const { return m_cp_stdev; }
+            uint8_t do_stdev() const { return m_do_stdev; }
+            uint8_t trk_stat() const { return m_trk_stat; }
+            std::string reserved3() const { return m_reserved3; }
+            ubx_t* _root() const { return m__root; }
+            ubx_t::rxm_rawx_t* _parent() const { return m__parent; }
+        };
+
+    private:
+        double m_rcv_tow;
+        uint16_t m_week;
+        int8_t m_leap_s;
+        uint8_t m_num_meas;
+        uint8_t m_rec_stat;
+        std::string m_reserved1;
+        std::vector<meas_t*>* m_measurements;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+        std::vector<std::string>* m__raw_measurements;
+        std::vector<kaitai::kstream*>* m__io__raw_measurements;
+
+    public:
+        double rcv_tow() const { return m_rcv_tow; }
+        uint16_t week() const { return m_week; }
+        int8_t leap_s() const { return m_leap_s; }
+        uint8_t num_meas() const { return m_num_meas; }
+        uint8_t rec_stat() const { return m_rec_stat; }
+        std::string reserved1() const { return m_reserved1; }
+        std::vector<meas_t*>* measurements() const { return m_measurements; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+        std::vector<std::string>* _raw_measurements() const { return m__raw_measurements; }
+        std::vector<kaitai::kstream*>* _io__raw_measurements() const { return m__io__raw_measurements; }
+    };
+
+    class rxm_sfrbx_t : public kaitai::kstruct {
+
+    public:
+
+        rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~rxm_sfrbx_t();
+
+    private:
+        gnss_type_t m_gnss_id;
+        uint8_t m_sv_id;
+        std::string m_reserved1;
+        uint8_t m_freq_id;
+        uint8_t m_num_words;
+        std::string m_reserved2;
+        uint8_t m_version;
+        std::string m_reserved3;
+        std::vector<uint32_t>* m_body;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+
+    public:
+        gnss_type_t gnss_id() const { return m_gnss_id; }
+        uint8_t sv_id() const { return m_sv_id; }
+        std::string reserved1() const { return m_reserved1; }
+        uint8_t freq_id() const { return m_freq_id; }
+        uint8_t num_words() const { return m_num_words; }
+        std::string reserved2() const { return m_reserved2; }
+        uint8_t version() const { return m_version; }
+        std::string reserved3() const { return m_reserved3; }
+        std::vector<uint32_t>* body() const { return m_body; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+    };
+
+    class nav_pvt_t : public kaitai::kstruct {
+
+    public:
+
+        nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~nav_pvt_t();
+
+    private:
+        uint32_t m_i_tow;
+        uint16_t m_year;
+        uint8_t m_month;
+        uint8_t m_day;
+        uint8_t m_hour;
+        uint8_t m_min;
+        uint8_t m_sec;
+        uint8_t m_valid;
+        uint32_t m_t_acc;
+        int32_t m_nano;
+        uint8_t m_fix_type;
+        uint8_t m_flags;
+        uint8_t m_flags2;
+        uint8_t m_num_sv;
+        int32_t m_lon;
+        int32_t m_lat;
+        int32_t m_height;
+        int32_t m_h_msl;
+        uint32_t m_h_acc;
+        uint32_t m_v_acc;
+        int32_t m_vel_n;
+        int32_t m_vel_e;
+        int32_t m_vel_d;
+        int32_t m_g_speed;
+        int32_t m_head_mot;
+        int32_t m_s_acc;
+        uint32_t m_head_acc;
+        uint16_t m_p_dop;
+        uint8_t m_flags3;
+        std::string m_reserved1;
+        int32_t m_head_veh;
+        int16_t m_mag_dec;
+        uint16_t m_mag_acc;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+
+    public:
+        uint32_t i_tow() const { return m_i_tow; }
+        uint16_t year() const { return m_year; }
+        uint8_t month() const { return m_month; }
+        uint8_t day() const { return m_day; }
+        uint8_t hour() const { return m_hour; }
+        uint8_t min() const { return m_min; }
+        uint8_t sec() const { return m_sec; }
+        uint8_t valid() const { return m_valid; }
+        uint32_t t_acc() const { return m_t_acc; }
+        int32_t nano() const { return m_nano; }
+        uint8_t fix_type() const { return m_fix_type; }
+        uint8_t flags() const { return m_flags; }
+        uint8_t flags2() const { return m_flags2; }
+        uint8_t num_sv() const { return m_num_sv; }
+        int32_t lon() const { return m_lon; }
+        int32_t lat() const { return m_lat; }
+        int32_t height() const { return m_height; }
+        int32_t h_msl() const { return m_h_msl; }
+        uint32_t h_acc() const { return m_h_acc; }
+        uint32_t v_acc() const { return m_v_acc; }
+        int32_t vel_n() const { return m_vel_n; }
+        int32_t vel_e() const { return m_vel_e; }
+        int32_t vel_d() const { return m_vel_d; }
+        int32_t g_speed() const { return m_g_speed; }
+        int32_t head_mot() const { return m_head_mot; }
+        int32_t s_acc() const { return m_s_acc; }
+        uint32_t head_acc() const { return m_head_acc; }
+        uint16_t p_dop() const { return m_p_dop; }
+        uint8_t flags3() const { return m_flags3; }
+        std::string reserved1() const { return m_reserved1; }
+        int32_t head_veh() const { return m_head_veh; }
+        int16_t mag_dec() const { return m_mag_dec; }
+        uint16_t mag_acc() const { return m_mag_acc; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+    };
+
+    class mon_hw2_t : public kaitai::kstruct {
+
+    public:
+
+        enum config_source_t {
+            CONFIG_SOURCE_FLASH = 102,
+            CONFIG_SOURCE_OTP = 111,
+            CONFIG_SOURCE_CONFIG_PINS = 112,
+            CONFIG_SOURCE_ROM = 113
+        };
+
+        mon_hw2_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~mon_hw2_t();
+
+    private:
+        int8_t m_ofs_i;
+        uint8_t m_mag_i;
+        int8_t m_ofs_q;
+        uint8_t m_mag_q;
+        config_source_t m_cfg_source;
+        std::string m_reserved1;
+        uint32_t m_low_lev_cfg;
+        std::string m_reserved2;
+        uint32_t m_post_status;
+        std::string m_reserved3;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+
+    public:
+        int8_t ofs_i() const { return m_ofs_i; }
+        uint8_t mag_i() const { return m_mag_i; }
+        int8_t ofs_q() const { return m_ofs_q; }
+        uint8_t mag_q() const { return m_mag_q; }
+        config_source_t cfg_source() const { return m_cfg_source; }
+        std::string reserved1() const { return m_reserved1; }
+        uint32_t low_lev_cfg() const { return m_low_lev_cfg; }
+        std::string reserved2() const { return m_reserved2; }
+        uint32_t post_status() const { return m_post_status; }
+        std::string reserved3() const { return m_reserved3; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+    };
+
+    class mon_hw_t : public kaitai::kstruct {
+
+    public:
+
+        enum antenna_status_t {
+            ANTENNA_STATUS_INIT = 0,
+            ANTENNA_STATUS_DONTKNOW = 1,
+            ANTENNA_STATUS_OK = 2,
+            ANTENNA_STATUS_SHORT = 3,
+            ANTENNA_STATUS_OPEN = 4
+        };
+
+        enum antenna_power_t {
+            ANTENNA_POWER_FALSE = 0,
+            ANTENNA_POWER_TRUE = 1,
+            ANTENNA_POWER_DONTKNOW = 2
+        };
+
+        mon_hw_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~mon_hw_t();
+
+    private:
+        uint32_t m_pin_sel;
+        uint32_t m_pin_bank;
+        uint32_t m_pin_dir;
+        uint32_t m_pin_val;
+        uint16_t m_noise_per_ms;
+        uint16_t m_agc_cnt;
+        antenna_status_t m_a_status;
+        antenna_power_t m_a_power;
+        uint8_t m_flags;
+        std::string m_reserved1;
+        uint32_t m_used_mask;
+        std::string m_vp;
+        uint8_t m_jam_ind;
+        std::string m_reserved2;
+        uint32_t m_pin_irq;
+        uint32_t m_pull_h;
+        uint32_t m_pull_l;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+
+    public:
+        uint32_t pin_sel() const { return m_pin_sel; }
+        uint32_t pin_bank() const { return m_pin_bank; }
+        uint32_t pin_dir() const { return m_pin_dir; }
+        uint32_t pin_val() const { return m_pin_val; }
+        uint16_t noise_per_ms() const { return m_noise_per_ms; }
+        uint16_t agc_cnt() const { return m_agc_cnt; }
+        antenna_status_t a_status() const { return m_a_status; }
+        antenna_power_t a_power() const { return m_a_power; }
+        uint8_t flags() const { return m_flags; }
+        std::string reserved1() const { return m_reserved1; }
+        uint32_t used_mask() const { return m_used_mask; }
+        std::string vp() const { return m_vp; }
+        uint8_t jam_ind() const { return m_jam_ind; }
+        std::string reserved2() const { return m_reserved2; }
+        uint32_t pin_irq() const { return m_pin_irq; }
+        uint32_t pull_h() const { return m_pull_h; }
+        uint32_t pull_l() const { return m_pull_l; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+    };
+
+private:
+    bool f_checksum;
+    uint16_t m_checksum;
+
+public:
+    uint16_t checksum();
+
+private:
+    std::string m_magic;
+    uint16_t m_msg_type;
+    uint16_t m_length;
+    kaitai::kstruct* m_body;
+    bool n_body;
+
+public:
+    bool _is_null_body() { body(); return n_body; };
+
+private:
+    ubx_t* m__root;
+    kaitai::kstruct* m__parent;
+
+public:
+    std::string magic() const { return m_magic; }
+    uint16_t msg_type() const { return m_msg_type; }
+    uint16_t length() const { return m_length; }
+    kaitai::kstruct* body() const { return m_body; }
+    ubx_t* _root() const { return m__root; }
+    kaitai::kstruct* _parent() const { return m__parent; }
+};
+
+#endif  // UBX_H_

selfdrive/locationd/gps.ksy

@@ -0,0 +1,189 @@
+# https://www.gps.gov/technical/icwg/IS-GPS-200E.pdf
+meta:
+  id: gps
+  endian: be
+  bit-endian: be
+seq:
+  - id: tlm
+    type: tlm
+  - id: how
+    type: how
+  - id: body
+    type:
+      switch-on: how.subframe_id
+      cases:
+        1: subframe_1
+        2: subframe_2
+        3: subframe_3
+        4: subframe_4
+types:
+  tlm:
+   seq:
+     - id: magic
+       contents: [0x8b]
+     - id: tlm
+       type: b14
+     - id: integrity_status
+       type: b1
+     - id: reserved
+       type: b1
+  how:
+    seq:
+      - id: tow_count
+        type: b17
+      - id: alert
+        type: b1
+      - id: anti_spoof
+        type: b1
+      - id: subframe_id
+        type: b3
+      - id: reserved
+        type: b2
+  subframe_1:
+    seq:
+      # Word 3
+      - id: week_no
+        type: b10
+      - id: code
+        type: b2
+      - id: sv_accuracy
+        type: b4
+      - id: sv_health
+        type: b6
+      - id: iodc_msb
+        type: b2
+      # Word 4
+      - id: l2_p_data_flag
+        type: b1
+      - id: reserved1
+        type: b23
+      # Word 5
+      - id: reserved2
+        type: b24
+      # Word 6
+      - id: reserved3
+        type: b24
+      # Word 7
+      - id: reserved4
+        type: b16
+      - id: t_gd
+        type: s1
+      # Word 8
+      - id: iodc_lsb
+        type: u1
+      - id: t_oc
+        type: u2
+      # Word 9
+      - id: af_2
+        type: s1
+      - id: af_1
+        type: s2
+      # Word 10
+      - id: af_0_sign
+        type: b1
+      - id: af_0_value
+        type: b21
+      - id: reserved5
+        type: b2
+    instances:
+      af_0:
+        value: 'af_0_sign ? (af_0_value - (1 << 21)) : af_0_value'
+  subframe_2:
+    seq:
+      # Word 3
+      - id: iode
+        type: u1
+      - id: c_rs
+        type: s2
+      # Word 4 & 5
+      - id: delta_n
+        type: s2
+      - id: m_0
+        type: s4
+      # Word 6 & 7
+      - id: c_uc
+        type: s2
+      - id: e
+        type: s4
+      # Word 8 & 9
+      - id: c_us
+        type: s2
+      - id: sqrt_a
+        type: u4
+      # Word 10
+      - id: t_oe
+        type: u2
+      - id: fit_interval_flag
+        type: b1
+      - id: aoda
+        type: b5
+      - id: reserved
+        type: b2
+  subframe_3:
+    seq:
+      # Word 3 & 4
+      - id: c_ic
+        type: s2
+      - id: omega_0
+        type: s4
+      # Word 5 & 6
+      - id: c_is
+        type: s2
+      - id: i_0
+        type: s4
+      # Word 7 & 8
+      - id: c_rc
+        type: s2
+      - id: omega
+        type: s4
+      # Word 9
+      - id: omega_dot_sign
+        type: b1
+      - id: omega_dot_value
+        type: b23
+      # Word 10
+      - id: iode
+        type: u1
+      - id: idot_sign
+        type: b1
+      - id: idot_value
+        type: b13
+      - id: reserved
+        type: b2
+    instances:
+      omega_dot:
+        value: 'omega_dot_sign ? (omega_dot_value - (1 << 23)) : omega_dot_value'
+      idot:
+        value: 'idot_sign ? (idot_value - (1 << 13)) : idot_value'
+  subframe_4:
+    seq:
+      # Word 3
+      - id: data_id
+        type: b2
+      - id: page_id
+        type: b6
+      - id: body
+        type:
+          switch-on: page_id
+          cases:
+            56: ionosphere_data
+    types:
+      ionosphere_data:
+        seq:
+          - id: a0
+            type: s1
+          - id: a1
+            type: s1
+          - id: a2
+            type: s1
+          - id: a3
+            type: s1
+          - id: b0
+            type: s1
+          - id: b1
+            type: s1
+          - id: b2
+            type: s1
+          - id: b3
+            type: s1
+

selfdrive/locationd/ublox_msg.cc

@@ -1,144 +1,29 @@
-#include <stdio.h>
-#include <stdlib.h>
+#include <cstdio>
+#include <cstdlib>
 #include <unistd.h>
-#include <math.h>
+#include <cmath>
 #include <ctime>
 #include <chrono>
+#include <iostream>
+#include <cassert>
+#include <unordered_map>
 
 #include "common/swaglog.h"
 
 #include "ublox_msg.h"
 
+const double gpsPi = 3.1415926535898;
 #define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
-#define GET_FIELD_U(w, nb, pos) (((w) >> (pos)) & ((1<<(nb))-1))
 
-namespace ublox {
-
-inline int twos_complement(uint32_t v, uint32_t nb) {
-  int sign = v >> (nb - 1);
-  int value = v;
-  if(sign != 0)
-    value = value - (1 << nb);
-  return value;
-}
-
-inline int GET_FIELD_S(uint32_t w, uint32_t nb, uint32_t pos) {
-  int v = GET_FIELD_U(w, nb, pos);
-  return twos_complement(v, nb);
-}
-
-class EphemerisData {
-  public:
-    EphemerisData(uint8_t svId, subframes_map &subframes) {
-      this->svId = svId;
-      int week_no = GET_FIELD_U(subframes[1][2+0], 10, 20);
-      int t_gd = GET_FIELD_S(subframes[1][2+4], 8, 6);
-      int iodc = (GET_FIELD_U(subframes[1][2+0], 2, 6) << 8) | GET_FIELD_U(
-        subframes[1][2+5], 8, 22);
-
-      int t_oc = GET_FIELD_U(subframes[1][2+5], 16, 6);
-      int a_f2 = GET_FIELD_S(subframes[1][2+6], 8, 22);
-      int a_f1 = GET_FIELD_S(subframes[1][2+6], 16, 6);
-      int a_f0 = GET_FIELD_S(subframes[1][2+7], 22, 8);
-
-      int c_rs = GET_FIELD_S(subframes[2][2+0], 16, 6);
-      int delta_n = GET_FIELD_S(subframes[2][2+1], 16, 14);
-      int m_0 = (GET_FIELD_S(subframes[2][2+1], 8, 6) << 24) | GET_FIELD_U(
-        subframes[2][2+2], 24, 6);
-      int c_uc = GET_FIELD_S(subframes[2][2+3], 16, 14);
-      int e = (GET_FIELD_U(subframes[2][2+3], 8, 6) << 24) | GET_FIELD_U(subframes[2][2+4], 24, 6);
-      int c_us = GET_FIELD_S(subframes[2][2+5], 16, 14);
-      uint32_t a_powhalf = (GET_FIELD_U(subframes[2][2+5], 8, 6) << 24) | GET_FIELD_U(
-        subframes[2][2+6], 24, 6);
-      int t_oe = GET_FIELD_U(subframes[2][2+7], 16, 14);
-
-      int c_ic = GET_FIELD_S(subframes[3][2+0], 16, 14);
-      int omega_0 = (GET_FIELD_S(subframes[3][2+0], 8, 6) << 24) | GET_FIELD_U(
-        subframes[3][2+1], 24, 6);
-      int c_is = GET_FIELD_S(subframes[3][2+2], 16, 14);
-      int i_0 = (GET_FIELD_S(subframes[3][2+2], 8, 6) << 24) | GET_FIELD_U(
-        subframes[3][2+3], 24, 6);
-      int c_rc = GET_FIELD_S(subframes[3][2+4], 16, 14);
-      int w = (GET_FIELD_S(subframes[3][2+4], 8, 6) << 24) | GET_FIELD_U(subframes[3][2+5], 24, 6);
-      int omega_dot = GET_FIELD_S(subframes[3][2+6], 24, 6);
-      int idot = GET_FIELD_S(subframes[3][2+7], 14, 8);
-
-      this->_rsvd1 = GET_FIELD_U(subframes[1][2+1], 23, 6);
-      this->_rsvd2 = GET_FIELD_U(subframes[1][2+2], 24, 6);
-      this->_rsvd3 = GET_FIELD_U(subframes[1][2+3], 24, 6);
-      this->_rsvd4 = GET_FIELD_U(subframes[1][2+4], 16, 14);
-      this->aodo = GET_FIELD_U(subframes[2][2+7], 5, 8);
-
-      double gpsPi = 3.1415926535898;
-
-      // now form variables in radians, meters and seconds etc
-      this->Tgd = t_gd * pow(2, -31);
-      this->A = pow(a_powhalf * pow(2, -19), 2.0);
-      this->cic = c_ic * pow(2, -29);
-      this->cis = c_is * pow(2, -29);
-      this->crc = c_rc * pow(2, -5);
-      this->crs = c_rs * pow(2, -5);
-      this->cuc = c_uc * pow(2, -29);
-      this->cus = c_us * pow(2, -29);
-      this->deltaN = delta_n * pow(2, -43) * gpsPi;
-      this->ecc = e * pow(2, -33);
-      this->i0 = i_0 * pow(2, -31) * gpsPi;
-      this->idot = idot * pow(2, -43) * gpsPi;
-      this->M0 = m_0 * pow(2, -31) * gpsPi;
-      this->omega = w * pow(2, -31) * gpsPi;
-      this->omega_dot = omega_dot * pow(2, -43) * gpsPi;
-      this->omega0 = omega_0 * pow(2, -31) * gpsPi;
-      this->toe = t_oe * pow(2, 4);
-
-      this->toc = t_oc * pow(2, 4);
-      this->gpsWeek = week_no;
-      this->af0 = a_f0 * pow(2, -31);
-      this->af1 = a_f1 * pow(2, -43);
-      this->af2 = a_f2 * pow(2, -55);
-
-      uint32_t iode1 = GET_FIELD_U(subframes[2][2+0], 8, 22);
-      uint32_t iode2 = GET_FIELD_U(subframes[3][2+7], 8, 22);
-      this->valid = (iode1 == iode2) && (iode1 == (iodc & 0xff));
-      this->iode = iode1;
-
-      if (GET_FIELD_U(subframes[4][2+0], 6, 22) == 56 &&
-        GET_FIELD_U(subframes[4][2+0], 2, 28) == 1 &&
-        GET_FIELD_U(subframes[5][2+0], 2, 28) == 1) {
-        double a0 = GET_FIELD_S(subframes[4][2], 8, 14) * pow(2, -30);
-        double a1 = GET_FIELD_S(subframes[4][2], 8, 6) * pow(2, -27);
-        double a2 = GET_FIELD_S(subframes[4][3], 8, 22) * pow(2, -24);
-        double a3 = GET_FIELD_S(subframes[4][3], 8, 14) * pow(2, -24);
-        double b0 = GET_FIELD_S(subframes[4][3], 8, 6) * pow(2, 11);
-        double b1 = GET_FIELD_S(subframes[4][4], 8, 22) * pow(2, 14);
-        double b2 = GET_FIELD_S(subframes[4][4], 8, 14) * pow(2, 16);
-        double b3 = GET_FIELD_S(subframes[4][4], 8, 6) * pow(2, 16);
-        this->ionoAlpha[0] = a0;this->ionoAlpha[1] = a1;this->ionoAlpha[2] = a2;this->ionoAlpha[3] = a3;
-        this->ionoBeta[0] = b0;this->ionoBeta[1] = b1;this->ionoBeta[2] = b2;this->ionoBeta[3] = b3;
-        this->ionoCoeffsValid = true;
-      } else {
-        this->ionoCoeffsValid = false;
-      }
-    }
-    uint16_t svId;
-    double Tgd, A, cic, cis, crc, crs, cuc, cus, deltaN, ecc, i0, idot, M0, omega, omega_dot, omega0, toe, toc;
-    uint32_t gpsWeek, iode, _rsvd1, _rsvd2, _rsvd3, _rsvd4, aodo;
-    double af0, af1, af2;
-    bool valid;
-    double ionoAlpha[4], ionoBeta[4];
-    bool ionoCoeffsValid;
-};
-
-UbloxMsgParser::UbloxMsgParser() :bytes_in_parse_buf(0) {
-  nav_frame_buffer[0U] = std::map<uint8_t, subframes_map>();
-  for(int i = 1;i < 33;i++)
-    nav_frame_buffer[0U][i] = subframes_map();
+inline static bool bit_to_bool(uint8_t val, int shifts) {
+  return (bool)(val & (1 << shifts));
 }
 
 inline int UbloxMsgParser::needed_bytes() {
   // Msg header incomplete?
-  if(bytes_in_parse_buf < UBLOX_HEADER_SIZE)
-    return UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
-  uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE;
+  if(bytes_in_parse_buf < ublox::UBLOX_HEADER_SIZE)
+    return ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE - bytes_in_parse_buf;
+  uint16_t needed = UBLOX_MSG_SIZE(msg_parse_buf) + ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE;
   // too much data
   if(needed < (uint16_t)bytes_in_parse_buf)
     return -1;
@@ -147,7 +32,7 @@ inline int UbloxMsgParser::needed_bytes() {
 
 inline bool UbloxMsgParser::valid_cheksum() {
   uint8_t ck_a = 0, ck_b = 0;
-  for(int i = 2; i < bytes_in_parse_buf - UBLOX_CHECKSUM_SIZE;i++) {
+  for(int i = 2; i < bytes_in_parse_buf - ublox::UBLOX_CHECKSUM_SIZE;i++) {
     ck_a = (ck_a + msg_parse_buf[i]) & 0xFF;
     ck_b = (ck_b + ck_a) & 0xFF;
   }
@@ -163,17 +48,15 @@ inline bool UbloxMsgParser::valid_cheksum() {
 }
 
 inline bool UbloxMsgParser::valid() {
-  return bytes_in_parse_buf >= UBLOX_HEADER_SIZE + UBLOX_CHECKSUM_SIZE &&
+  return bytes_in_parse_buf >= ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_CHECKSUM_SIZE &&
          needed_bytes() == 0 && valid_cheksum();
 }
 
 inline bool UbloxMsgParser::valid_so_far() {
-  if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != PREAMBLE1) {
-    //LOGD("PREAMBLE1 invalid, %02X.", msg_parse_buf[0]);
+  if(bytes_in_parse_buf > 0 && msg_parse_buf[0] != ublox::PREAMBLE1) {
     return false;
   }
-  if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != PREAMBLE2) {
-    //LOGD("PREAMBLE2 invalid, %02X.", msg_parse_buf[1]);
+  if(bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
     return false;
   }
   if(needed_bytes() == 0 && !valid()) {
@@ -182,192 +65,6 @@ inline bool UbloxMsgParser::valid_so_far() {
   return true;
 }
 
-kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
-  nav_pvt_msg *msg = (nav_pvt_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-  MessageBuilder msg_builder;
-  auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
-  gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
-  gpsLoc.setFlags(msg->flags);
-  gpsLoc.setLatitude(msg->lat * 1e-07);
-  gpsLoc.setLongitude(msg->lon * 1e-07);
-  gpsLoc.setAltitude(msg->height * 1e-03);
-  gpsLoc.setSpeed(msg->gSpeed * 1e-03);
-  gpsLoc.setBearingDeg(msg->headMot * 1e-5);
-  gpsLoc.setAccuracy(msg->hAcc * 1e-03);
-  std::tm timeinfo = std::tm();
-  timeinfo.tm_year = msg->year - 1900;
-  timeinfo.tm_mon = msg->month - 1;
-  timeinfo.tm_mday = msg->day;
-  timeinfo.tm_hour = msg->hour;
-  timeinfo.tm_min = msg->min;
-  timeinfo.tm_sec = msg->sec;
-  std::time_t utc_tt = timegm(&timeinfo);
-  gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano * 1e-06);
-  float f[] = { msg->velN * 1e-03f, msg->velE * 1e-03f, msg->velD * 1e-03f };
-  gpsLoc.setVNED(f);
-  gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
-  gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
-  gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05);
-  return capnp::messageToFlatArray(msg_builder);
-}
-
-inline bool bit_to_bool(uint8_t val, int shifts) {
-  return (bool)(val & (1 << shifts));
-}
-
-kj::Array<capnp::word> UbloxMsgParser::gen_raw() {
-  rxm_raw_msg *msg = (rxm_raw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-  if(bytes_in_parse_buf != (
-    UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg) + msg->numMeas * sizeof(rxm_raw_msg_extra) + UBLOX_CHECKSUM_SIZE
-    )) {
-    LOGD("Invalid measurement size %u, %u, %u, %u", msg->numMeas, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
-    return kj::Array<capnp::word>();
-  }
-  rxm_raw_msg_extra *measurements = (rxm_raw_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_raw_msg)];
-  MessageBuilder msg_builder;
-  auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport();
-  mr.setRcvTow(msg->rcvTow);
-  mr.setGpsWeek(msg->week);
-  mr.setLeapSeconds(msg->leapS);
-  mr.setGpsWeek(msg->week);
-  auto mb = mr.initMeasurements(msg->numMeas);
-  for(int8_t i = 0; i < msg->numMeas; i++) {
-    mb[i].setSvId(measurements[i].svId);
-    mb[i].setSigId(measurements[i].sigId);
-    mb[i].setPseudorange(measurements[i].prMes);
-    mb[i].setCarrierCycles(measurements[i].cpMes);
-    mb[i].setDoppler(measurements[i].doMes);
-    mb[i].setGnssId(measurements[i].gnssId);
-    mb[i].setGlonassFrequencyIndex(measurements[i].freqId);
-    mb[i].setLocktime(measurements[i].locktime);
-    mb[i].setCno(measurements[i].cno);
-    mb[i].setPseudorangeStdev(0.01*(pow(2, (measurements[i].prStdev & 15)))); // weird scaling, might be wrong
-    mb[i].setCarrierPhaseStdev(0.004*(measurements[i].cpStdev & 15));
-    mb[i].setDopplerStdev(0.002*(pow(2, (measurements[i].doStdev & 15)))); // weird scaling, might be wrong
-    auto ts = mb[i].initTrackingStatus();
-    ts.setPseudorangeValid(bit_to_bool(measurements[i].trkStat, 0));
-    ts.setCarrierPhaseValid(bit_to_bool(measurements[i].trkStat, 1));
-    ts.setHalfCycleValid(bit_to_bool(measurements[i].trkStat, 2));
-    ts.setHalfCycleSubtracted(bit_to_bool(measurements[i].trkStat, 3));
-  }
-
-  mr.setNumMeas(msg->numMeas);
-  auto rs = mr.initReceiverStatus();
-  rs.setLeapSecValid(bit_to_bool(msg->recStat, 0));
-  rs.setClkReset(bit_to_bool(msg->recStat, 2));
-  return capnp::messageToFlatArray(msg_builder);
-}
-
-kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
-  rxm_sfrbx_msg *msg = (rxm_sfrbx_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-  if(bytes_in_parse_buf != (
-    UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg) + msg->numWords * sizeof(rxm_sfrbx_msg_extra) + UBLOX_CHECKSUM_SIZE
-    )) {
-    LOGD("Invalid sfrbx words size %u, %u, %u, %u", msg->numWords, bytes_in_parse_buf, sizeof(rxm_raw_msg_extra), sizeof(rxm_raw_msg));
-    return kj::Array<capnp::word>();
-  }
-  rxm_sfrbx_msg_extra *measurements = (rxm_sfrbx_msg_extra *)&msg_parse_buf[UBLOX_HEADER_SIZE + sizeof(rxm_sfrbx_msg)];
-  if(msg->gnssId  == 0) {
-    uint8_t subframeId =  GET_FIELD_U(measurements[1].dwrd, 3, 8);
-    std::vector<uint32_t> words;
-    for(int i = 0; i < msg->numWords;i++)
-      words.push_back(measurements[i].dwrd);
-
-    subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid];
-    if (subframeId == 1) {
-      map = subframes_map();
-      map[subframeId] = words;
-    } else if (map.find(subframeId-1) != map.end()) {
-      map[subframeId] = words;
-    }
-    if(map.size() == 5) {
-      EphemerisData ephem_data(msg->svid, map);
-      MessageBuilder msg_builder;
-      auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
-      eph.setSvId(ephem_data.svId);
-      eph.setToc(ephem_data.toc);
-      eph.setGpsWeek(ephem_data.gpsWeek);
-      eph.setAf0(ephem_data.af0);
-      eph.setAf1(ephem_data.af1);
-      eph.setAf2(ephem_data.af2);
-      eph.setIode(ephem_data.iode);
-      eph.setCrs(ephem_data.crs);
-      eph.setDeltaN(ephem_data.deltaN);
-      eph.setM0(ephem_data.M0);
-      eph.setCuc(ephem_data.cuc);
-      eph.setEcc(ephem_data.ecc);
-      eph.setCus(ephem_data.cus);
-      eph.setA(ephem_data.A);
-      eph.setToe(ephem_data.toe);
-      eph.setCic(ephem_data.cic);
-      eph.setOmega0(ephem_data.omega0);
-      eph.setCis(ephem_data.cis);
-      eph.setI0(ephem_data.i0);
-      eph.setCrc(ephem_data.crc);
-      eph.setOmega(ephem_data.omega);
-      eph.setOmegaDot(ephem_data.omega_dot);
-      eph.setIDot(ephem_data.idot);
-      eph.setTgd(ephem_data.Tgd);
-      eph.setIonoCoeffsValid(ephem_data.ionoCoeffsValid);
-      if(ephem_data.ionoCoeffsValid) {
-        eph.setIonoAlpha(ephem_data.ionoAlpha);
-        eph.setIonoBeta(ephem_data.ionoBeta);
-      } else {
-        eph.setIonoAlpha(kj::ArrayPtr<const double>());
-        eph.setIonoBeta(kj::ArrayPtr<const double>());
-      }
-      return capnp::messageToFlatArray(msg_builder);
-    }
-  }
-  return kj::Array<capnp::word>();
-}
-
-kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
-  mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-
-  MessageBuilder msg_builder;
-  auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
-  hwStatus.setNoisePerMS(msg->noisePerMS);
-  hwStatus.setAgcCnt(msg->agcCnt);
-  hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus);
-  hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower);
-  hwStatus.setJamInd(msg->jamInd);
-  return capnp::messageToFlatArray(msg_builder);
-}
-
-kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2() {
-  mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-
-  MessageBuilder msg_builder;
-  auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2();
-  hwStatus.setOfsI(msg->ofsI);
-  hwStatus.setMagI(msg->magI);
-  hwStatus.setOfsQ(msg->ofsQ);
-  hwStatus.setMagQ(msg->magQ);
-
-  switch (msg->cfgSource) {
-    case 114:
-      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM);
-      break;
-    case 111:
-      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
-      break;
-    case 112:
-      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
-      break;
-    case 102:
-      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
-      break;
-    default:
-      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED);
-      break;
-  }
-
-  hwStatus.setLowLevCfg(msg->lowLevCfg);
-  hwStatus.setPostStatus(msg->postStatus);
-
-  return capnp::messageToFlatArray(msg_builder);
-}
 
 bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
   int needed = needed_bytes();
@@ -379,18 +76,267 @@ bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_da
   } else {
     bytes_consumed = incoming_data_len;
   }
+
   // Validate msg format, detect invalid header and invalid checksum.
   while(!valid_so_far() && bytes_in_parse_buf != 0) {
-    //LOGD("Drop corrupt data, remained in buf: %u", bytes_in_parse_buf);
     // Corrupted msg, drop a byte.
     bytes_in_parse_buf -= 1;
     if(bytes_in_parse_buf > 0)
       memmove(&msg_parse_buf[0], &msg_parse_buf[1], bytes_in_parse_buf);
   }
+
   // There is redundant data at the end of buffer, reset the buffer.
-  if(needed_bytes() == -1)
+  if(needed_bytes() == -1) {
     bytes_in_parse_buf = 0;
+  }
   return valid();
 }
 
+
+std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
+  std::string dat = data();
+  kaitai::kstream stream(dat);
+
+  ubx_t ubx_message(&stream);
+  auto body = ubx_message.body();
+
+  switch (ubx_message.msg_type()) {
+  case 0x0107:
+    return {"gpsLocationExternal", gen_nav_pvt(static_cast<ubx_t::nav_pvt_t*>(body))};
+    break;
+  case 0x0213:
+    return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
+    break;
+  case 0x0215:
+    return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
+    break;
+  case 0x0a09:
+    return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
+    break;
+  case 0x0a0b:
+    return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
+    break;
+  default:
+    LOGE("Unkown message type %x", ubx_message.msg_type());
+    return {"ubloxGnss", kj::Array<capnp::word>()};
+    break;
+  }
+}
+
+
+kj::Array<capnp::word> UbloxMsgParser::gen_nav_pvt(ubx_t::nav_pvt_t *msg) {
+  MessageBuilder msg_builder;
+  auto gpsLoc = msg_builder.initEvent().initGpsLocationExternal();
+  gpsLoc.setSource(cereal::GpsLocationData::SensorSource::UBLOX);
+  gpsLoc.setFlags(msg->flags());
+  gpsLoc.setLatitude(msg->lat() * 1e-07);
+  gpsLoc.setLongitude(msg->lon() * 1e-07);
+  gpsLoc.setAltitude(msg->height() * 1e-03);
+  gpsLoc.setSpeed(msg->g_speed() * 1e-03);
+  gpsLoc.setBearingDeg(msg->head_mot() * 1e-5);
+  gpsLoc.setAccuracy(msg->h_acc() * 1e-03);
+  std::tm timeinfo = std::tm();
+  timeinfo.tm_year = msg->year() - 1900;
+  timeinfo.tm_mon = msg->month() - 1;
+  timeinfo.tm_mday = msg->day();
+  timeinfo.tm_hour = msg->hour();
+  timeinfo.tm_min = msg->min();
+  timeinfo.tm_sec = msg->sec();
+
+  std::time_t utc_tt = timegm(&timeinfo);
+  gpsLoc.setTimestamp(utc_tt * 1e+03 + msg->nano() * 1e-06);
+  float f[] = { msg->vel_n() * 1e-03f, msg->vel_e() * 1e-03f, msg->vel_d() * 1e-03f };
+  gpsLoc.setVNED(f);
+  gpsLoc.setVerticalAccuracy(msg->v_acc() * 1e-03);
+  gpsLoc.setSpeedAccuracy(msg->s_acc() * 1e-03);
+  gpsLoc.setBearingAccuracyDeg(msg->head_acc() * 1e-05);
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+
+kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
+  auto body = *msg->body();
+
+  if (msg->gnss_id() == ubx_t::gnss_type_t::GNSS_TYPE_GPS) {
+    // GPS subframes are packed into 10x 4 bytes, each containing 3 actual bytes
+    // We will first need to separate the data from the padding and parity
+    assert(body.size() == 10);
+
+    std::string subframe_data;
+    subframe_data.reserve(30);
+    for (uint32_t word : body) {
+      word = word >> 6; // TODO: Verify parity
+      subframe_data.push_back(word >> 16);
+      subframe_data.push_back(word >> 8);
+      subframe_data.push_back(word >> 0);
+    }
+
+    // Collect subframes in map and parse when we have all the parts
+    kaitai::kstream stream(subframe_data);
+    gps_t subframe(&stream);
+    int subframe_id = subframe.how()->subframe_id();
+
+    if (subframe_id == 1) gps_subframes[msg->sv_id()].clear();
+    gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
+
+    if (gps_subframes[msg->sv_id()].size() == 5) {
+      MessageBuilder msg_builder;
+      auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
+      eph.setSvId(msg->sv_id());
+
+      // Subframe 1
+      {
+        kaitai::kstream stream(gps_subframes[msg->sv_id()][1]);
+        gps_t subframe(&stream);
+        gps_t::subframe_1_t* subframe_1 = static_cast<gps_t::subframe_1_t*>(subframe.body());
+
+        eph.setGpsWeek(subframe_1->week_no());
+        eph.setTgd(subframe_1->t_gd() * pow(2, -31));
+        eph.setToc(subframe_1->t_oc() * pow(2, 4));
+        eph.setAf2(subframe_1->af_2() * pow(2, -55));
+        eph.setAf1(subframe_1->af_1() * pow(2, -43));
+        eph.setAf0(subframe_1->af_0() * pow(2, -31));
+      }
+
+      // Subframe 2
+      {
+        kaitai::kstream stream(gps_subframes[msg->sv_id()][2]);
+        gps_t subframe(&stream);
+        gps_t::subframe_2_t* subframe_2 = static_cast<gps_t::subframe_2_t*>(subframe.body());
+
+        eph.setCrs(subframe_2->c_rs() * pow(2, -5));
+        eph.setDeltaN(subframe_2->delta_n() * pow(2, -43) * gpsPi);
+        eph.setM0(subframe_2->m_0() * pow(2, -31) * gpsPi);
+        eph.setCuc(subframe_2->c_uc() * pow(2, -29));
+        eph.setEcc(subframe_2->e() * pow(2, -33));
+        eph.setCus(subframe_2->c_us() * pow(2, -29));
+        eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0));
+        eph.setToe(subframe_2->t_oe() * pow(2, 4));
+      }
+
+      // Subframe 3
+      {
+        kaitai::kstream stream(gps_subframes[msg->sv_id()][3]);
+        gps_t subframe(&stream);
+        gps_t::subframe_3_t* subframe_3 = static_cast<gps_t::subframe_3_t*>(subframe.body());
+
+        eph.setCic(subframe_3->c_ic() * pow(2, -29));
+        eph.setOmega0(subframe_3->omega_0() * pow(2, -31) * gpsPi);
+        eph.setCis(subframe_3->c_is() * pow(2, -29));
+        eph.setI0(subframe_3->i_0() * pow(2, -31) * gpsPi);
+        eph.setCrc(subframe_3->c_rc() * pow(2, -5));
+        eph.setOmega(subframe_3->omega() * pow(2, -31) * gpsPi);
+        eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi);
+        eph.setIode(subframe_3->iode());
+        eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi);
+      }
+
+      // Subframe 4
+      {
+        kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
+        gps_t subframe(&stream);
+        gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(subframe.body());
+
+        // This is page 18, why is the page id 56?
+        if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) {
+          auto iono = static_cast<gps_t::subframe_4_t::ionosphere_data_t*>(subframe_4->body());
+          double a0 = iono->a0() * pow(2, -30);
+          double a1 = iono->a1() * pow(2, -27);
+          double a2 = iono->a2() * pow(2, -24);
+          double a3 = iono->a3() * pow(2, -24);
+          eph.setIonoAlpha({a0, a1, a2, a3});
+
+          double b0 = iono->b0() * pow(2, 11);
+          double b1 = iono->b1() * pow(2, 14);
+          double b2 = iono->b2() * pow(2, 16);
+          double b3 = iono->b3() * pow(2, 16);
+          eph.setIonoBeta({b0, b1, b2, b3});
+        }
+      }
+
+      return capnp::messageToFlatArray(msg_builder);
+    }
+  }
+  return kj::Array<capnp::word>();
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
+  MessageBuilder msg_builder;
+  auto mr = msg_builder.initEvent().initUbloxGnss().initMeasurementReport();
+  mr.setRcvTow(msg->rcv_tow());
+  mr.setGpsWeek(msg->week());
+  mr.setLeapSeconds(msg->leap_s());
+  mr.setGpsWeek(msg->week());
+
+  auto mb = mr.initMeasurements(msg->num_meas());
+  auto measurements = *msg->measurements();
+  for(int8_t i = 0; i < msg->num_meas(); i++) {
+    mb[i].setSvId(measurements[i]->sv_id());
+    mb[i].setPseudorange(measurements[i]->pr_mes());
+    mb[i].setCarrierCycles(measurements[i]->cp_mes());
+    mb[i].setDoppler(measurements[i]->do_mes());
+    mb[i].setGnssId(measurements[i]->gnss_id());
+    mb[i].setGlonassFrequencyIndex(measurements[i]->freq_id());
+    mb[i].setLocktime(measurements[i]->lock_time());
+    mb[i].setCno(measurements[i]->cno());
+    mb[i].setPseudorangeStdev(0.01 * (pow(2, (measurements[i]->pr_stdev() & 15)))); // weird scaling, might be wrong
+    mb[i].setCarrierPhaseStdev(0.004 * (measurements[i]->cp_stdev() & 15));
+    mb[i].setDopplerStdev(0.002 * (pow(2, (measurements[i]->do_stdev() & 15)))); // weird scaling, might be wrong
+
+    auto ts = mb[i].initTrackingStatus();
+    auto trk_stat = measurements[i]->trk_stat();
+    ts.setPseudorangeValid(bit_to_bool(trk_stat, 0));
+    ts.setCarrierPhaseValid(bit_to_bool(trk_stat, 1));
+    ts.setHalfCycleValid(bit_to_bool(trk_stat, 2));
+    ts.setHalfCycleSubtracted(bit_to_bool(trk_stat, 3));
+  }
+
+  mr.setNumMeas(msg->num_meas());
+  auto rs = mr.initReceiverStatus();
+  rs.setLeapSecValid(bit_to_bool(msg->rec_stat(), 0));
+  rs.setClkReset(bit_to_bool(msg->rec_stat(), 2));
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
+  MessageBuilder msg_builder;
+  auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
+  hwStatus.setNoisePerMS(msg->noise_per_ms());
+  hwStatus.setAgcCnt(msg->agc_cnt());
+  hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status());
+  hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power());
+  hwStatus.setJamInd(msg->jam_ind());
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2(ubx_t::mon_hw2_t *msg) {
+  MessageBuilder msg_builder;
+  auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2();
+  hwStatus.setOfsI(msg->ofs_i());
+  hwStatus.setMagI(msg->mag_i());
+  hwStatus.setOfsQ(msg->ofs_q());
+  hwStatus.setMagQ(msg->mag_q());
+
+  switch (msg->cfg_source()) {
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_ROM:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM);
+      break;
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
+      break;
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
+      break;
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
+      break;
+    default:
+      hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED);
+      break;
+  }
+
+  hwStatus.setLowLevCfg(msg->low_lev_cfg());
+  hwStatus.setPostStatus(msg->post_status());
+
+  return capnp::messageToFlatArray(msg_builder);
 }

selfdrive/locationd/ublox_msg.h

@@ -1,191 +1,52 @@
 #pragma once
 
-#include <stdint.h>
+#include <string>
+#include <cstdint>
+#include <memory>
+#include <unordered_map>
+
 #include "messaging.hpp"
+#include "generated/ubx.h"
+#include "generated/gps.h"
 
-// NAV_PVT
-typedef struct __attribute__((packed)) {
-  uint32_t iTOW;
-  uint16_t year;
-  int8_t month;
-  int8_t day;
-  int8_t hour;
-  int8_t min;
-  int8_t sec;
-  int8_t valid;
-  uint32_t tAcc;
-  int32_t nano;
-  int8_t fixType;
-  int8_t flags;
-  int8_t flags2;
-  int8_t numSV;
-  int32_t lon;
-  int32_t lat;
-  int32_t height;
-  int32_t hMSL;
-  uint32_t hAcc;
-  uint32_t vAcc;
-  int32_t velN;
-  int32_t velE;
-  int32_t velD;
-  int32_t gSpeed;
-  int32_t headMot;
-  uint32_t sAcc;
-  uint32_t headAcc;
-  uint16_t pDOP;
-  int8_t reserverd1[6];
-  int32_t headVeh;
-  int16_t magDec;
-  uint16_t magAcc;
-} nav_pvt_msg;
-
-// RXM_RAW
-typedef struct __attribute__((packed)) {
-  double rcvTow;
-  uint16_t week;
-  int8_t leapS;
-  int8_t numMeas;
-  int8_t recStat;
-  int8_t reserved1[3];
-} rxm_raw_msg;
-
-// Extra data count is in numMeas
-typedef struct __attribute__((packed)) {
-  double prMes;
-  double cpMes;
-  float doMes;
-  int8_t gnssId;
-  int8_t svId;
-  int8_t sigId;
-  int8_t freqId;
-  uint16_t locktime;
-  int8_t cno;
-  int8_t prStdev;
-  int8_t cpStdev;
-  int8_t doStdev;
-  int8_t trkStat;
-  int8_t reserved3;
-} rxm_raw_msg_extra;
-
-// RXM_SFRBX
-typedef struct __attribute__((packed)) {
-  int8_t gnssId;
-  int8_t svid;
-  int8_t reserved1;
-  int8_t freqId;
-  int8_t numWords;
-  int8_t reserved2;
-  int8_t version;
-  int8_t reserved3;
-} rxm_sfrbx_msg;
-
-// Extra data count is in numWords
-typedef struct __attribute__((packed)) {
-  uint32_t dwrd;
-} rxm_sfrbx_msg_extra;
-
-// MON_HW
-typedef struct __attribute__((packed)) {
-  uint32_t pinSel;
-  uint32_t pinBank;
-  uint32_t pinDir;
-  uint32_t pinVal;
-  uint16_t noisePerMS;
-  uint16_t agcCnt;
-  uint8_t aStatus;
-  uint8_t aPower;
-  uint8_t flags;
-  uint8_t reserved1;
-  uint32_t usedMask;
-  uint8_t VP[17];
-  uint8_t jamInd;
-  uint8_t reserved2[2];
-  uint32_t pinIrq;
-  uint32_t pullH;
-  uint32_t pullL;
-} mon_hw_msg;
-
-// MON_HW2
-typedef struct __attribute__((packed)) {
-  int8_t ofsI;
-  uint8_t magI;
-  int8_t ofsQ;
-  uint8_t magQ;
-  uint8_t cfgSource;
-  uint8_t reserved1[3];
-  uint32_t lowLevCfg;
-  uint8_t reserved2[8];
-  uint32_t postStatus;
-  uint8_t reserved3[4];
-} mon_hw2_msg;
-
+// protocol constants
 namespace ublox {
-  // protocol constants
   const uint8_t PREAMBLE1 = 0xb5;
   const uint8_t PREAMBLE2 = 0x62;
 
-  // message classes
-  const uint8_t CLASS_NAV = 0x01;
-  const uint8_t CLASS_RXM = 0x02;
-  const uint8_t CLASS_MON = 0x0A;
-
-  // NAV messages
-  const uint8_t MSG_NAV_PVT = 0x7;
-
-  // RXM messages
-  const uint8_t MSG_RXM_RAW = 0x15;
-  const uint8_t MSG_RXM_SFRBX = 0x13;
-
-  // MON messages
-  const uint8_t MSG_MON_HW = 0x09;
-  const uint8_t MSG_MON_HW2 = 0x0B;
-
   const int UBLOX_HEADER_SIZE = 6;
   const int UBLOX_CHECKSUM_SIZE = 2;
   const int UBLOX_MAX_MSG_SIZE = 65536;
 
-  typedef std::map<uint8_t, std::vector<uint32_t>> subframes_map;
-
-  class UbloxMsgParser {
-    public:
-
-      UbloxMsgParser();
-      kj::Array<capnp::word> gen_solution();
-      kj::Array<capnp::word> gen_raw();
-      kj::Array<capnp::word> gen_mon_hw();
-      kj::Array<capnp::word> gen_mon_hw2();
-
-      kj::Array<capnp::word> gen_nav_data();
-      bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
-      inline void reset() {bytes_in_parse_buf = 0;}
-      inline uint8_t msg_class() {
-        return msg_parse_buf[2];
-      }
-
-      inline uint8_t msg_id() {
-        return msg_parse_buf[3];
-      }
-      inline int needed_bytes();
-
-      void hexdump(uint8_t *d, int l) {
-        for (int i = 0; i < l; i++) {
-          if (i%0x10 == 0 && i != 0) printf("\n");
-          printf("%02X ", d[i]);
-        }
-        printf("\n");
-      }
-    private:
-      inline bool valid_cheksum();
-      inline bool valid();
-      inline bool valid_so_far();
-
-      uint8_t msg_parse_buf[UBLOX_HEADER_SIZE + UBLOX_MAX_MSG_SIZE];
-      int bytes_in_parse_buf;
-      std::map<uint8_t, std::map<uint8_t, subframes_map>> nav_frame_buffer;
-  };
-
+  // Boardd still uses these:
+  const uint8_t CLASS_NAV = 0x01;
+  const uint8_t CLASS_RXM = 0x02;
+  const uint8_t CLASS_MON = 0x0A;
 }
 
-typedef Message * (*poll_ubloxraw_msg_func)(Poller *poller);
-typedef int (*send_gps_event_func)(PubSocket *s, const void *buf, size_t len);
-int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func);
+class UbloxMsgParser {
+  public:
+    bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);
+    inline void reset() {bytes_in_parse_buf = 0;}
+    inline int needed_bytes();
+    inline std::string data() {return std::string((const char*)msg_parse_buf, bytes_in_parse_buf);}
+
+    std::pair<std::string, kj::Array<capnp::word>> gen_msg();
+    kj::Array<capnp::word> gen_nav_pvt(ubx_t::nav_pvt_t *msg);
+    kj::Array<capnp::word> gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg);
+    kj::Array<capnp::word> gen_rxm_rawx(ubx_t::rxm_rawx_t *msg);
+    kj::Array<capnp::word> gen_mon_hw(ubx_t::mon_hw_t *msg);
+    kj::Array<capnp::word> gen_mon_hw2(ubx_t::mon_hw2_t *msg);
+
+  private:
+    inline bool valid_cheksum();
+    inline bool valid();
+    inline bool valid_so_far();
+
+    std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
+
+    size_t bytes_in_parse_buf = 0;
+    uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
+
+};
+

selfdrive/locationd/ubloxd.cc

@@ -3,6 +3,7 @@
 #include "common/swaglog.h"
 
 #include "ublox_msg.h"
+#include "kaitai/kaitaistream.h"
 
 ExitHandler do_exit;
 using namespace ublox;
@@ -35,57 +36,21 @@ int main() {
     const uint8_t *data = ubloxRaw.begin();
     size_t len = ubloxRaw.size();
     size_t bytes_consumed = 0;
+
     while(bytes_consumed < len && !do_exit) {
       size_t bytes_consumed_this_time = 0U;
       if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
-        // New message available
-        if(parser.msg_class() == CLASS_NAV) {
-          if(parser.msg_id() == MSG_NAV_PVT) {
-            //LOGD("MSG_NAV_PVT");
-            auto words = parser.gen_solution();
-            if(words.size() > 0) {
-              auto bytes = words.asBytes();
-              pm.send("gpsLocationExternal", bytes.begin(), bytes.size());
-            }
-          } else
-            LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
-        } else if(parser.msg_class() == CLASS_RXM) {
-          if(parser.msg_id() == MSG_RXM_RAW) {
-            //LOGD("MSG_RXM_RAW");
-            auto words = parser.gen_raw();
-            if(words.size() > 0) {
-              auto bytes = words.asBytes();
-              pm.send("ubloxGnss", bytes.begin(), bytes.size());
-            }
-          } else if(parser.msg_id() == MSG_RXM_SFRBX) {
-            //LOGD("MSG_RXM_SFRBX");
-            auto words = parser.gen_nav_data();
-            if(words.size() > 0) {
-              auto bytes = words.asBytes();
-              pm.send("ubloxGnss", bytes.begin(), bytes.size());
-            }
-          } else
-            LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
-        } else if(parser.msg_class() == CLASS_MON) {
-          if(parser.msg_id() == MSG_MON_HW) {
-            //LOGD("MSG_MON_HW");
-            auto words = parser.gen_mon_hw();
-            if(words.size() > 0) {
-              auto bytes = words.asBytes();
-              pm.send("ubloxGnss", bytes.begin(), bytes.size());
-            }
-          } else if(parser.msg_id() == MSG_MON_HW2) {
-            //LOGD("MSG_MON_HW2");
-            auto words = parser.gen_mon_hw2();
-            if(words.size() > 0) {
-              auto bytes = words.asBytes();
-              pm.send("ubloxGnss", bytes.begin(), bytes.size());
-            }
-          } else {
-            LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
+
+        try {
+          auto msg = parser.gen_msg();
+          if (msg.second.size() > 0) {
+            auto bytes = msg.second.asBytes();
+            pm.send(msg.first.c_str(), bytes.begin(), bytes.size());
           }
-        } else
-          LOGW("Unknown msg class: 0x%02X", parser.msg_class());
+        } catch (const std::exception& e) {
+          LOGE("Error parsing ublox message %s", e.what());
+        }
+
         parser.reset();
       }
       bytes_consumed += bytes_consumed_this_time;

selfdrive/locationd/ubx.ksy

@@ -0,0 +1,259 @@
+meta:
+  id: ubx
+  endian: le
+seq:
+  - id: magic
+    contents: [0xb5, 0x62]
+  - id: msg_type
+    type: u2be
+  - id: length
+    type: u2
+  - id: body
+    type:
+      switch-on: msg_type
+      cases:
+        0x0107: nav_pvt
+        0x0213: rxm_sfrbx
+        0x0215: rxm_rawx
+        0x0a09: mon_hw
+        0x0a0b: mon_hw2
+instances:
+  checksum:
+    pos: length + 6
+    type: u2
+
+types:
+  mon_hw:
+    seq:
+      - id: pin_sel
+        type: u4
+      - id: pin_bank
+        type: u4
+      - id: pin_dir
+        type: u4
+      - id: pin_val
+        type: u4
+      - id: noise_per_ms
+        type: u2
+      - id: agc_cnt
+        type: u2
+      - id: a_status
+        type: u1
+        enum: antenna_status
+      - id: a_power
+        type: u1
+        enum: antenna_power
+      - id: flags
+        type: u1
+      - id: reserved1
+        size: 1
+      - id: used_mask
+        type: u4
+      - id: vp
+        size: 17
+      - id: jam_ind
+        type: u1
+      - id: reserved2
+        size: 2
+      - id: pin_irq
+        type: u4
+      - id: pull_h
+        type: u4
+      - id: pull_l
+        type: u4
+    enums:
+      antenna_status:
+        0: init
+        1: dontknow
+        2: ok
+        3: short
+        4: open
+      antenna_power:
+        0: off
+        1: on
+        2: dontknow
+
+  mon_hw2:
+    seq:
+      - id: ofs_i
+        type: s1
+      - id: mag_i
+        type: u1
+      - id: ofs_q
+        type: s1
+      - id: mag_q
+        type: u1
+      - id: cfg_source
+        type: u1
+        enum: config_source
+      - id: reserved1
+        size: 3
+      - id: low_lev_cfg
+        type: u4
+      - id: reserved2
+        size: 8
+      - id: post_status
+        type: u4
+      - id: reserved3
+        size: 4
+
+    enums:
+      config_source:
+        113: rom
+        111: otp
+        112: config_pins
+        102: flash
+
+  rxm_sfrbx:
+    seq:
+      - id: gnss_id
+        type: u1
+        enum: gnss_type
+      - id: sv_id
+        type: u1
+      - id: reserved1
+        size: 1
+      - id: freq_id
+        type: u1
+      - id: num_words
+        type: u1
+      - id: reserved2
+        size: 1
+      - id: version
+        type: u1
+      - id: reserved3
+        size: 1
+      - id: body
+        type: u4
+        repeat: expr
+        repeat-expr: num_words
+
+  rxm_rawx:
+    seq:
+      - id: rcv_tow
+        type: f8
+      - id: week
+        type: u2
+      - id: leap_s
+        type: s1
+      - id: num_meas
+        type: u1
+      - id: rec_stat
+        type: u1
+      - id: reserved1
+        size: 3
+      - id: measurements
+        type: meas
+        size: 32
+        repeat: expr
+        repeat-expr: num_meas
+    types:
+      meas:
+        seq:
+          - id: pr_mes
+            type: f8
+          - id: cp_mes
+            type: f8
+          - id: do_mes
+            type: f4
+          - id: gnss_id
+            type: u1
+            enum: gnss_type
+          - id: sv_id
+            type: u1
+          - id: reserved2
+            size: 1
+          - id: freq_id
+            type: u1
+          - id: lock_time
+            type: u2
+          - id: cno
+            type: u1
+          - id: pr_stdev
+            type: u1
+          - id: cp_stdev
+            type: u1
+          - id: do_stdev
+            type: u1
+          - id: trk_stat
+            type: u1
+          - id: reserved3
+            size: 1
+
+  nav_pvt:
+    seq:
+      - id: i_tow
+        type: u4
+      - id: year
+        type: u2
+      - id: month
+        type: u1
+      - id: day
+        type: u1
+      - id: hour
+        type: u1
+      - id: min
+        type: u1
+      - id: sec
+        type: u1
+      - id: valid
+        type: u1
+      - id: t_acc
+        type: u4
+      - id: nano
+        type: s4
+      - id: fix_type
+        type: u1
+      - id: flags
+        type: u1
+      - id: flags2
+        type: u1
+      - id: num_sv
+        type: u1
+      - id: lon
+        type: s4
+      - id: lat
+        type: s4
+      - id: height
+        type: s4
+      - id: h_msl
+        type: s4
+      - id: h_acc
+        type: u4
+      - id: v_acc
+        type: u4
+      - id: vel_n
+        type: s4
+      - id: vel_e
+        type: s4
+      - id: vel_d
+        type: s4
+      - id: g_speed
+        type: s4
+      - id: head_mot
+        type: s4
+      - id: s_acc
+        type: s4
+      - id: head_acc
+        type: u4
+      - id: p_dop
+        type: u2
+      - id: flags3
+        type: u1
+      - id: reserved1
+        size: 5
+      - id: head_veh
+        type: s4
+      - id: mag_dec
+        type: s2
+      - id: mag_acc
+        type: u2
+enums:
+  gnss_type:
+    0: gps
+    1: sbas
+    2: galileo
+    3: beidou
+    4: imes
+    5: qzss
+    6: glonass