diff --git a/system/ubloxd/SConscript b/system/ubloxd/SConscript new file mode 100644 index 0000000..67d9856 --- /dev/null +++ b/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]) \ No newline at end of file diff --git a/system/ubloxd/generated/glonass.h b/system/ubloxd/generated/glonass.h new file mode 100644 index 0000000..19867ba --- /dev/null +++ b/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 + +#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_ diff --git a/system/ubloxd/generated/gps.h b/system/ubloxd/generated/gps.h new file mode 100644 index 0000000..9dfc503 --- /dev/null +++ b/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 + +#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_ diff --git a/system/ubloxd/generated/ubx.h b/system/ubloxd/generated/ubx.h new file mode 100644 index 0000000..0221084 --- /dev/null +++ b/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 +#include + +#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* m_meas; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_meas; + std::vector* 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* meas() const { return m_meas; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_meas() const { return m__raw_meas; } + std::vector* _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* 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* 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* m_svs; + ubx_t* m__root; + ubx_t* m__parent; + std::vector* m__raw_svs; + std::vector* 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* svs() const { return m_svs; } + ubx_t* _root() const { return m__root; } + ubx_t* _parent() const { return m__parent; } + std::vector* _raw_svs() const { return m__raw_svs; } + std::vector* _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_ diff --git a/system/ubloxd/glonass.ksy b/system/ubloxd/glonass.ksy new file mode 100644 index 0000000..be99f6e --- /dev/null +++ b/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 diff --git a/system/ubloxd/glonass_fix.patch b/system/ubloxd/glonass_fix.patch new file mode 100644 index 0000000..7eb973a --- /dev/null +++ b/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); diff --git a/system/ubloxd/gps.ksy b/system/ubloxd/gps.ksy new file mode 100644 index 0000000..893ad1b --- /dev/null +++ b/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 + diff --git a/system/ubloxd/pigeond.py b/system/ubloxd/pigeond.py new file mode 100755 index 0000000..21b3a86 --- /dev/null +++ b/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(" 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() diff --git a/system/ubloxd/tests/print_gps_stats.py b/system/ubloxd/tests/print_gps_stats.py new file mode 100644 index 0000000..edf94c0 --- /dev/null +++ b/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) diff --git a/system/ubloxd/tests/test_glonass_kaitai.cc b/system/ubloxd/tests/test_glonass_kaitai.cc new file mode 100644 index 0000000..96f4374 --- /dev/null +++ b/system/ubloxd/tests/test_glonass_kaitai.cc @@ -0,0 +1,360 @@ +#include +#include +#include +#include +#include +#include + +#include "catch2/catch.hpp" +#include "system/ubloxd/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/system/ubloxd/tests/test_glonass_runner.cc b/system/ubloxd/tests/test_glonass_runner.cc new file mode 100644 index 0000000..62bf747 --- /dev/null +++ b/system/ubloxd/tests/test_glonass_runner.cc @@ -0,0 +1,2 @@ +#define CATCH_CONFIG_MAIN +#include "catch2/catch.hpp" diff --git a/system/ubloxd/tests/test_pigeond.py b/system/ubloxd/tests/test_pigeond.py new file mode 100644 index 0000000..742e20b --- /dev/null +++ b/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() diff --git a/system/ubloxd/tests/ubloxd.py b/system/ubloxd/tests/ubloxd.py new file mode 100644 index 0000000..c173871 --- /dev/null +++ b/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(' + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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> 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(body))}; + case 0x0213: // UBX-RXM-SFRB (Broadcast Navigation Data Subframe) + return {"ubloxGnss", gen_rxm_sfrbx(static_cast(body))}; + case 0x0215: // UBX-RXM-RAW (Multi-GNSS Raw Measurement Data) + return {"ubloxGnss", gen_rxm_rawx(static_cast(body))}; + case 0x0a09: + return {"ubloxGnss", gen_mon_hw(static_cast(body))}; + case 0x0a0b: + return {"ubloxGnss", gen_mon_hw2(static_cast(body))}; + case 0x0135: + return {"ubloxGnss", gen_nav_sat(static_cast(body))}; + default: + LOGE("Unknown message type %x", ubx_message.msg_type()); + return {"ubloxGnss", kj::Array()}; + } +} + + +kj::Array 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 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(); + } + 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(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(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(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(); + } + return capnp::messageToFlatArray(msg_builder); + } + return kj::Array(); +} + +kj::Array 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(); + } + + // 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(); + } + + // publish if strings 1-5 have been collected + if (glonass_strings[msg->freq_id()].size() != 5) { + return kj::Array(); + } + + 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(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(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(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(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(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 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(); + } +} + +kj::Array 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 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 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 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); +} diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h new file mode 100644 index 0000000..d21760e --- /dev/null +++ b/system/ubloxd/ublox_msg.h @@ -0,0 +1,131 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#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> gen_msg(); + kj::Array gen_nav_pvt(ubx_t::nav_pvt_t *msg); + kj::Array gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg); + kj::Array gen_rxm_rawx(ubx_t::rxm_rawx_t *msg); + kj::Array gen_mon_hw(ubx_t::mon_hw_t *msg); + kj::Array gen_mon_hw2(ubx_t::mon_hw2_t *msg); + kj::Array gen_nav_sat(ubx_t::nav_sat_t *msg); + + private: + inline bool valid_cheksum(); + inline bool valid(); + inline bool valid_so_far(); + + kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); + kj::Array parse_glonass_ephemeris(ubx_t::rxm_sfrbx_t *msg); + + std::unordered_map> 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 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> glonass_strings; + std::unordered_map> glonass_string_times; + std::unordered_map> glonass_string_superframes; +}; diff --git a/system/ubloxd/ubloxd b/system/ubloxd/ubloxd deleted file mode 100755 index dfbd92d..0000000 Binary files a/system/ubloxd/ubloxd and /dev/null differ diff --git a/system/ubloxd/ubloxd.cc b/system/ubloxd/ubloxd.cc new file mode 100644 index 0000000..668c1a7 --- /dev/null +++ b/system/ubloxd/ubloxd.cc @@ -0,0 +1,65 @@ +#include + +#include + +#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::create()); + std::unique_ptr subscriber(SubSocket::create(context.get(), "ubloxRaw")); + assert(subscriber != NULL); + subscriber->setTimeout(100); + + + while (!do_exit) { + std::unique_ptr 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(); + 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; +} diff --git a/system/ubloxd/ubx.ksy b/system/ubloxd/ubx.ksy new file mode 100644 index 0000000..02c757f --- /dev/null +++ b/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