wip

system/ubloxd/SConscript

@@ -0,0 +1,20 @@
+Import('env', 'common', 'cereal', 'messaging')
+
+loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'kaitai', 'pthread']
+
+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)
+  glonass = env.Command(['generated/glonass.cpp', 'generated/glonass.h'], 'glonass.ksy', cmd)
+
+  # kaitai issue: https://github.com/kaitai-io/kaitai_struct/issues/910
+  patch = env.Command(None, 'glonass_fix.patch', 'git apply $SOURCES')
+  env.Depends(patch, glonass)
+
+glonass_obj = env.Object('generated/glonass.cpp')
+env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp",  glonass_obj], LIBS=loc_libs)
+
+if GetOption('extras'):
+  env.Program("tests/test_glonass_runner", ['tests/test_glonass_runner.cc', 'tests/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs])

system/ubloxd/generated/glonass.h

@@ -0,0 +1,375 @@
+#ifndef GLONASS_H_
+#define GLONASS_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 glonass_t : public kaitai::kstruct {
+
+public:
+    class string_4_t;
+    class string_non_immediate_t;
+    class string_5_t;
+    class string_1_t;
+    class string_2_t;
+    class string_3_t;
+
+    glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent = 0, glonass_t* p__root = 0);
+
+private:
+    void _read();
+    void _clean_up();
+
+public:
+    ~glonass_t();
+
+    class string_4_t : public kaitai::kstruct {
+
+    public:
+
+        string_4_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_4_t();
+
+    private:
+        bool f_tau_n;
+        int32_t m_tau_n;
+
+    public:
+        int32_t tau_n();
+
+    private:
+        bool f_delta_tau_n;
+        int32_t m_delta_tau_n;
+
+    public:
+        int32_t delta_tau_n();
+
+    private:
+        bool m_tau_n_sign;
+        uint64_t m_tau_n_value;
+        bool m_delta_tau_n_sign;
+        uint64_t m_delta_tau_n_value;
+        uint64_t m_e_n;
+        uint64_t m_not_used_1;
+        bool m_p4;
+        uint64_t m_f_t;
+        uint64_t m_not_used_2;
+        uint64_t m_n_t;
+        uint64_t m_n;
+        uint64_t m_m;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        bool tau_n_sign() const { return m_tau_n_sign; }
+        uint64_t tau_n_value() const { return m_tau_n_value; }
+        bool delta_tau_n_sign() const { return m_delta_tau_n_sign; }
+        uint64_t delta_tau_n_value() const { return m_delta_tau_n_value; }
+        uint64_t e_n() const { return m_e_n; }
+        uint64_t not_used_1() const { return m_not_used_1; }
+        bool p4() const { return m_p4; }
+        uint64_t f_t() const { return m_f_t; }
+        uint64_t not_used_2() const { return m_not_used_2; }
+        uint64_t n_t() const { return m_n_t; }
+        uint64_t n() const { return m_n; }
+        uint64_t m() const { return m_m; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+    class string_non_immediate_t : public kaitai::kstruct {
+
+    public:
+
+        string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_non_immediate_t();
+
+    private:
+        uint64_t m_data_1;
+        uint64_t m_data_2;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        uint64_t data_1() const { return m_data_1; }
+        uint64_t data_2() const { return m_data_2; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+    class string_5_t : public kaitai::kstruct {
+
+    public:
+
+        string_5_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_5_t();
+
+    private:
+        uint64_t m_n_a;
+        uint64_t m_tau_c;
+        bool m_not_used;
+        uint64_t m_n_4;
+        uint64_t m_tau_gps;
+        bool m_l_n;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        uint64_t n_a() const { return m_n_a; }
+        uint64_t tau_c() const { return m_tau_c; }
+        bool not_used() const { return m_not_used; }
+        uint64_t n_4() const { return m_n_4; }
+        uint64_t tau_gps() const { return m_tau_gps; }
+        bool l_n() const { return m_l_n; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+    class string_1_t : public kaitai::kstruct {
+
+    public:
+
+        string_1_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_1_t();
+
+    private:
+        bool f_x_vel;
+        int32_t m_x_vel;
+
+    public:
+        int32_t x_vel();
+
+    private:
+        bool f_x_accel;
+        int32_t m_x_accel;
+
+    public:
+        int32_t x_accel();
+
+    private:
+        bool f_x;
+        int32_t m_x;
+
+    public:
+        int32_t x();
+
+    private:
+        uint64_t m_not_used;
+        uint64_t m_p1;
+        uint64_t m_t_k;
+        bool m_x_vel_sign;
+        uint64_t m_x_vel_value;
+        bool m_x_accel_sign;
+        uint64_t m_x_accel_value;
+        bool m_x_sign;
+        uint64_t m_x_value;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        uint64_t not_used() const { return m_not_used; }
+        uint64_t p1() const { return m_p1; }
+        uint64_t t_k() const { return m_t_k; }
+        bool x_vel_sign() const { return m_x_vel_sign; }
+        uint64_t x_vel_value() const { return m_x_vel_value; }
+        bool x_accel_sign() const { return m_x_accel_sign; }
+        uint64_t x_accel_value() const { return m_x_accel_value; }
+        bool x_sign() const { return m_x_sign; }
+        uint64_t x_value() const { return m_x_value; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+    class string_2_t : public kaitai::kstruct {
+
+    public:
+
+        string_2_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_2_t();
+
+    private:
+        bool f_y_vel;
+        int32_t m_y_vel;
+
+    public:
+        int32_t y_vel();
+
+    private:
+        bool f_y_accel;
+        int32_t m_y_accel;
+
+    public:
+        int32_t y_accel();
+
+    private:
+        bool f_y;
+        int32_t m_y;
+
+    public:
+        int32_t y();
+
+    private:
+        uint64_t m_b_n;
+        bool m_p2;
+        uint64_t m_t_b;
+        uint64_t m_not_used;
+        bool m_y_vel_sign;
+        uint64_t m_y_vel_value;
+        bool m_y_accel_sign;
+        uint64_t m_y_accel_value;
+        bool m_y_sign;
+        uint64_t m_y_value;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        uint64_t b_n() const { return m_b_n; }
+        bool p2() const { return m_p2; }
+        uint64_t t_b() const { return m_t_b; }
+        uint64_t not_used() const { return m_not_used; }
+        bool y_vel_sign() const { return m_y_vel_sign; }
+        uint64_t y_vel_value() const { return m_y_vel_value; }
+        bool y_accel_sign() const { return m_y_accel_sign; }
+        uint64_t y_accel_value() const { return m_y_accel_value; }
+        bool y_sign() const { return m_y_sign; }
+        uint64_t y_value() const { return m_y_value; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+    class string_3_t : public kaitai::kstruct {
+
+    public:
+
+        string_3_t(kaitai::kstream* p__io, glonass_t* p__parent = 0, glonass_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~string_3_t();
+
+    private:
+        bool f_gamma_n;
+        int32_t m_gamma_n;
+
+    public:
+        int32_t gamma_n();
+
+    private:
+        bool f_z_vel;
+        int32_t m_z_vel;
+
+    public:
+        int32_t z_vel();
+
+    private:
+        bool f_z_accel;
+        int32_t m_z_accel;
+
+    public:
+        int32_t z_accel();
+
+    private:
+        bool f_z;
+        int32_t m_z;
+
+    public:
+        int32_t z();
+
+    private:
+        bool m_p3;
+        bool m_gamma_n_sign;
+        uint64_t m_gamma_n_value;
+        bool m_not_used;
+        uint64_t m_p;
+        bool m_l_n;
+        bool m_z_vel_sign;
+        uint64_t m_z_vel_value;
+        bool m_z_accel_sign;
+        uint64_t m_z_accel_value;
+        bool m_z_sign;
+        uint64_t m_z_value;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        bool p3() const { return m_p3; }
+        bool gamma_n_sign() const { return m_gamma_n_sign; }
+        uint64_t gamma_n_value() const { return m_gamma_n_value; }
+        bool not_used() const { return m_not_used; }
+        uint64_t p() const { return m_p; }
+        bool l_n() const { return m_l_n; }
+        bool z_vel_sign() const { return m_z_vel_sign; }
+        uint64_t z_vel_value() const { return m_z_vel_value; }
+        bool z_accel_sign() const { return m_z_accel_sign; }
+        uint64_t z_accel_value() const { return m_z_accel_value; }
+        bool z_sign() const { return m_z_sign; }
+        uint64_t z_value() const { return m_z_value; }
+        glonass_t* _root() const { return m__root; }
+        glonass_t* _parent() const { return m__parent; }
+    };
+
+private:
+    bool m_idle_chip;
+    uint64_t m_string_number;
+    kaitai::kstruct* m_data;
+    uint64_t m_hamming_code;
+    uint64_t m_pad_1;
+    uint64_t m_superframe_number;
+    uint64_t m_pad_2;
+    uint64_t m_frame_number;
+    glonass_t* m__root;
+    kaitai::kstruct* m__parent;
+
+public:
+    bool idle_chip() const { return m_idle_chip; }
+    uint64_t string_number() const { return m_string_number; }
+    kaitai::kstruct* data() const { return m_data; }
+    uint64_t hamming_code() const { return m_hamming_code; }
+    uint64_t pad_1() const { return m_pad_1; }
+    uint64_t superframe_number() const { return m_superframe_number; }
+    uint64_t pad_2() const { return m_pad_2; }
+    uint64_t frame_number() const { return m_frame_number; }
+    glonass_t* _root() const { return m__root; }
+    kaitai::kstruct* _parent() const { return m__parent; }
+};
+
+#endif  // GLONASS_H_

system/ubloxd/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_preamble;
+        uint64_t m_tlm;
+        bool m_integrity_status;
+        bool m_reserved;
+        gps_t* m__root;
+        gps_t* m__parent;
+
+    public:
+        std::string preamble() const { return m_preamble; }
+        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_

system/ubloxd/generated/ubx.h

@@ -0,0 +1,484 @@
+#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_sat_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 measurement_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 measurement_t : public kaitai::kstruct {
+
+        public:
+
+            measurement_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:
+            ~measurement_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<measurement_t*>* m_meas;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+        std::vector<std::string>* m__raw_meas;
+        std::vector<kaitai::kstream*>* m__io__raw_meas;
+
+    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<measurement_t*>* meas() const { return m_meas; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+        std::vector<std::string>* _raw_meas() const { return m__raw_meas; }
+        std::vector<kaitai::kstream*>* _io__raw_meas() const { return m__io__raw_meas; }
+    };
+
+    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_sat_t : public kaitai::kstruct {
+
+    public:
+        class nav_t;
+
+        nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0);
+
+    private:
+        void _read();
+        void _clean_up();
+
+    public:
+        ~nav_sat_t();
+
+        class nav_t : public kaitai::kstruct {
+
+        public:
+
+            nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent = 0, ubx_t* p__root = 0);
+
+        private:
+            void _read();
+            void _clean_up();
+
+        public:
+            ~nav_t();
+
+        private:
+            gnss_type_t m_gnss_id;
+            uint8_t m_sv_id;
+            uint8_t m_cno;
+            int8_t m_elev;
+            int16_t m_azim;
+            int16_t m_pr_res;
+            uint32_t m_flags;
+            ubx_t* m__root;
+            ubx_t::nav_sat_t* m__parent;
+
+        public:
+            gnss_type_t gnss_id() const { return m_gnss_id; }
+            uint8_t sv_id() const { return m_sv_id; }
+            uint8_t cno() const { return m_cno; }
+            int8_t elev() const { return m_elev; }
+            int16_t azim() const { return m_azim; }
+            int16_t pr_res() const { return m_pr_res; }
+            uint32_t flags() const { return m_flags; }
+            ubx_t* _root() const { return m__root; }
+            ubx_t::nav_sat_t* _parent() const { return m__parent; }
+        };
+
+    private:
+        uint32_t m_itow;
+        uint8_t m_version;
+        uint8_t m_num_svs;
+        std::string m_reserved;
+        std::vector<nav_t*>* m_svs;
+        ubx_t* m__root;
+        ubx_t* m__parent;
+        std::vector<std::string>* m__raw_svs;
+        std::vector<kaitai::kstream*>* m__io__raw_svs;
+
+    public:
+        uint32_t itow() const { return m_itow; }
+        uint8_t version() const { return m_version; }
+        uint8_t num_svs() const { return m_num_svs; }
+        std::string reserved() const { return m_reserved; }
+        std::vector<nav_t*>* svs() const { return m_svs; }
+        ubx_t* _root() const { return m__root; }
+        ubx_t* _parent() const { return m__parent; }
+        std::vector<std::string>* _raw_svs() const { return m__raw_svs; }
+        std::vector<kaitai::kstream*>* _io__raw_svs() const { return m__io__raw_svs; }
+    };
+
+    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_

system/ubloxd/glonass.ksy

@@ -0,0 +1,176 @@
+# http://gauss.gge.unb.ca/GLONASS.ICD.pdf
+# some variables are misprinted but good in the old doc
+# https://www.unavco.org/help/glossary/docs/ICD_GLONASS_4.0_(1998)_en.pdf
+meta:
+  id: glonass
+  endian: be
+  bit-endian: be
+seq:
+  - id: idle_chip
+    type: b1
+  - id: string_number
+    type: b4
+  - id: data
+    type:
+      switch-on: string_number
+      cases:
+        1: string_1
+        2: string_2
+        3: string_3
+        4: string_4
+        5: string_5
+        _: string_non_immediate
+  - id: hamming_code
+    type: b8
+  - id: pad_1
+    type: b11
+  - id: superframe_number
+    type: b16
+  - id: pad_2
+    type: b8
+  - id: frame_number
+    type: b8
+
+types:
+  string_1:
+    seq:
+      - id: not_used
+        type: b2
+      - id: p1
+        type: b2
+      - id: t_k
+        type: b12
+      - id: x_vel_sign
+        type: b1
+      - id: x_vel_value
+        type: b23
+      - id: x_accel_sign
+        type: b1
+      - id: x_accel_value
+        type: b4
+      - id: x_sign
+        type: b1
+      - id: x_value
+        type: b26
+    instances:
+      x_vel:
+        value: 'x_vel_sign ? (x_vel_value * (-1)) : x_vel_value'
+      x_accel:
+        value: 'x_accel_sign ? (x_accel_value * (-1)) : x_accel_value'
+      x:
+        value: 'x_sign ? (x_value * (-1)) : x_value'
+  string_2:
+    seq:
+      - id: b_n
+        type: b3
+      - id: p2
+        type: b1
+      - id: t_b
+        type: b7
+      - id: not_used
+        type: b5
+      - id: y_vel_sign
+        type: b1
+      - id: y_vel_value
+        type: b23
+      - id: y_accel_sign
+        type: b1
+      - id: y_accel_value
+        type: b4
+      - id: y_sign
+        type: b1
+      - id: y_value
+        type: b26
+    instances:
+      y_vel:
+        value: 'y_vel_sign ? (y_vel_value * (-1)) : y_vel_value'
+      y_accel:
+        value: 'y_accel_sign ? (y_accel_value * (-1)) : y_accel_value'
+      y:
+        value: 'y_sign ? (y_value * (-1)) : y_value'
+  string_3:
+    seq:
+      - id: p3
+        type: b1
+      - id: gamma_n_sign
+        type: b1
+      - id: gamma_n_value
+        type: b10
+      - id: not_used
+        type: b1
+      - id: p
+        type: b2
+      - id: l_n
+        type: b1
+      - id: z_vel_sign
+        type: b1
+      - id: z_vel_value
+        type: b23
+      - id: z_accel_sign
+        type: b1
+      - id: z_accel_value
+        type: b4
+      - id: z_sign
+        type: b1
+      - id: z_value
+        type: b26
+    instances:
+      gamma_n:
+        value: 'gamma_n_sign ? (gamma_n_value * (-1)) : gamma_n_value'
+      z_vel:
+        value: 'z_vel_sign ? (z_vel_value * (-1)) : z_vel_value'
+      z_accel:
+        value: 'z_accel_sign ? (z_accel_value * (-1)) : z_accel_value'
+      z:
+        value: 'z_sign ? (z_value * (-1)) : z_value'
+  string_4:
+    seq:
+      - id: tau_n_sign
+        type: b1
+      - id: tau_n_value
+        type: b21
+      - id: delta_tau_n_sign
+        type: b1
+      - id: delta_tau_n_value
+        type: b4
+      - id: e_n
+        type: b5
+      - id: not_used_1
+        type: b14
+      - id: p4
+        type: b1
+      - id: f_t
+        type: b4
+      - id: not_used_2
+        type: b3
+      - id: n_t
+        type: b11
+      - id: n
+        type: b5
+      - id: m
+        type: b2
+    instances:
+      tau_n:
+        value: 'tau_n_sign ? (tau_n_value * (-1)) : tau_n_value'
+      delta_tau_n:
+        value: 'delta_tau_n_sign ? (delta_tau_n_value * (-1)) : delta_tau_n_value'
+  string_5:
+    seq:
+      - id: n_a
+        type: b11
+      - id: tau_c
+        type: b32
+      - id: not_used
+        type: b1
+      - id: n_4
+        type: b5
+      - id: tau_gps
+        type: b22
+      - id: l_n
+        type: b1
+  string_non_immediate:
+    seq:
+      - id: data_1
+        type: b64
+      - id: data_2
+        type: b8

system/ubloxd/glonass_fix.patch

@@ -0,0 +1,13 @@
+diff --git a/system/ubloxd/generated/glonass.cpp b/system/ubloxd/generated/glonass.cpp
+index 5b17bc327..b5c6aa610 100644
+--- a/system/ubloxd/generated/glonass.cpp
++++ b/system/ubloxd/generated/glonass.cpp
+@@ -17,7 +17,7 @@ glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass
+ void glonass_t::_read() {
+     m_idle_chip = m__io->read_bits_int_be(1);
+     m_string_number = m__io->read_bits_int_be(4);
+-    m__io->align_to_byte();
++    //m__io->align_to_byte();
+     switch (string_number()) {
+     case 4: {
+         m_data = new string_4_t(m__io, this, m__root);

system/ubloxd/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: preamble
+       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
+

system/ubloxd/pigeond.py

@@ -0,0 +1,312 @@
+#!/usr/bin/env python3
+import sys
+import time
+import signal
+import serial
+import struct
+import requests
+import urllib.parse
+from datetime import datetime
+
+from cereal import messaging
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.system.hardware import TICI
+from openpilot.common.gpio import gpio_init, gpio_set
+from openpilot.system.hardware.tici.pins import GPIO
+
+UBLOX_TTY = "/dev/ttyHS0"
+
+UBLOX_ACK = b"\xb5\x62\x05\x01\x02\x00"
+UBLOX_NACK = b"\xb5\x62\x05\x00\x02\x00"
+UBLOX_SOS_ACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x01\x00\x00\x00"
+UBLOX_SOS_NACK = b"\xb5\x62\x09\x14\x08\x00\x02\x00\x00\x00\x00\x00\x00\x00"
+UBLOX_BACKUP_RESTORE_MSG = b"\xb5\x62\x09\x14\x08\x00\x03"
+UBLOX_ASSIST_ACK = b"\xb5\x62\x13\x60\x08\x00"
+
+def set_power(enabled: bool) -> None:
+  gpio_init(GPIO.UBLOX_SAFEBOOT_N, True)
+  gpio_init(GPIO.GNSS_PWR_EN, True)
+  gpio_init(GPIO.UBLOX_RST_N, True)
+
+  gpio_set(GPIO.UBLOX_SAFEBOOT_N, True)
+  gpio_set(GPIO.GNSS_PWR_EN, enabled)
+  gpio_set(GPIO.UBLOX_RST_N, enabled)
+
+def add_ubx_checksum(msg: bytes) -> bytes:
+  A = B = 0
+  for b in msg[2:]:
+    A = (A + b) % 256
+    B = (B + A) % 256
+  return msg + bytes([A, B])
+
+def get_assistnow_messages(token: bytes) -> list[bytes]:
+  # make request
+  # TODO: implement adding the last known location
+  r = requests.get("https://online-live2.services.u-blox.com/GetOnlineData.ashx", params=urllib.parse.urlencode({
+    'token': token,
+    'gnss': 'gps,glo',
+    'datatype': 'eph,alm,aux',
+  }, safe=':,'), timeout=5)
+  assert r.status_code == 200, "Got invalid status code"
+  dat = r.content
+
+  # split up messages
+  msgs = []
+  while len(dat) > 0:
+    assert dat[:2] == b"\xB5\x62"
+    msg_len = 6 + (dat[5] << 8 | dat[4]) + 2
+    msgs.append(dat[:msg_len])
+    dat = dat[msg_len:]
+  return msgs
+
+
+class TTYPigeon():
+  def __init__(self):
+    self.tty = serial.VTIMESerial(UBLOX_TTY, baudrate=9600, timeout=0)
+
+  def send(self, dat: bytes) -> None:
+    self.tty.write(dat)
+
+  def receive(self) -> bytes:
+    dat = b''
+    while len(dat) < 0x1000:
+      d = self.tty.read(0x40)
+      dat += d
+      if len(d) == 0:
+        break
+    return dat
+
+  def set_baud(self, baud: int) -> None:
+    self.tty.baudrate = baud
+
+  def wait_for_ack(self, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK, timeout: float = 0.5) -> bool:
+    dat = b''
+    st = time.monotonic()
+    while True:
+      dat += self.receive()
+      if ack in dat:
+        cloudlog.debug("Received ACK from ublox")
+        return True
+      elif nack in dat:
+        cloudlog.error("Received NACK from ublox")
+        return False
+      elif time.monotonic() - st > timeout:
+        cloudlog.error("No response from ublox")
+        raise TimeoutError('No response from ublox')
+      time.sleep(0.001)
+
+  def send_with_ack(self, dat: bytes, ack: bytes = UBLOX_ACK, nack: bytes = UBLOX_NACK) -> None:
+    self.send(dat)
+    self.wait_for_ack(ack, nack)
+
+  def wait_for_backup_restore_status(self, timeout: float = 1.) -> int:
+    dat = b''
+    st = time.monotonic()
+    while True:
+      dat += self.receive()
+      position = dat.find(UBLOX_BACKUP_RESTORE_MSG)
+      if position >= 0 and len(dat) >= position + 11:
+        return dat[position + 10]
+      elif time.monotonic() - st > timeout:
+        cloudlog.error("No backup restore response from ublox")
+        raise TimeoutError('No response from ublox')
+      time.sleep(0.001)
+
+  def reset_device(self) -> bool:
+    # deleting the backup does not always work on first try (mostly on second try)
+    for _ in range(5):
+      # device cold start
+      self.send(b"\xb5\x62\x06\x04\x04\x00\xff\xff\x00\x00\x0c\x5d")
+      time.sleep(1) # wait for cold start
+      init_baudrate(self)
+
+      # clear configuration
+      self.send_with_ack(b"\xb5\x62\x06\x09\x0d\x00\x1f\x1f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x17\x71\xd7")
+
+      # clear flash memory (almanac backup)
+      self.send_with_ack(b"\xB5\x62\x09\x14\x04\x00\x01\x00\x00\x00\x22\xf0")
+
+      # try restoring backup to verify it got deleted
+      self.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
+      # 1: failed to restore, 2: could restore, 3: no backup
+      status = self.wait_for_backup_restore_status()
+      if status == 1 or status == 3:
+        return True
+    return False
+
+def init_baudrate(pigeon: TTYPigeon):
+  # ublox default setting on startup is 9600 baudrate
+  pigeon.set_baud(9600)
+
+  # $PUBX,41,1,0007,0003,460800,0*15\r\n
+  pigeon.send(b"\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A")
+  time.sleep(0.1)
+  pigeon.set_baud(460800)
+
+
+def initialize_pigeon(pigeon: TTYPigeon) -> bool:
+  # try initializing a few times
+  for _ in range(10):
+    try:
+
+      # setup port config
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x00\x00\x06\x18")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x01\x08\x22")
+      pigeon.send_with_ack(b"\xb5\x62\x06\x00\x01\x00\x03\x0A\x24")
+
+      # UBX-CFG-RATE (0x06 0x08)
+      pigeon.send_with_ack(b"\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10")
+
+      # UBX-CFG-NAV5 (0x06 0x24)
+      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63")
+
+      # UBX-CFG-ODO (0x06 0x1E)
+      pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x39\x08\x00\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x28\x00\x00\x00\x00\x04\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x56\x24")
+
+      # UBX-CFG-NAV5 (0x06 0x24)
+      pigeon.send_with_ack(b"\xB5\x62\x06\x24\x00\x00\x2A\x84")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x23\x00\x00\x29\x81")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x1E\x00\x00\x24\x72")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x39\x00\x00\x3F\xC3")
+
+      # UBX-CFG-MSG (set message rate)
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74")
+      pigeon.send_with_ack(b"\xB5\x62\x06\x01\x03\x00\x01\x35\x01\x41\xAD")
+      cloudlog.debug("pigeon configured")
+
+      # try restoring almanac backup
+      pigeon.send(b"\xB5\x62\x09\x14\x00\x00\x1D\x60")
+      restore_status = pigeon.wait_for_backup_restore_status()
+      if restore_status == 2:
+        cloudlog.warning("almanac backup restored")
+      elif restore_status == 3:
+        cloudlog.warning("no almanac backup found")
+      else:
+        cloudlog.error(f"failed to restore almanac backup, status: {restore_status}")
+
+      # sending time to ublox
+      t_now = datetime.utcnow()
+      if t_now >= datetime(2021, 6, 1):
+        cloudlog.warning("Sending current time to ublox")
+
+        # UBX-MGA-INI-TIME_UTC
+        msg = add_ubx_checksum(b"\xB5\x62\x13\x40\x18\x00" + struct.pack("<BBBBHBBBBBxIHxxI",
+          0x10,
+          0x00,
+          0x00,
+          0x80,
+          t_now.year,
+          t_now.month,
+          t_now.day,
+          t_now.hour,
+          t_now.minute,
+          t_now.second,
+          0,
+          30,
+          0
+        ))
+        pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
+
+      # try getting AssistNow if we have a token
+      token = Params().get('AssistNowToken')
+      if token is not None:
+        try:
+          for msg in get_assistnow_messages(token):
+            pigeon.send_with_ack(msg, ack=UBLOX_ASSIST_ACK)
+          cloudlog.warning("AssistNow messages sent")
+        except Exception:
+          cloudlog.warning("failed to get AssistNow messages")
+
+      cloudlog.warning("Pigeon GPS on!")
+      break
+    except TimeoutError:
+      cloudlog.warning("Initialization failed, trying again!")
+  else:
+    cloudlog.warning("Failed to initialize pigeon")
+    return False
+  return True
+
+def deinitialize_and_exit(pigeon: TTYPigeon | None):
+  cloudlog.warning("Storing almanac in ublox flash")
+
+  if pigeon is not None:
+    # controlled GNSS stop
+    pigeon.send(b"\xB5\x62\x06\x04\x04\x00\x00\x00\x08\x00\x16\x74")
+
+    # store almanac in flash
+    pigeon.send(b"\xB5\x62\x09\x14\x04\x00\x00\x00\x00\x00\x21\xEC")
+    try:
+      if pigeon.wait_for_ack(ack=UBLOX_SOS_ACK, nack=UBLOX_SOS_NACK):
+        cloudlog.warning("Done storing almanac")
+      else:
+        cloudlog.error("Error storing almanac")
+    except TimeoutError:
+      pass
+
+  # turn off power and exit cleanly
+  set_power(False)
+  sys.exit(0)
+
+def create_pigeon() -> tuple[TTYPigeon, messaging.PubMaster]:
+  pigeon = None
+
+  # register exit handler
+  signal.signal(signal.SIGINT, lambda sig, frame: deinitialize_and_exit(pigeon))
+  pm = messaging.PubMaster(['ubloxRaw'])
+
+  # power cycle ublox
+  set_power(False)
+  time.sleep(0.1)
+  set_power(True)
+  time.sleep(0.5)
+
+  pigeon = TTYPigeon()
+  return pigeon, pm
+
+def run_receiving(pigeon: TTYPigeon, pm: messaging.PubMaster, duration: int = 0):
+
+  start_time = time.monotonic()
+  def end_condition():
+    return True if duration == 0 else time.monotonic() - start_time < duration
+
+  while end_condition():
+    dat = pigeon.receive()
+    if len(dat) > 0:
+      if dat[0] == 0x00:
+        cloudlog.warning("received invalid data from ublox, re-initing!")
+        init_baudrate(pigeon)
+        initialize_pigeon(pigeon)
+        continue
+
+      # send out to socket
+      msg = messaging.new_message('ubloxRaw', len(dat), valid=True)
+      msg.ubloxRaw = dat[:]
+      pm.send('ubloxRaw', msg)
+    else:
+      # prevent locking up a CPU core if ublox disconnects
+      time.sleep(0.001)
+
+
+def main():
+  assert TICI, "unsupported hardware for pigeond"
+
+  pigeon, pm = create_pigeon()
+  init_baudrate(pigeon)
+  initialize_pigeon(pigeon)
+
+  # start receiving data
+  run_receiving(pigeon, pm)
+
+if __name__ == "__main__":
+  main()

system/ubloxd/tests/print_gps_stats.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+import time
+import cereal.messaging as messaging
+
+if __name__ == "__main__":
+  sm = messaging.SubMaster(['ubloxGnss', 'gpsLocationExternal'])
+
+  while 1:
+    ug = sm['ubloxGnss']
+    gle = sm['gpsLocationExternal']
+
+    try:
+      cnos = []
+      for m in ug.measurementReport.measurements:
+        cnos.append(m.cno)
+      print("Sats: %d   Accuracy: %.2f m   cnos" % (ug.measurementReport.numMeas, gle.horizontalAccuracy), sorted(cnos))
+    except Exception:
+      pass
+    sm.update()
+    time.sleep(0.1)

system/ubloxd/tests/test_glonass_kaitai.cc

@@ -0,0 +1,360 @@
+#include <iostream>
+#include <vector>
+#include <bitset>
+#include <cassert>
+#include <cstdlib>
+#include <ctime>
+
+#include "catch2/catch.hpp"
+#include "system/ubloxd/generated/glonass.h"
+
+typedef std::vector<std::pair<int, int64_t>> string_data;
+
+#define IDLE_CHIP_IDX 0
+#define STRING_NUMBER_IDX 1
+// string data 1-5
+#define HC_IDX 0
+#define PAD1_IDX 1
+#define SUPERFRAME_IDX 2
+#define PAD2_IDX 3
+#define FRAME_IDX 4
+
+// Indexes for string number 1
+#define ST1_NU_IDX 2
+#define ST1_P1_IDX 3
+#define ST1_T_K_IDX 4
+#define ST1_X_VEL_S_IDX 5
+#define ST1_X_VEL_V_IDX 6
+#define ST1_X_ACCEL_S_IDX 7
+#define ST1_X_ACCEL_V_IDX 8
+#define ST1_X_S_IDX 9
+#define ST1_X_V_IDX 10
+#define ST1_HC_OFF 11
+
+// Indexes for string number 2
+#define ST2_BN_IDX 2
+#define ST2_P2_IDX 3
+#define ST2_TB_IDX 4
+#define ST2_NU_IDX 5
+#define ST2_Y_VEL_S_IDX 6
+#define ST2_Y_VEL_V_IDX 7
+#define ST2_Y_ACCEL_S_IDX 8
+#define ST2_Y_ACCEL_V_IDX 9
+#define ST2_Y_S_IDX 10
+#define ST2_Y_V_IDX 11
+#define ST2_HC_OFF 12
+
+// Indexes for string number 3
+#define ST3_P3_IDX 2
+#define ST3_GAMMA_N_S_IDX 3
+#define ST3_GAMMA_N_V_IDX 4
+#define ST3_NU_1_IDX 5
+#define ST3_P_IDX 6
+#define ST3_L_N_IDX 7
+#define ST3_Z_VEL_S_IDX 8
+#define ST3_Z_VEL_V_IDX 9
+#define ST3_Z_ACCEL_S_IDX 10
+#define ST3_Z_ACCEL_V_IDX 11
+#define ST3_Z_S_IDX 12
+#define ST3_Z_V_IDX 13
+#define ST3_HC_OFF 14
+
+// Indexes for string number 4
+#define ST4_TAU_N_S_IDX 2
+#define ST4_TAU_N_V_IDX 3
+#define ST4_DELTA_TAU_N_S_IDX 4
+#define ST4_DELTA_TAU_N_V_IDX 5
+#define ST4_E_N_IDX 6
+#define ST4_NU_1_IDX 7
+#define ST4_P4_IDX 8
+#define ST4_F_T_IDX 9
+#define ST4_NU_2_IDX 10
+#define ST4_N_T_IDX 11
+#define ST4_N_IDX 12
+#define ST4_M_IDX 13
+#define ST4_HC_OFF 14
+
+// Indexes for string number 5
+#define ST5_N_A_IDX 2
+#define ST5_TAU_C_IDX 3
+#define ST5_NU_IDX 4
+#define ST5_N_4_IDX 5
+#define ST5_TAU_GPS_IDX 6
+#define ST5_L_N_IDX 7
+#define ST5_HC_OFF 8
+
+// Indexes for non immediate
+#define ST6_DATA_1_IDX 2
+#define ST6_DATA_2_IDX 3
+#define ST6_HC_OFF 4
+
+
+std::string generate_inp_data(string_data& data) {
+  std::string inp_data = "";
+  for (auto& [b, v] : data) {
+    std::string tmp = std::bitset<64>(v).to_string();
+    inp_data += tmp.substr(64-b, b);
+  }
+  assert(inp_data.size() == 128);
+
+  std::string string_data;
+  string_data.reserve(16);
+  for (int i = 0; i < 128; i+=8) {
+    std::string substr = inp_data.substr(i, 8);
+    string_data.push_back((uint8_t)std::stoi(substr.c_str(), 0, 2));
+  }
+
+  return string_data;
+}
+
+string_data generate_string_data(uint8_t string_number) {
+
+  srand((unsigned)time(0));
+  string_data data; //<bit length, value>
+  data.push_back({1, 0}); // idle chip
+  data.push_back({4, string_number}); // string number
+
+  if (string_number == 1) {
+    data.push_back({2, 3}); // not_used
+    data.push_back({2, 1}); // p1
+    data.push_back({12, 113}); // t_k
+    data.push_back({1, rand() & 1}); // x_vel_sign
+    data.push_back({23, 7122}); // x_vel_value
+    data.push_back({1, rand() & 1}); // x_accel_sign
+    data.push_back({4, 3}); // x_accel_value
+    data.push_back({1, rand() & 1}); // x_sign
+    data.push_back({26, 33554431}); // x_value
+  } else if (string_number == 2) {
+    data.push_back({3, 3}); // b_n
+    data.push_back({1, 1}); // p2
+    data.push_back({7, 123}); // t_b
+    data.push_back({5, 31}); // not_used
+    data.push_back({1, rand() & 1}); // y_vel_sign
+    data.push_back({23, 7422}); // y_vel_value
+    data.push_back({1, rand() & 1}); // y_accel_sign
+    data.push_back({4, 3}); // y_accel_value
+    data.push_back({1, rand() & 1}); // y_sign
+    data.push_back({26, 67108863}); // y_value
+  } else if (string_number == 3) {
+    data.push_back({1, 0}); // p3
+    data.push_back({1, 1}); // gamma_n_sign
+    data.push_back({10, 123}); // gamma_n_value
+    data.push_back({1, 0}); // not_used
+    data.push_back({2, 2}); // p
+    data.push_back({1, 1}); // l_n
+    data.push_back({1, rand() & 1}); // z_vel_sign
+    data.push_back({23, 1337}); // z_vel_value
+    data.push_back({1, rand() & 1}); // z_accel_sign
+    data.push_back({4, 9}); // z_accel_value
+    data.push_back({1, rand() & 1}); // z_sign
+    data.push_back({26, 100023}); // z_value
+  } else if (string_number == 4) {
+    data.push_back({1, rand() & 1}); // tau_n_sign
+    data.push_back({21, 197152}); // tau_n_value
+    data.push_back({1, rand() & 1}); // delta_tau_n_sign
+    data.push_back({4, 4}); // delta_tau_n_value
+    data.push_back({5, 0}); // e_n
+    data.push_back({14, 2}); // not_used_1
+    data.push_back({1, 1}); // p4
+    data.push_back({4, 9}); // f_t
+    data.push_back({3, 3}); // not_used_2
+    data.push_back({11, 2047}); // n_t
+    data.push_back({5, 2}); // n
+    data.push_back({2, 1}); // m
+  } else if (string_number == 5) {
+    data.push_back({11, 2047}); // n_a
+    data.push_back({32, 4294767295}); // tau_c
+    data.push_back({1, 0}); // not_used_1
+    data.push_back({5, 2}); // n_4
+    data.push_back({22, 4114304}); // tau_gps
+    data.push_back({1, 0}); // l_n
+  } else { // non-immediate data is not parsed
+    data.push_back({64, rand()}); // data_1
+    data.push_back({8, 6}); // data_2
+  }
+
+  data.push_back({8, rand() & 0xFF}); // hamming code
+  data.push_back({11, rand() & 0x7FF}); // pad
+  data.push_back({16, rand() & 0xFFFF}); // superframe
+  data.push_back({8, rand() & 0xFF}); // pad
+  data.push_back({8, rand() & 0xFF}); // frame
+  return data;
+}
+
+TEST_CASE("parse_string_number_1"){
+  string_data data = generate_string_data(1);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST1_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST1_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST1_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST1_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST1_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream str1(inp_data);
+  glonass_t str1_data(&str1);
+  glonass_t::string_1_t* s1 = static_cast<glonass_t::string_1_t*>(str1_data.data());
+
+  REQUIRE(s1->not_used() == data[ST1_NU_IDX].second);
+  REQUIRE(s1->p1() == data[ST1_P1_IDX].second);
+  REQUIRE(s1->t_k() == data[ST1_T_K_IDX].second);
+
+  int mul = s1->x_vel_sign() ? (-1) : 1;
+  REQUIRE(s1->x_vel() == (data[ST1_X_VEL_V_IDX].second * mul));
+  mul = s1->x_accel_sign() ? (-1) : 1;
+  REQUIRE(s1->x_accel() == (data[ST1_X_ACCEL_V_IDX].second * mul));
+  mul = s1->x_sign() ? (-1) : 1;
+  REQUIRE(s1->x() == (data[ST1_X_V_IDX].second * mul));
+}
+
+TEST_CASE("parse_string_number_2"){
+  string_data data = generate_string_data(2);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST2_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST2_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST2_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST2_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST2_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream str2(inp_data);
+  glonass_t str2_data(&str2);
+  glonass_t::string_2_t* s2 = static_cast<glonass_t::string_2_t*>(str2_data.data());
+
+  REQUIRE(s2->b_n() == data[ST2_BN_IDX].second);
+  REQUIRE(s2->not_used() == data[ST2_NU_IDX].second);
+  REQUIRE(s2->p2() == data[ST2_P2_IDX].second);
+  REQUIRE(s2->t_b() == data[ST2_TB_IDX].second);
+  int mul = s2->y_vel_sign() ? (-1) : 1;
+  REQUIRE(s2->y_vel() == (data[ST2_Y_VEL_V_IDX].second * mul));
+  mul = s2->y_accel_sign() ? (-1) : 1;
+  REQUIRE(s2->y_accel() == (data[ST2_Y_ACCEL_V_IDX].second * mul));
+  mul = s2->y_sign() ? (-1) : 1;
+  REQUIRE(s2->y() == (data[ST2_Y_V_IDX].second * mul));
+}
+
+TEST_CASE("parse_string_number_3"){
+  string_data data = generate_string_data(3);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST3_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST3_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST3_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST3_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST3_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream str3(inp_data);
+  glonass_t str3_data(&str3);
+  glonass_t::string_3_t* s3 = static_cast<glonass_t::string_3_t*>(str3_data.data());
+
+  REQUIRE(s3->p3() == data[ST3_P3_IDX].second);
+  int mul = s3->gamma_n_sign() ? (-1) : 1;
+  REQUIRE(s3->gamma_n() == (data[ST3_GAMMA_N_V_IDX].second * mul));
+  REQUIRE(s3->not_used() == data[ST3_NU_1_IDX].second);
+  REQUIRE(s3->p() == data[ST3_P_IDX].second);
+  REQUIRE(s3->l_n() == data[ST3_L_N_IDX].second);
+  mul = s3->z_vel_sign() ? (-1) : 1;
+  REQUIRE(s3->z_vel() == (data[ST3_Z_VEL_V_IDX].second * mul));
+  mul = s3->z_accel_sign() ? (-1) : 1;
+  REQUIRE(s3->z_accel() == (data[ST3_Z_ACCEL_V_IDX].second * mul));
+  mul = s3->z_sign() ? (-1) : 1;
+  REQUIRE(s3->z() == (data[ST3_Z_V_IDX].second * mul));
+}
+
+TEST_CASE("parse_string_number_4"){
+  string_data data = generate_string_data(4);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST4_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST4_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST4_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST4_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST4_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream str4(inp_data);
+  glonass_t str4_data(&str4);
+  glonass_t::string_4_t* s4 = static_cast<glonass_t::string_4_t*>(str4_data.data());
+
+  int mul = s4->tau_n_sign() ? (-1) : 1;
+  REQUIRE(s4->tau_n() == (data[ST4_TAU_N_V_IDX].second * mul));
+  mul = s4->delta_tau_n_sign() ? (-1) : 1;
+  REQUIRE(s4->delta_tau_n() == (data[ST4_DELTA_TAU_N_V_IDX].second * mul));
+  REQUIRE(s4->e_n() == data[ST4_E_N_IDX].second);
+  REQUIRE(s4->not_used_1() == data[ST4_NU_1_IDX].second);
+  REQUIRE(s4->p4() == data[ST4_P4_IDX].second);
+  REQUIRE(s4->f_t() == data[ST4_F_T_IDX].second);
+  REQUIRE(s4->not_used_2() == data[ST4_NU_2_IDX].second);
+  REQUIRE(s4->n_t() == data[ST4_N_T_IDX].second);
+  REQUIRE(s4->n() == data[ST4_N_IDX].second);
+  REQUIRE(s4->m() == data[ST4_M_IDX].second);
+}
+
+TEST_CASE("parse_string_number_5"){
+  string_data data = generate_string_data(5);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST5_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST5_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST5_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST5_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST5_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream str5(inp_data);
+  glonass_t str5_data(&str5);
+  glonass_t::string_5_t* s5 = static_cast<glonass_t::string_5_t*>(str5_data.data());
+
+  REQUIRE(s5->n_a() == data[ST5_N_A_IDX].second);
+  REQUIRE(s5->tau_c() == data[ST5_TAU_C_IDX].second);
+  REQUIRE(s5->not_used() == data[ST5_NU_IDX].second);
+  REQUIRE(s5->n_4() == data[ST5_N_4_IDX].second);
+  REQUIRE(s5->tau_gps() == data[ST5_TAU_GPS_IDX].second);
+  REQUIRE(s5->l_n() == data[ST5_L_N_IDX].second);
+}
+
+TEST_CASE("parse_string_number_NI"){
+  string_data data = generate_string_data((rand() % 10) +  6);
+  std::string inp_data = generate_inp_data(data);
+
+  kaitai::kstream stream(inp_data);
+  glonass_t gl_string(&stream);
+
+  REQUIRE(gl_string.idle_chip() == data[IDLE_CHIP_IDX].second);
+  REQUIRE(gl_string.string_number() == data[STRING_NUMBER_IDX].second);
+  REQUIRE(gl_string.hamming_code() == data[ST6_HC_OFF + HC_IDX].second);
+  REQUIRE(gl_string.pad_1() == data[ST6_HC_OFF + PAD1_IDX].second);
+  REQUIRE(gl_string.superframe_number() == data[ST6_HC_OFF + SUPERFRAME_IDX].second);
+  REQUIRE(gl_string.pad_2() == data[ST6_HC_OFF + PAD2_IDX].second);
+  REQUIRE(gl_string.frame_number() == data[ST6_HC_OFF + FRAME_IDX].second);
+
+  kaitai::kstream strni(inp_data);
+  glonass_t strni_data(&strni);
+  glonass_t::string_non_immediate_t* sni = static_cast<glonass_t::string_non_immediate_t*>(strni_data.data());
+
+  REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second);
+  REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second);
+}

system/ubloxd/tests/test_glonass_runner.cc

@@ -0,0 +1,2 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"

system/ubloxd/tests/test_pigeond.py

@@ -0,0 +1,60 @@
+#!/usr/bin/env python3
+import pytest
+import time
+import unittest
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.common.gpio import gpio_read
+from openpilot.selfdrive.test.helpers import with_processes
+from openpilot.selfdrive.manager.process_config import managed_processes
+from openpilot.system.hardware.tici.pins import GPIO
+
+
+# TODO: test TTFF when we have good A-GNSS
+@pytest.mark.tici
+class TestPigeond(unittest.TestCase):
+
+  def tearDown(self):
+    managed_processes['pigeond'].stop()
+
+  @with_processes(['pigeond'])
+  def test_frequency(self):
+    sm = messaging.SubMaster(['ubloxRaw'])
+
+    # setup time
+    for _ in range(int(5 * SERVICE_LIST['ubloxRaw'].frequency)):
+      sm.update()
+
+    for _ in range(int(10 * SERVICE_LIST['ubloxRaw'].frequency)):
+      sm.update()
+      assert sm.all_checks()
+
+  def test_startup_time(self):
+    for _ in range(5):
+      sm = messaging.SubMaster(['ubloxRaw'])
+      managed_processes['pigeond'].start()
+
+      start_time = time.monotonic()
+      for __ in range(10):
+        sm.update(1 * 1000)
+        if sm.updated['ubloxRaw']:
+          break
+      assert sm.recv_frame['ubloxRaw'] > 0, "pigeond didn't start outputting messages in time"
+
+      et = time.monotonic() - start_time
+      assert et < 5, f"pigeond took {et:.1f}s to start"
+      managed_processes['pigeond'].stop()
+
+  def test_turns_off_ublox(self):
+    for s in (0.1, 0.5, 1, 5):
+      managed_processes['pigeond'].start()
+      time.sleep(s)
+      managed_processes['pigeond'].stop()
+
+      assert gpio_read(GPIO.UBLOX_RST_N) == 0
+      assert gpio_read(GPIO.GNSS_PWR_EN) == 0
+
+
+if __name__ == "__main__":
+  unittest.main()

system/ubloxd/tests/ubloxd.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+# type: ignore
+
+from openpilot.selfdrive.locationd.test import ublox
+import struct
+
+baudrate = 460800
+rate = 100  # send new data every 100ms
+
+
+def configure_ublox(dev):
+  # configure ports  and solution parameters and rate
+  dev.configure_port(port=ublox.PORT_USB, inMask=1, outMask=1)  # enable only UBX on USB
+  dev.configure_port(port=0, inMask=0, outMask=0)  # disable DDC
+
+  payload = struct.pack('<BBHIIHHHBB', 1, 0, 0, 2240, baudrate, 1, 1, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_PRT, payload)  # enable UART
+
+  dev.configure_port(port=4, inMask=0, outMask=0)  # disable SPI
+  dev.configure_poll_port()
+  dev.configure_poll_port(ublox.PORT_SERIAL1)
+  dev.configure_poll_port(ublox.PORT_USB)
+  dev.configure_solution_rate(rate_ms=rate)
+
+  # Configure solution
+  payload = struct.pack('<HBBIIBB4H6BH6B', 5, 4, 3, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0, 0,
+                                           0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5, payload)
+  payload = struct.pack('<B3BBB6BBB2BBB2B', 0, 0, 0, 0, 1,
+                                            3, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0,
+                                            0, 0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
+  #bits_ITMF_config1 = '10101101011000101010110111111111'
+  #bits_ITMF_config2 = '00000000000000000110001100011110'
+  ITMF_config1 = 2908925439
+  ITMF_config2 = 25374
+  payload = struct.pack('<II', ITMF_config1, ITMF_config2)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
+  payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, (1 << 10), 0, 0, 0,
+                                                      0, 0, 0, 0, 0, 0,
+                                                      0, 1, 0, 0, 0, 0,
+                                                      0, 0, 0, 0, 0, 0,
+                                                      0, 0, 0, 0, 0, 0,
+                                                      0, 0, 0, 0)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5, payload)
+
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF)
+
+  # Configure RAW, PVT and HW messages to be sent every solution cycle
+  dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1)
+  dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1)
+  dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1)
+  dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1)
+  dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_SAT, 1)
+
+  # Query the backup restore status
+  print("backup restore polling message (implement custom response handler!):")
+  dev.configure_poll(0x09, 0x14)
+
+  print("if successful, send this to clear the flash:")
+  dev.send_message(0x09, 0x14, b"\x01\x00\x00\x00")
+
+  print("send on stop:")
+
+  # Save on shutdown
+  # Controlled GNSS stop and hot start
+  payload = struct.pack('<HBB', 0x0000, 0x08, 0x00)
+  dev.send_message(ublox.CLASS_CFG, ublox.MSG_CFG_RST, payload)
+
+  # UBX-UPD-SOS backup
+  dev.send_message(0x09, 0x14, b"\x00\x00\x00\x00")
+
+
+if __name__ == "__main__":
+  class Device:
+    def write(self, s):
+      d = '"{}"s'.format(''.join(f'\\x{b:02X}' for b in s))
+      print(f"    if (!send_with_ack({d})) continue;")
+
+  dev = ublox.UBlox(Device(), baudrate=baudrate)
+  configure_ublox(dev)

system/ubloxd/ublox_msg.cc

@@ -0,0 +1,525 @@
+#include "system/ubloxd/ublox_msg.h"
+
+#include <unistd.h>
+
+#include <algorithm>
+#include <cassert>
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <cstdlib>
+#include <ctime>
+#include <unordered_map>
+#include <utility>
+
+#include "common/swaglog.h"
+
+const double gpsPi = 3.1415926535898;
+#define UBLOX_MSG_SIZE(hdr) (*(uint16_t *)&hdr[4])
+
+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::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;
+  return needed - (uint16_t)bytes_in_parse_buf;
+}
+
+inline bool UbloxMsgParser::valid_cheksum() {
+  uint8_t ck_a = 0, ck_b = 0;
+  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;
+  }
+  if (ck_a != msg_parse_buf[bytes_in_parse_buf - 2]) {
+    LOGD("Checksum a mismatch: %02X, %02X", ck_a, msg_parse_buf[6]);
+    return false;
+  }
+  if (ck_b != msg_parse_buf[bytes_in_parse_buf - 1]) {
+    LOGD("Checksum b mismatch: %02X, %02X", ck_b, msg_parse_buf[7]);
+    return false;
+  }
+  return true;
+}
+
+inline bool UbloxMsgParser::valid() {
+  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] != ublox::PREAMBLE1) {
+    return false;
+  }
+  if (bytes_in_parse_buf > 1 && msg_parse_buf[1] != ublox::PREAMBLE2) {
+    return false;
+  }
+  if (needed_bytes() == 0 && !valid()) {
+    return false;
+  }
+  return true;
+}
+
+bool UbloxMsgParser::add_data(float log_time, const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) {
+  last_log_time = log_time;
+  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))};
+  case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe)
+    return {"ubloxGnss", gen_rxm_sfrbx(static_cast<ubx_t::rxm_sfrbx_t*>(body))};
+  case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data)
+    return {"ubloxGnss", gen_rxm_rawx(static_cast<ubx_t::rxm_rawx_t*>(body))};
+  case 0x0a09:
+    return {"ubloxGnss", gen_mon_hw(static_cast<ubx_t::mon_hw_t*>(body))};
+  case 0x0a0b:
+    return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
+  case 0x0135:
+    return {"ubloxGnss", gen_nav_sat(static_cast<ubx_t::nav_sat_t*>(body))};
+  default:
+    LOGE("Unknown message type %x", ubx_message.msg_type());
+    return {"ubloxGnss", kj::Array<capnp::word>()};
+  }
+}
+
+
+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.setHasFix((msg->flags() % 2) == 1);
+  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.setHorizontalAccuracy(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.setUnixTimestampMillis(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::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
+  // 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
+  auto body = *msg->body();
+  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 > 3 || subframe_id < 1) {
+      // dont parse almanac subframes
+      return kj::Array<capnp::word>();
+    }
+    gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
+  }
+
+  // publish if subframes 1-3 have been collected
+  if (gps_subframes[msg->sv_id()].size() == 3) {
+    MessageBuilder msg_builder;
+    auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris();
+    eph.setSvId(msg->sv_id());
+
+    int iode_s2 = 0;
+    int iode_s3 = 0;
+    int iodc_lsb = 0;
+    int week;
+
+    // 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());
+
+      // Each message is incremented to be greater or equal than week 1877 (2015-12-27).
+      //  To skip this use the current_time argument
+      week = subframe_1->week_no();
+      week += 1024;
+      if (week < 1877) {
+        week += 1024;
+      }
+      //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));
+      eph.setSvHealth(subframe_1->sv_health());
+      eph.setTowCount(subframe.how()->tow_count());
+      iodc_lsb = subframe_1->iodc_lsb();
+    }
+
+    // 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());
+
+      // GPS week refers to current week, the ephemeris can be valid for the next
+      // if toe equals 0, this can be verified by the TOW count if it is within the
+      // last 2 hours of the week (gps ephemeris valid for 4hours)
+      if (subframe_2->t_oe() == 0 and subframe.how()->tow_count()*6 >= (SECS_IN_WEEK - 2*SECS_IN_HR)){
+        week += 1;
+      }
+      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));
+      iode_s2 = subframe_2->iode();
+    }
+
+    // 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);
+      iode_s3 = subframe_3->iode();
+    }
+
+    eph.setToeWeek(week);
+    eph.setTocWeek(week);
+
+    gps_subframes[msg->sv_id()].clear();
+    if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
+      // data set cutover, reject ephemeris
+      return kj::Array<capnp::word>();
+    }
+    return capnp::messageToFlatArray(msg_builder);
+  }
+  return kj::Array<capnp::word>();
+}
+
+kj::Array<capnp::word> UbloxMsgParser::parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg) {
+  // This parser assumes that no 2 satellites of the same frequency
+  // can be in view at the same time
+  auto body = *msg->body();
+  assert(body.size() == 4);
+  {
+    std::string string_data;
+    string_data.reserve(16);
+    for (uint32_t word : body) {
+      for (int i = 3; i >= 0; i--)
+        string_data.push_back(word >> 8*i);
+    }
+
+    kaitai::kstream stream(string_data);
+    glonass_t gl_string(&stream);
+    int string_number = gl_string.string_number();
+    if (string_number < 1 || string_number > 5 || gl_string.idle_chip()) {
+      // dont parse non immediate data, idle_chip == 0
+      return kj::Array<capnp::word>();
+    }
+
+    // Check if new string either has same superframe_id or log transmission times make sense
+    bool superframe_unknown = false;
+    bool needs_clear = false;
+    for (int i = 1; i <= 5; i++) {
+      if (glonass_strings[msg->freq_id()].find(i) == glonass_strings[msg->freq_id()].end())
+        continue;
+      if (glonass_string_superframes[msg->freq_id()][i] == 0 || gl_string.superframe_number() == 0) {
+        superframe_unknown = true;
+      } else if (glonass_string_superframes[msg->freq_id()][i] != gl_string.superframe_number()) {
+        needs_clear = true;
+      }
+      // Check if string times add up to being from the same frame
+      // If superframe is known this is redundant
+      // Strings are sent 2s apart and frames are 30s apart
+      if (superframe_unknown &&
+          std::abs((glonass_string_times[msg->freq_id()][i] - 2.0 * i) - (last_log_time - 2.0 * string_number)) > 10)
+        needs_clear = true;
+    }
+    if (needs_clear) {
+      glonass_strings[msg->freq_id()].clear();
+      glonass_string_superframes[msg->freq_id()].clear();
+      glonass_string_times[msg->freq_id()].clear();
+    }
+    glonass_strings[msg->freq_id()][string_number] = string_data;
+    glonass_string_superframes[msg->freq_id()][string_number] = gl_string.superframe_number();
+    glonass_string_times[msg->freq_id()][string_number] = last_log_time;
+  }
+  if (msg->sv_id() == 255) {
+    // data can be decoded before identifying the SV number, in this case 255
+    // is returned, which means "unknown"  (ublox p32)
+    return kj::Array<capnp::word>();
+  }
+
+  // publish if strings 1-5 have been collected
+  if (glonass_strings[msg->freq_id()].size() != 5) {
+    return kj::Array<capnp::word>();
+  }
+
+  MessageBuilder msg_builder;
+  auto eph = msg_builder.initEvent().initUbloxGnss().initGlonassEphemeris();
+  eph.setSvId(msg->sv_id());
+  eph.setFreqNum(msg->freq_id() - 7);
+
+  uint16_t current_day = 0;
+  uint16_t tk = 0;
+
+  // string number 1
+  {
+    kaitai::kstream stream(glonass_strings[msg->freq_id()][1]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_1_t* data = static_cast<glonass_t::string_1_t*>(gl_stream.data());
+
+    eph.setP1(data->p1());
+    tk = data->t_k();
+    eph.setTkDEPRECATED(tk);
+    eph.setXVel(data->x_vel() * pow(2, -20));
+    eph.setXAccel(data->x_accel() * pow(2, -30));
+    eph.setX(data->x() * pow(2, -11));
+  }
+
+  // string number 2
+  {
+    kaitai::kstream stream(glonass_strings[msg->freq_id()][2]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_2_t* data = static_cast<glonass_t::string_2_t*>(gl_stream.data());
+
+    eph.setSvHealth(data->b_n()>>2); // MSB indicates health
+    eph.setP2(data->p2());
+    eph.setTb(data->t_b());
+    eph.setYVel(data->y_vel() * pow(2, -20));
+    eph.setYAccel(data->y_accel() * pow(2, -30));
+    eph.setY(data->y() * pow(2, -11));
+  }
+
+  // string number 3
+  {
+    kaitai::kstream stream(glonass_strings[msg->freq_id()][3]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_3_t* data = static_cast<glonass_t::string_3_t*>(gl_stream.data());
+
+    eph.setP3(data->p3());
+    eph.setGammaN(data->gamma_n() * pow(2, -40));
+    eph.setSvHealth(eph.getSvHealth() | data->l_n());
+    eph.setZVel(data->z_vel() * pow(2, -20));
+    eph.setZAccel(data->z_accel() * pow(2, -30));
+    eph.setZ(data->z() * pow(2, -11));
+  }
+
+  // string number 4
+  {
+    kaitai::kstream stream(glonass_strings[msg->freq_id()][4]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_4_t* data = static_cast<glonass_t::string_4_t*>(gl_stream.data());
+
+    current_day = data->n_t();
+    eph.setNt(current_day);
+    eph.setTauN(data->tau_n() * pow(2, -30));
+    eph.setDeltaTauN(data->delta_tau_n() * pow(2, -30));
+    eph.setAge(data->e_n());
+    eph.setP4(data->p4());
+    eph.setSvURA(glonass_URA_lookup.at(data->f_t()));
+    if (msg->sv_id() != data->n()) {
+      LOGE("SV_ID != SLOT_NUMBER: %d %" PRIu64, msg->sv_id(), data->n());
+    }
+    eph.setSvType(data->m());
+  }
+
+  // string number 5
+  {
+    kaitai::kstream stream(glonass_strings[msg->freq_id()][5]);
+    glonass_t gl_stream(&stream);
+    glonass_t::string_5_t* data = static_cast<glonass_t::string_5_t*>(gl_stream.data());
+
+    // string5 parsing is only needed to get the year, this can be removed and
+    // the year can be fetched later in laika (note rollovers and leap year)
+    eph.setN4(data->n_4());
+    int tk_seconds = SECS_IN_HR * ((tk>>7) & 0x1F) + SECS_IN_MIN * ((tk>>1) & 0x3F) + (tk & 0x1) * 30;
+    eph.setTkSeconds(tk_seconds);
+  }
+
+  glonass_strings[msg->freq_id()].clear();
+  return capnp::messageToFlatArray(msg_builder);
+}
+
+
+kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
+  switch (msg->gnss_id()) {
+    case ubx_t::gnss_type_t::GNSS_TYPE_GPS:
+      return parse_gps_ephemeris(msg);
+    case ubx_t::gnss_type_t::GNSS_TYPE_GLONASS:
+      return parse_glonass_ephemeris(msg);
+    default:
+      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->meas();
+  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_nav_sat(ubx_t::nav_sat_t *msg) {
+  MessageBuilder msg_builder;
+  auto sr = msg_builder.initEvent().initUbloxGnss().initSatReport();
+  sr.setITow(msg->itow());
+
+  auto svs = sr.initSvs(msg->num_svs());
+  auto svs_data = *msg->svs();
+  for (int8_t i = 0; i < msg->num_svs(); i++) {
+    svs[i].setSvId(svs_data[i]->sv_id());
+    svs[i].setGnssId(svs_data[i]->gnss_id());
+    svs[i].setFlagsBitfield(svs_data[i]->flags());
+  }
+
+  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.setFlags(msg->flags());
+  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);
+}

system/ubloxd/ublox_msg.h

@@ -0,0 +1,131 @@
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <ctime>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <utility>
+
+#include "cereal/messaging/messaging.h"
+#include "common/util.h"
+#include "system/ubloxd/generated/gps.h"
+#include "system/ubloxd/generated/glonass.h"
+#include "system/ubloxd/generated/ubx.h"
+
+using namespace std::string_literals;
+
+const int SECS_IN_MIN = 60;
+const int SECS_IN_HR = 60 * SECS_IN_MIN;
+const int SECS_IN_DAY = 24 * SECS_IN_HR;
+const int SECS_IN_WEEK = 7 * SECS_IN_DAY;
+
+// protocol constants
+namespace ublox {
+  const uint8_t PREAMBLE1 = 0xb5;
+  const uint8_t PREAMBLE2 = 0x62;
+
+  const int UBLOX_HEADER_SIZE = 6;
+  const int UBLOX_CHECKSUM_SIZE = 2;
+  const int UBLOX_MAX_MSG_SIZE = 65536;
+
+  struct ubx_mga_ini_time_utc_t {
+    uint8_t type;
+    uint8_t version;
+    uint8_t ref;
+    int8_t leapSecs;
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+    uint8_t reserved1;
+    uint32_t ns;
+    uint16_t tAccS;
+    uint16_t reserved2;
+    uint32_t tAccNs;
+  } __attribute__((packed));
+
+  inline std::string ubx_add_checksum(const std::string &msg) {
+    assert(msg.size() > 2);
+
+    uint8_t ck_a = 0, ck_b = 0;
+    for (int i = 2; i < msg.size(); i++) {
+      ck_a = (ck_a + msg[i]) & 0xFF;
+      ck_b = (ck_b + ck_a) & 0xFF;
+    }
+
+    std::string r = msg;
+    r.push_back(ck_a);
+    r.push_back(ck_b);
+    return r;
+  }
+
+  inline std::string build_ubx_mga_ini_time_utc(struct tm time) {
+    ublox::ubx_mga_ini_time_utc_t payload = {
+      .type = 0x10,
+      .version = 0x0,
+      .ref = 0x0,
+      .leapSecs = -128, // Unknown
+      .year = (uint16_t)(1900 + time.tm_year),
+      .month = (uint8_t)(1 + time.tm_mon),
+      .day = (uint8_t)time.tm_mday,
+      .hour = (uint8_t)time.tm_hour,
+      .minute = (uint8_t)time.tm_min,
+      .second = (uint8_t)time.tm_sec,
+      .reserved1 = 0x0,
+      .ns = 0,
+      .tAccS = 30,
+      .reserved2 = 0x0,
+      .tAccNs = 0,
+    };
+    assert(sizeof(payload) == 24);
+
+    std::string msg = "\xb5\x62\x13\x40\x18\x00"s;
+    msg += std::string((char*)&payload, sizeof(payload));
+
+    return ubx_add_checksum(msg);
+  }
+}
+
+class UbloxMsgParser {
+  public:
+    bool add_data(float log_time, 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);
+    kj::Array<capnp::word> gen_nav_sat(ubx_t::nav_sat_t *msg);
+
+  private:
+    inline bool valid_cheksum();
+    inline bool valid();
+    inline bool valid_so_far();
+
+    kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
+    kj::Array<capnp::word> parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg);
+
+    std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
+
+    float last_log_time = 0.0;
+    size_t bytes_in_parse_buf = 0;
+    uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];
+
+    // user range accuracy in meters
+    const std::unordered_map<uint8_t, float> glonass_URA_lookup =
+      {{ 0,  1}, { 1,   2}, { 2, 2.5}, { 3,   4}, { 4,  5}, {5, 7},
+       { 6, 10}, { 7,  12}, { 8,  14}, { 9,  16}, {10, 32},
+       {11, 64}, {12, 128}, {13, 256}, {14, 512}, {15, 1024}};
+
+    std::unordered_map<int, std::unordered_map<int, std::string>> glonass_strings;
+    std::unordered_map<int, std::unordered_map<int, long>> glonass_string_times;
+    std::unordered_map<int, std::unordered_map<int, int>> glonass_string_superframes;
+};

system/ubloxd/ubloxd.cc

@@ -0,0 +1,65 @@
+#include <cassert>
+
+#include <kaitai/kaitaistream.h>
+
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+#include "system/ubloxd/ublox_msg.h"
+
+ExitHandler do_exit;
+using namespace ublox;
+
+int main() {
+  LOGW("starting ubloxd");
+  AlignedBuffer aligned_buf;
+  UbloxMsgParser parser;
+
+  PubMaster pm({"ubloxGnss", "gpsLocationExternal"});
+
+  std::unique_ptr<Context> context(Context::create());
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "ubloxRaw"));
+  assert(subscriber != NULL);
+  subscriber->setTimeout(100);
+
+
+  while (!do_exit) {
+    std::unique_ptr<Message> msg(subscriber->receive());
+    if (!msg) {
+      if (errno == EINTR) {
+        do_exit = true;
+      }
+      continue;
+    }
+
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+    auto ubloxRaw = event.getUbloxRaw();
+    float log_time = 1e-9 * event.getLogMonoTime();
+
+    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(log_time, data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
+
+        try {
+          auto ublox_msg = parser.gen_msg();
+          if (ublox_msg.second.size() > 0) {
+            auto bytes = ublox_msg.second.asBytes();
+            pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
+          }
+        } catch (const std::exception& e) {
+          LOGE("Error parsing ublox message %s", e.what());
+        }
+
+        parser.reset();
+      }
+      bytes_consumed += bytes_consumed_this_time;
+    }
+  }
+
+  return 0;
+}

system/ubloxd/ubx.ksy

@@ -0,0 +1,293 @@
+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
+        0x0135: nav_sat
+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: meas
+        type: measurement
+        size: 32
+        repeat: expr
+        repeat-expr: num_meas
+    types:
+      measurement:
+        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_sat:
+    seq:
+      - id: itow
+        type: u4
+      - id: version
+        type: u1
+      - id: num_svs
+        type: u1
+      - id: reserved
+        size: 2
+      - id: svs
+        type: nav
+        size: 12
+        repeat: expr
+        repeat-expr: num_svs
+    types:
+      nav:
+        seq:
+          - id: gnss_id
+            type: u1
+            enum: gnss_type
+          - id: sv_id
+            type: u1
+          - id: cno
+            type: u1
+          - id: elev
+            type: s1
+          - id: azim
+            type: s2
+          - id: pr_res
+            type: s2
+          - id: flags
+            type: u4
+
+  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