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
old-commit-hash: eacacb8fba

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

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:2ac1dffc2e7aab750dc0c581366db0061b28ca25379ff7527c97a3511e92ebe9
-size 96
+oid sha256:6f0c49035e54d1c6781f4d167142d87708f646fc9afb618532a4feab095c4686
+size 185

phonelibs/kaitai/custom_decoder.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4715786d62496eec7a4099e1ab5c3bfa89a595760b1f04d7f2e09a9f19bff25b
+size 236

phonelibs/kaitai/exceptions.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:678ca5a541cea90fa93d2736ef15c0e2b5b2f8345bfd8cade118ff77f90fd41d
+size 5243

phonelibs/kaitai/kaitaistream.cpp

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8563da112b3934e3f8a9b3076b2f4e90cebde6ba3dbaf3e46e11f186fa36400e
+size 18998

phonelibs/kaitai/kaitaistream.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c986cb51e0e3bfcd16341e73b24e3db5e05f3cdc1a67a0d2d7c9523cc93f6d2b
+size 8318

phonelibs/kaitai/kaitaistruct.h

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b9f412d10c04f9cb1e99173d3a62460a1cb3ac26fcd46dee51b860d225b18c40
+size 286

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,180 +65,269 @@ 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];
+
+bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
+  int needed = needed_bytes();
+  if(needed > 0) {
+    bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
+    // Add data to buffer
+    memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
+    bytes_in_parse_buf += bytes_consumed;
+  } else {
+    bytes_consumed = incoming_data_len;
+  }
+
+  // Validate msg format, detect invalid header and invalid checksum.
+  while(!valid_so_far() && bytes_in_parse_buf != 0) {
+    // 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) {
+    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->gSpeed * 1e-03);
-  gpsLoc.setBearingDeg(msg->headMot * 1e-5);
-  gpsLoc.setAccuracy(msg->hAcc * 1e-03);
+  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;
+  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.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->vAcc * 1e-03);
-  gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
-  gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05);
+  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);
 }
 
-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));
-  }
+kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
+  auto body = *msg->body();
 
-  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);
-}
+  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);
 
-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;
+    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);
     }
-    if(map.size() == 5) {
-      EphemerisData ephem_data(msg->svid, map);
+
+    // 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(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>());
+      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_mon_hw() {
-  mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+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->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);
+  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() {
-  mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
-
+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->ofsI);
-  hwStatus.setMagI(msg->magI);
-  hwStatus.setOfsQ(msg->ofsQ);
-  hwStatus.setMagQ(msg->magQ);
+  hwStatus.setOfsI(msg->ofs_i());
+  hwStatus.setMagI(msg->mag_i());
+  hwStatus.setOfsQ(msg->ofs_q());
+  hwStatus.setMagQ(msg->mag_q());
 
-  switch (msg->cfgSource) {
-    case 114:
+  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 111:
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_OTP:
       hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP);
       break;
-    case 112:
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_CONFIG_PINS:
       hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS);
       break;
-    case 102:
+    case ubx_t::mon_hw2_t::config_source_t::CONFIG_SOURCE_FLASH:
       hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH);
       break;
     default:
@@ -363,34 +335,8 @@ kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw2() {
       break;
   }
 
-  hwStatus.setLowLevCfg(msg->lowLevCfg);
-  hwStatus.setPostStatus(msg->postStatus);
+  hwStatus.setLowLevCfg(msg->low_lev_cfg());
+  hwStatus.setPostStatus(msg->post_status());
 
   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();
-  if(needed > 0) {
-    bytes_consumed = std::min((uint32_t)needed, incoming_data_len );
-    // Add data to buffer
-    memcpy(msg_parse_buf + bytes_in_parse_buf, incoming_data, bytes_consumed);
-    bytes_in_parse_buf += bytes_consumed;
-  } 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)
-    bytes_in_parse_buf = 0;
-  return valid();
-}
-
-}

selfdrive/locationd/ublox_msg.h

@@ -1,191 +1,52 @@
 #pragma once
 
-#include <stdint.h>
-#include "messaging.hpp"
-
-// 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;
+#include <string>
+#include <cstdint>
+#include <memory>
+#include <unordered_map>
 
-// 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;
+#include "messaging.hpp"
+#include "generated/ubx.h"
+#include "generated/gps.h"
 
+// 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:
+  // Boardd still uses these:
+  const uint8_t CLASS_NAV = 0x01;
+  const uint8_t CLASS_RXM = 0x02;
+  const uint8_t CLASS_MON = 0x0A;
+}
 
-      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();
+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);}
 
-      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];
-      }
+    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);
 
-      inline uint8_t msg_id() {
-        return msg_parse_buf[3];
-      }
-      inline int needed_bytes();
+  private:
+    inline bool valid_cheksum();
+    inline bool valid();
+    inline bool valid_so_far();
 
-      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();
+    std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
 
-      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;
-  };
+    size_t bytes_in_parse_buf = 0;
+    uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
 
-}
+};
 
-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);

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