Glonass ephemeris support (#27088)

* add glonass kaitai parsing

* add kaita generated files

* remove glonass from build

* add string non immediate type

* fix kaitai bug

* add patch file

* fix scons order

* Update selfdrive/locationd/SConscript

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

selfdrive/locationd/SConscript

@@ -7,6 +7,11 @@ if GetOption('kaitai'):
   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)
 
 env.Program("ubloxd", ["ubloxd.cc", "ublox_msg.cc", "generated/ubx.cpp", "generated/gps.cpp"], LIBS=loc_libs)
 

selfdrive/locationd/generated/glonass.cpp

@@ -0,0 +1,201 @@
+// This is a generated file! Please edit source .ksy file and use kaitai-struct-compiler to rebuild
+
+#include "glonass.h"
+
+glonass_t::glonass_t(kaitai::kstream* p__io, kaitai::kstruct* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = this;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+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();
+    switch (string_number()) {
+    case 4: {
+        m_data = new string_4_t(m__io, this, m__root);
+        break;
+    }
+    case 1: {
+        m_data = new string_1_t(m__io, this, m__root);
+        break;
+    }
+    case 3: {
+        m_data = new string_3_t(m__io, this, m__root);
+        break;
+    }
+    case 2: {
+        m_data = new string_2_t(m__io, this, m__root);
+        break;
+    }
+    default: {
+        m_data = new string_non_immediate_t(m__io, this, m__root);
+        break;
+    }
+    }
+    m_hamming_code = m__io->read_bits_int_be(8);
+    m_pad_1 = m__io->read_bits_int_be(11);
+    m_superframe_number = m__io->read_bits_int_be(16);
+    m_pad_2 = m__io->read_bits_int_be(8);
+    m_frame_number = m__io->read_bits_int_be(8);
+}
+
+glonass_t::~glonass_t() {
+    _clean_up();
+}
+
+void glonass_t::_clean_up() {
+    if (m_data) {
+        delete m_data; m_data = 0;
+    }
+}
+
+glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void glonass_t::string_4_t::_read() {
+    m_tau_n = m__io->read_bits_int_be(22);
+    m_delta_tau_n = m__io->read_bits_int_be(5);
+    m_e_n = m__io->read_bits_int_be(5);
+    m_not_used_1 = m__io->read_bits_int_be(14);
+    m_p4 = m__io->read_bits_int_be(1);
+    m_f_t = m__io->read_bits_int_be(4);
+    m_not_used_2 = m__io->read_bits_int_be(3);
+    m_n_t = m__io->read_bits_int_be(11);
+    m_n = m__io->read_bits_int_be(5);
+    m_m = m__io->read_bits_int_be(2);
+}
+
+glonass_t::string_4_t::~string_4_t() {
+    _clean_up();
+}
+
+void glonass_t::string_4_t::_clean_up() {
+}
+
+glonass_t::string_non_immediate_t::string_non_immediate_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void glonass_t::string_non_immediate_t::_read() {
+    m_data_1 = m__io->read_bits_int_be(64);
+    m_data_2 = m__io->read_bits_int_be(8);
+}
+
+glonass_t::string_non_immediate_t::~string_non_immediate_t() {
+    _clean_up();
+}
+
+void glonass_t::string_non_immediate_t::_clean_up() {
+}
+
+glonass_t::string_1_t::string_1_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void glonass_t::string_1_t::_read() {
+    m_not_used = m__io->read_bits_int_be(2);
+    m_p1 = m__io->read_bits_int_be(2);
+    m_t_k = m__io->read_bits_int_be(12);
+    m_x_vel = m__io->read_bits_int_be(24);
+    m_x_speedup = m__io->read_bits_int_be(5);
+    m_x = m__io->read_bits_int_be(27);
+}
+
+glonass_t::string_1_t::~string_1_t() {
+    _clean_up();
+}
+
+void glonass_t::string_1_t::_clean_up() {
+}
+
+glonass_t::string_2_t::string_2_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void glonass_t::string_2_t::_read() {
+    m_b_n = m__io->read_bits_int_be(3);
+    m_p2 = m__io->read_bits_int_be(1);
+    m_t_b = m__io->read_bits_int_be(7);
+    m_not_used = m__io->read_bits_int_be(5);
+    m_y_vel = m__io->read_bits_int_be(24);
+    m_y_speedup = m__io->read_bits_int_be(5);
+    m_y = m__io->read_bits_int_be(27);
+}
+
+glonass_t::string_2_t::~string_2_t() {
+    _clean_up();
+}
+
+void glonass_t::string_2_t::_clean_up() {
+}
+
+glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, glonass_t* p__root) : kaitai::kstruct(p__io) {
+    m__parent = p__parent;
+    m__root = p__root;
+
+    try {
+        _read();
+    } catch(...) {
+        _clean_up();
+        throw;
+    }
+}
+
+void glonass_t::string_3_t::_read() {
+    m_p3 = m__io->read_bits_int_be(1);
+    m_gamma_n = m__io->read_bits_int_be(11);
+    m_not_used = m__io->read_bits_int_be(1);
+    m_p = m__io->read_bits_int_be(2);
+    m_l_n = m__io->read_bits_int_be(1);
+    m_z_vel = m__io->read_bits_int_be(24);
+    m_z_speedup = m__io->read_bits_int_be(5);
+    m_z = m__io->read_bits_int_be(27);
+}
+
+glonass_t::string_3_t::~string_3_t() {
+    _clean_up();
+}
+
+void glonass_t::string_3_t::_clean_up() {
+}

