diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index a4f7c68573..d5be221303 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -219,6 +219,7 @@ jobs: $UNIT_TEST selfdrive/car && \ $UNIT_TEST selfdrive/locationd && \ selfdrive/locationd/test/_test_locationd_lib.py && \ + ./selfdrive/locationd/test/test_glonass_runner && \ $UNIT_TEST selfdrive/athena && \ $UNIT_TEST selfdrive/thermald && \ $UNIT_TEST system/hardware/tici && \ diff --git a/release/files_common b/release/files_common index 547bb58144..5b310d5c80 100644 --- a/release/files_common +++ b/release/files_common @@ -225,10 +225,7 @@ selfdrive/locationd/SConscript selfdrive/locationd/ubloxd.cc selfdrive/locationd/ublox_msg.cc selfdrive/locationd/ublox_msg.h -selfdrive/locationd/generated/ubx.cpp -selfdrive/locationd/generated/ubx.h -selfdrive/locationd/generated/gps.cpp -selfdrive/locationd/generated/gps.h +selfdrive/locationd/generated/* selfdrive/locationd/laikad.py selfdrive/locationd/locationd.h diff --git a/selfdrive/locationd/.gitignore b/selfdrive/locationd/.gitignore index 5268902785..86a228a6ff 100644 --- a/selfdrive/locationd/.gitignore +++ b/selfdrive/locationd/.gitignore @@ -3,3 +3,4 @@ ubloxd_test params_learner paramsd locationd +test/test_glonass_runner diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index 7024196efd..61a0ed7f42 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -13,7 +13,8 @@ if GetOption('kaitai'): 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) +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) ekf_sym_cc = env.SharedObject("#rednose/helpers/ekf_sym.cc") locationd_sources = ["locationd.cc", "models/live_kf.cc", ekf_sym_cc] @@ -25,3 +26,6 @@ lenv.Depends(locationd, libkf) if File("liblocationd.cc").exists(): liblocationd = lenv.SharedLibrary("liblocationd", ["liblocationd.cc"] + locationd_sources, LIBS=loc_libs + transformations) lenv.Depends(liblocationd, libkf) + +if GetOption('test'): + env.Program("test/test_glonass_runner", ['test/test_glonass_runner.cc', 'test/test_glonass_kaitai.cc', glonass_obj], LIBS=[loc_libs]) \ No newline at end of file diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp index 149134fbb5..cd0f96ab68 100644 --- a/selfdrive/locationd/generated/glonass.cpp +++ b/selfdrive/locationd/generated/glonass.cpp @@ -31,6 +31,10 @@ void glonass_t::_read() { m_data = new string_3_t(m__io, this, m__root); break; } + case 5: { + m_data = new string_5_t(m__io, this, m__root); + break; + } case 2: { m_data = new string_2_t(m__io, this, m__root); break; @@ -60,6 +64,8 @@ void glonass_t::_clean_up() { 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; + f_tau_n = false; + f_delta_tau_n = false; try { _read(); @@ -70,8 +76,10 @@ glonass_t::string_4_t::string_4_t(kaitai::kstream* p__io, glonass_t* p__parent, } 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_tau_n_sign = m__io->read_bits_int_be(1); + m_tau_n_value = m__io->read_bits_int_be(21); + m_delta_tau_n_sign = m__io->read_bits_int_be(1); + m_delta_tau_n_value = m__io->read_bits_int_be(4); 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); @@ -89,6 +97,22 @@ glonass_t::string_4_t::~string_4_t() { void glonass_t::string_4_t::_clean_up() { } +int32_t glonass_t::string_4_t::tau_n() { + if (f_tau_n) + return m_tau_n; + m_tau_n = ((tau_n_sign()) ? ((tau_n_value() * -1)) : (tau_n_value())); + f_tau_n = true; + return m_tau_n; +} + +int32_t glonass_t::string_4_t::delta_tau_n() { + if (f_delta_tau_n) + return m_delta_tau_n; + m_delta_tau_n = ((delta_tau_n_sign()) ? ((delta_tau_n_value() * -1)) : (delta_tau_n_value())); + f_delta_tau_n = true; + return m_delta_tau_n; +} + 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; @@ -113,9 +137,40 @@ glonass_t::string_non_immediate_t::~string_non_immediate_t() { void glonass_t::string_non_immediate_t::_clean_up() { } +glonass_t::string_5_t::string_5_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_5_t::_read() { + m_n_a = m__io->read_bits_int_be(11); + m_tau_c = m__io->read_bits_int_be(32); + m_not_used = m__io->read_bits_int_be(1); + m_n_4 = m__io->read_bits_int_be(5); + m_tau_gps = m__io->read_bits_int_be(22); + m_l_n = m__io->read_bits_int_be(1); +} + +glonass_t::string_5_t::~string_5_t() { + _clean_up(); +} + +void glonass_t::string_5_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; + f_x_vel = false; + f_x_accel = false; + f_x = false; try { _read(); @@ -129,9 +184,12 @@ 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); + m_x_vel_sign = m__io->read_bits_int_be(1); + m_x_vel_value = m__io->read_bits_int_be(23); + m_x_accel_sign = m__io->read_bits_int_be(1); + m_x_accel_value = m__io->read_bits_int_be(4); + m_x_sign = m__io->read_bits_int_be(1); + m_x_value = m__io->read_bits_int_be(26); } glonass_t::string_1_t::~string_1_t() { @@ -141,9 +199,36 @@ glonass_t::string_1_t::~string_1_t() { void glonass_t::string_1_t::_clean_up() { } +int32_t glonass_t::string_1_t::x_vel() { + if (f_x_vel) + return m_x_vel; + m_x_vel = ((x_vel_sign()) ? ((x_vel_value() * -1)) : (x_vel_value())); + f_x_vel = true; + return m_x_vel; +} + +int32_t glonass_t::string_1_t::x_accel() { + if (f_x_accel) + return m_x_accel; + m_x_accel = ((x_accel_sign()) ? ((x_accel_value() * -1)) : (x_accel_value())); + f_x_accel = true; + return m_x_accel; +} + +int32_t glonass_t::string_1_t::x() { + if (f_x) + return m_x; + m_x = ((x_sign()) ? ((x_value() * -1)) : (x_value())); + f_x = true; + return m_x; +} + 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; + f_y_vel = false; + f_y_accel = false; + f_y = false; try { _read(); @@ -158,9 +243,12 @@ void glonass_t::string_2_t::_read() { 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); + m_y_vel_sign = m__io->read_bits_int_be(1); + m_y_vel_value = m__io->read_bits_int_be(23); + m_y_accel_sign = m__io->read_bits_int_be(1); + m_y_accel_value = m__io->read_bits_int_be(4); + m_y_sign = m__io->read_bits_int_be(1); + m_y_value = m__io->read_bits_int_be(26); } glonass_t::string_2_t::~string_2_t() { @@ -170,9 +258,37 @@ glonass_t::string_2_t::~string_2_t() { void glonass_t::string_2_t::_clean_up() { } +int32_t glonass_t::string_2_t::y_vel() { + if (f_y_vel) + return m_y_vel; + m_y_vel = ((y_vel_sign()) ? ((y_vel_value() * -1)) : (y_vel_value())); + f_y_vel = true; + return m_y_vel; +} + +int32_t glonass_t::string_2_t::y_accel() { + if (f_y_accel) + return m_y_accel; + m_y_accel = ((y_accel_sign()) ? ((y_accel_value() * -1)) : (y_accel_value())); + f_y_accel = true; + return m_y_accel; +} + +int32_t glonass_t::string_2_t::y() { + if (f_y) + return m_y; + m_y = ((y_sign()) ? ((y_value() * -1)) : (y_value())); + f_y = true; + return m_y; +} + 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; + f_gamma_n = false; + f_z_vel = false; + f_z_accel = false; + f_z = false; try { _read(); @@ -184,13 +300,17 @@ glonass_t::string_3_t::string_3_t(kaitai::kstream* p__io, glonass_t* p__parent, 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_gamma_n_sign = m__io->read_bits_int_be(1); + m_gamma_n_value = m__io->read_bits_int_be(10); 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); + m_z_vel_sign = m__io->read_bits_int_be(1); + m_z_vel_value = m__io->read_bits_int_be(23); + m_z_accel_sign = m__io->read_bits_int_be(1); + m_z_accel_value = m__io->read_bits_int_be(4); + m_z_sign = m__io->read_bits_int_be(1); + m_z_value = m__io->read_bits_int_be(26); } glonass_t::string_3_t::~string_3_t() { @@ -199,3 +319,35 @@ glonass_t::string_3_t::~string_3_t() { void glonass_t::string_3_t::_clean_up() { } + +int32_t glonass_t::string_3_t::gamma_n() { + if (f_gamma_n) + return m_gamma_n; + m_gamma_n = ((gamma_n_sign()) ? ((gamma_n_value() * -1)) : (gamma_n_value())); + f_gamma_n = true; + return m_gamma_n; +} + +int32_t glonass_t::string_3_t::z_vel() { + if (f_z_vel) + return m_z_vel; + m_z_vel = ((z_vel_sign()) ? ((z_vel_value() * -1)) : (z_vel_value())); + f_z_vel = true; + return m_z_vel; +} + +int32_t glonass_t::string_3_t::z_accel() { + if (f_z_accel) + return m_z_accel; + m_z_accel = ((z_accel_sign()) ? ((z_accel_value() * -1)) : (z_accel_value())); + f_z_accel = true; + return m_z_accel; +} + +int32_t glonass_t::string_3_t::z() { + if (f_z) + return m_z; + m_z = ((z_sign()) ? ((z_value() * -1)) : (z_value())); + f_z = true; + return m_z; +} diff --git a/selfdrive/locationd/generated/glonass.h b/selfdrive/locationd/generated/glonass.h index 48bdabfe96..19867ba22b 100644 --- a/selfdrive/locationd/generated/glonass.h +++ b/selfdrive/locationd/generated/glonass.h @@ -15,6 +15,7 @@ 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; @@ -42,8 +43,24 @@ public: ~string_4_t(); private: - uint64_t m_tau_n; - uint64_t m_delta_tau_n; + 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; @@ -56,8 +73,10 @@ public: 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; } + 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; } @@ -96,6 +115,40 @@ public: 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: @@ -109,13 +162,37 @@ public: 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; - uint64_t m_x_vel; - uint64_t m_x_speedup; - uint64_t m_x; + 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; @@ -123,9 +200,12 @@ 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; } + 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; } }; @@ -143,14 +223,38 @@ public: 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; - uint64_t m_y_vel; - uint64_t m_y_speedup; - uint64_t m_y; + 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; @@ -159,9 +263,12 @@ public: 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; } + 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; } }; @@ -179,27 +286,63 @@ public: 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; - uint64_t m_gamma_n; + bool m_gamma_n_sign; + uint64_t m_gamma_n_value; 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; + 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; } - uint64_t gamma_n() const { return m_gamma_n; } + 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; } - 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; } + 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; } }; diff --git a/selfdrive/locationd/glonass.ksy b/selfdrive/locationd/glonass.ksy index 95ca78c16f..be99f6e497 100644 --- a/selfdrive/locationd/glonass.ksy +++ b/selfdrive/locationd/glonass.ksy @@ -1,4 +1,6 @@ # 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 @@ -16,6 +18,7 @@ seq: 2: string_2 3: string_3 4: string_4 + 5: string_5 _: string_non_immediate - id: hamming_code type: b8 @@ -37,12 +40,25 @@ types: type: b2 - id: t_k type: b12 - - id: x_vel - type: b24 - - id: x_speedup - type: b5 - - id: x - type: b27 + - 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 @@ -53,36 +69,70 @@ types: type: b7 - id: not_used type: b5 - - id: y_vel - type: b24 - - id: y_speedup - type: b5 - - id: y - type: b27 + - 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 - type: b11 + - 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 - type: b24 - - id: z_speedup - type: b5 - - id: z - type: b27 + - 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 - type: b22 - - id: delta_tau_n - type: b5 + - 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 @@ -99,9 +149,28 @@ types: 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 \ No newline at end of file + type: b8 diff --git a/selfdrive/locationd/glonass_fix.patch b/selfdrive/locationd/glonass_fix.patch index 2f99e93637..fa34a8ef15 100644 --- a/selfdrive/locationd/glonass_fix.patch +++ b/selfdrive/locationd/glonass_fix.patch @@ -1,5 +1,5 @@ diff --git a/selfdrive/locationd/generated/glonass.cpp b/selfdrive/locationd/generated/glonass.cpp -index 6a48fe62c..149134fbb 100644 +index 5b17bc327..b5c6aa610 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 diff --git a/selfdrive/locationd/test/test_glonass_kaitai.cc b/selfdrive/locationd/test/test_glonass_kaitai.cc new file mode 100644 index 0000000000..22f5202a3d --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "selfdrive/locationd/generated/glonass.h" + +typedef std::vector> 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; // + 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(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(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(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(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(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(strni_data.data()); + + REQUIRE(sni->data_1() == data[ST6_DATA_1_IDX].second); + REQUIRE(sni->data_2() == data[ST6_DATA_2_IDX].second); +} diff --git a/selfdrive/locationd/test/test_glonass_runner.cc b/selfdrive/locationd/test/test_glonass_runner.cc new file mode 100644 index 0000000000..62bf7476a1 --- /dev/null +++ b/selfdrive/locationd/test/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp"