diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 4b7fba19b6..7024196efd 100644 --- a/selfdrive/locationd/SConscript +++ b/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) diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp new file mode 100644 index 0000000000..149134fbb5 --- /dev/null +++ b/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() { +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h new file mode 100644 index 0000000000..48bdabfe96 --- /dev/null +++ b/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 + +#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_ diff --git a/selfdrive/locationd/generated/gps.cpp b/selfdrive/locationd/generated/gps.cpp index 9b020735bb..8e1cb85b95 100644 --- a/selfdrive/locationd/generated/gps.cpp +++ b/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("\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("\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); diff --git a/selfdrive/locationd/generated/gps.h b/selfdrive/locationd/generated/gps.h index 293e2e4a05..9dfc5031f5 100644 --- a/selfdrive/locationd/generated/gps.h +++ b/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; } diff --git a/selfdrive/locationd/generated/ubx.cpp b/selfdrive/locationd/generated/ubx.cpp index 5e743e1ee7..34fe1e52ca 100644 --- a/selfdrive/locationd/generated/ubx.cpp +++ b/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(); - m__raw_measurements->reserve(l_measurements); m__io__raw_measurements = new std::vector(); - m__io__raw_measurements->reserve(l_measurements); m_measurements = new std::vector(); - 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(); - 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()); } diff --git a/selfdrive/locationd/glonass.ksy b/selfdrive/locationd/glonass.ksy new file mode 100644 index 0000000000..95ca78c16f --- /dev/null +++ b/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 \ No newline at end of file diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch new file mode 100644 index 0000000000..2f99e93637 --- /dev/null +++ b/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); diff --git a/selfdrive/locationd/gps.ksy b/selfdrive/locationd/gps.ksy index 6f5cde316b..893ad1b25b 100644 --- a/selfdrive/locationd/gps.ksy +++ b/selfdrive/locationd/gps.ksy @@ -19,7 +19,7 @@ seq: types: tlm: seq: - - id: magic + - id: preamble contents: [0x8b] - id: tlm type: b14