selfdrive/locationd/generated/glonass.h

@@ -0,0 +1,232 @@
+#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_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:
+        uint64_t m_tau_n;
+        uint64_t m_delta_tau_n;
+        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:
+        uint64_t tau_n() const { return m_tau_n; }
+        uint64_t delta_tau_n() const { return m_delta_tau_n; }
+        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_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:
+        uint64_t m_not_used;
+        uint64_t m_p1;
+        uint64_t m_t_k;
+        uint64_t m_x_vel;
+        uint64_t m_x_speedup;
+        uint64_t m_x;
+        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; }
+        uint64_t x_vel() const { return m_x_vel; }
+        uint64_t x_speedup() const { return m_x_speedup; }
+        uint64_t x() const { return m_x; }
+        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:
+        uint64_t m_b_n;
+        bool m_p2;
+        uint64_t m_t_b;
+        uint64_t m_not_used;
+        uint64_t m_y_vel;
+        uint64_t m_y_speedup;
+        uint64_t m_y;
+        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; }
+        uint64_t y_vel() const { return m_y_vel; }
+        uint64_t y_speedup() const { return m_y_speedup; }
+        uint64_t y() const { return m_y; }
+        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 m_p3;
+        uint64_t m_gamma_n;
+        bool m_not_used;
+        uint64_t m_p;
+        bool m_l_n;
+        uint64_t m_z_vel;
+        uint64_t m_z_speedup;
+        uint64_t m_z;
+        glonass_t* m__root;
+        glonass_t* m__parent;
+
+    public:
+        bool p3() const { return m_p3; }
+        uint64_t gamma_n() const { return m_gamma_n; }
+        bool not_used() const { return m_not_used; }
+        uint64_t p() const { return m_p; }
+        bool l_n() const { return m_l_n; }
+        uint64_t z_vel() const { return m_z_vel; }
+        uint64_t z_speedup() const { return m_z_speedup; }
+        uint64_t z() const { return m_z; }
+        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_

selfdrive/locationd/generated/gps.cpp

@@ -274,9 +274,9 @@ gps_t::tlm_t::tlm_t(kaitai::kstream* p__io, gps_t* p__parent, gps_t* p__root) :
 }
 
 void gps_t::tlm_t::_read() {
-    m_magic = m__io->read_bytes(1);
-    if (!(magic() == std::string("\x8B", 1))) {
-        throw kaitai::validation_not_equal_error<std::string>(std::string("\x8B", 1), magic(), _io(), std::string("/types/tlm/seq/0"));
+    m_preamble = m__io->read_bytes(1);
+    if (!(preamble() == std::string("\x8B", 1))) {
+        throw kaitai::validation_not_equal_error<std::string>(std::string("\x8B", 1), preamble(), _io(), std::string("/types/tlm/seq/0"));
     }
     m_tlm = m__io->read_bits_int_be(14);
     m_integrity_status = m__io->read_bits_int_be(1);

selfdrive/locationd/generated/gps.h

@@ -273,7 +273,7 @@ public:
         ~tlm_t();
 
     private:
-        std::string m_magic;
+        std::string m_preamble;
         uint64_t m_tlm;
         bool m_integrity_status;
         bool m_reserved;
@@ -281,7 +281,7 @@ public:
         gps_t* m__parent;
 
     public:
-        std::string magic() const { return m_magic; }
+        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; }

selfdrive/locationd/generated/ubx.cpp

@@ -89,13 +89,10 @@ void ubx_t::rxm_rawx_t::_read() {
     m_num_meas = m__io->read_u1();
     m_rec_stat = m__io->read_u1();
     m_reserved1 = m__io->read_bytes(3);
-    int l_measurements = num_meas();
     m__raw_measurements = new std::vector<std::string>();
-    m__raw_measurements->reserve(l_measurements);
     m__io__raw_measurements = new std::vector<kaitai::kstream*>();
-    m__io__raw_measurements->reserve(l_measurements);
     m_measurements = new std::vector<meas_t*>();
-    m_measurements->reserve(l_measurements);
+    const int l_measurements = num_meas();
     for (int i = 0; i < l_measurements; i++) {
         m__raw_measurements->push_back(m__io->read_bytes(32));
         kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1));
@@ -184,9 +181,8 @@ void ubx_t::rxm_sfrbx_t::_read() {
     m_reserved2 = m__io->read_bytes(1);
     m_version = m__io->read_u1();
     m_reserved3 = m__io->read_bytes(1);
-    int l_body = num_words();
     m_body = new std::vector<uint32_t>();
-    m_body->reserve(l_body);
+    const int l_body = num_words();
     for (int i = 0; i < l_body; i++) {
         m_body->push_back(m__io->read_u4le());
     }

selfdrive/locationd/glonass.ksy

@@ -0,0 +1,107 @@
+# http://gauss.gge.unb.ca/GLONASS.ICD.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
+        _: 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
+        type: b24
+      - id: x_speedup
+        type: b5
+      - id: x
+        type: b27
+  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
+        type: b24
+      - id: y_speedup
+        type: b5
+      - id: y
+        type: b27
+  string_3:
+    seq:
+      - id: p3
+        type: b1
+      - id: gamma_n
+        type: b11
+      - id: not_used
+        type: b1
+      - id: p
+        type: b2
+      - id: l_n
+        type: b1
+      - id: z_vel
+        type: b24
+      - id: z_speedup
+        type: b5
+      - id: z
+        type: b27
+  string_4:
+    seq:
+      - id: tau_n
+        type: b22
+      - id: delta_tau_n
+        type: b5
+      - 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
+  string_non_immediate:
+    seq:
+      - id: data_1
+        type: b64
+      - id: data_2
+        type: b8

selfdrive/locationd/glonass_fix.patch

@@ -0,0 +1,13 @@
+diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp
+index 6a48fe62c..149134fbb 100644
+--- a/selfdrive/locationd/generated/glonass.cpp
++++ b/selfdrive/locationd/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);

selfdrive/locationd/gps.ksy

@@ -19,7 +19,7 @@ seq:
 types:
   tlm:
    seq:
-     - id: magic
+     - id: preamble
        contents: [0x8b]
      - id: tlm
        type: b14