diff --git a/cereal b/cereal index c579889f39..80a8eb8459 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit c579889f396cd754048c7b1a51c6f33b1988762a +Subproject commit 80a8eb84593feb08dfc0bd47e5c04e586ea4b7a6 diff --git a/system/ubloxd/generated/ubx.cpp b/system/ubloxd/generated/ubx.cpp index 34fe1e52ca..81b82ccafc 100644 --- a/system/ubloxd/generated/ubx.cpp +++ b/system/ubloxd/generated/ubx.cpp @@ -40,6 +40,11 @@ void ubx_t::_read() { m_body = new rxm_sfrbx_t(m__io, this, m__root); break; } + case 309: { + n_body = false; + m_body = new nav_sat_t(m__io, this, m__root); + break; + } case 2571: { n_body = false; m_body = new mon_hw2_t(m__io, this, m__root); @@ -70,9 +75,9 @@ void ubx_t::_clean_up() { ubx_t::rxm_rawx_t::rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; - m_measurements = 0; - m__raw_measurements = 0; - m__io__raw_measurements = 0; + m_meas = 0; + m__raw_meas = 0; + m__io__raw_meas = 0; try { _read(); @@ -89,15 +94,15 @@ void ubx_t::rxm_rawx_t::_read() { m_num_meas = m__io->read_u1(); m_rec_stat = m__io->read_u1(); m_reserved1 = m__io->read_bytes(3); - m__raw_measurements = new std::vector(); - m__io__raw_measurements = new std::vector(); - m_measurements = new std::vector(); - const int l_measurements = num_meas(); - for (int i = 0; i < l_measurements; i++) { - m__raw_measurements->push_back(m__io->read_bytes(32)); - kaitai::kstream* io__raw_measurements = new kaitai::kstream(m__raw_measurements->at(m__raw_measurements->size() - 1)); - m__io__raw_measurements->push_back(io__raw_measurements); - m_measurements->push_back(new meas_t(io__raw_measurements, this, m__root)); + m__raw_meas = new std::vector(); + m__io__raw_meas = new std::vector(); + m_meas = new std::vector(); + const int l_meas = num_meas(); + for (int i = 0; i < l_meas; i++) { + m__raw_meas->push_back(m__io->read_bytes(32)); + kaitai::kstream* io__raw_meas = new kaitai::kstream(m__raw_meas->at(m__raw_meas->size() - 1)); + m__io__raw_meas->push_back(io__raw_meas); + m_meas->push_back(new measurement_t(io__raw_meas, this, m__root)); } } @@ -106,24 +111,24 @@ ubx_t::rxm_rawx_t::~rxm_rawx_t() { } void ubx_t::rxm_rawx_t::_clean_up() { - if (m__raw_measurements) { - delete m__raw_measurements; m__raw_measurements = 0; + if (m__raw_meas) { + delete m__raw_meas; m__raw_meas = 0; } - if (m__io__raw_measurements) { - for (std::vector::iterator it = m__io__raw_measurements->begin(); it != m__io__raw_measurements->end(); ++it) { + if (m__io__raw_meas) { + for (std::vector::iterator it = m__io__raw_meas->begin(); it != m__io__raw_meas->end(); ++it) { delete *it; } - delete m__io__raw_measurements; m__io__raw_measurements = 0; + delete m__io__raw_meas; m__io__raw_meas = 0; } - if (m_measurements) { - for (std::vector::iterator it = m_measurements->begin(); it != m_measurements->end(); ++it) { + if (m_meas) { + for (std::vector::iterator it = m_meas->begin(); it != m_meas->end(); ++it) { delete *it; } - delete m_measurements; m_measurements = 0; + delete m_meas; m_meas = 0; } } -ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { +ubx_t::rxm_rawx_t::measurement_t::measurement_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; @@ -135,7 +140,7 @@ ubx_t::rxm_rawx_t::meas_t::meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__ } } -void ubx_t::rxm_rawx_t::meas_t::_read() { +void ubx_t::rxm_rawx_t::measurement_t::_read() { m_pr_mes = m__io->read_f8le(); m_cp_mes = m__io->read_f8le(); m_do_mes = m__io->read_f4le(); @@ -152,11 +157,11 @@ void ubx_t::rxm_rawx_t::meas_t::_read() { m_reserved3 = m__io->read_bytes(1); } -ubx_t::rxm_rawx_t::meas_t::~meas_t() { +ubx_t::rxm_rawx_t::measurement_t::~measurement_t() { _clean_up(); } -void ubx_t::rxm_rawx_t::meas_t::_clean_up() { +void ubx_t::rxm_rawx_t::measurement_t::_clean_up() { } ubx_t::rxm_sfrbx_t::rxm_sfrbx_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { @@ -198,6 +203,89 @@ void ubx_t::rxm_sfrbx_t::_clean_up() { } } +ubx_t::nav_sat_t::nav_sat_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + m_svs = 0; + m__raw_svs = 0; + m__io__raw_svs = 0; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_sat_t::_read() { + m_itow = m__io->read_u4le(); + m_version = m__io->read_u1(); + m_num_svs = m__io->read_u1(); + m_reserved = m__io->read_bytes(2); + m__raw_svs = new std::vector(); + m__io__raw_svs = new std::vector(); + m_svs = new std::vector(); + const int l_svs = num_svs(); + for (int i = 0; i < l_svs; i++) { + m__raw_svs->push_back(m__io->read_bytes(12)); + kaitai::kstream* io__raw_svs = new kaitai::kstream(m__raw_svs->at(m__raw_svs->size() - 1)); + m__io__raw_svs->push_back(io__raw_svs); + m_svs->push_back(new nav_t(io__raw_svs, this, m__root)); + } +} + +ubx_t::nav_sat_t::~nav_sat_t() { + _clean_up(); +} + +void ubx_t::nav_sat_t::_clean_up() { + if (m__raw_svs) { + delete m__raw_svs; m__raw_svs = 0; + } + if (m__io__raw_svs) { + for (std::vector::iterator it = m__io__raw_svs->begin(); it != m__io__raw_svs->end(); ++it) { + delete *it; + } + delete m__io__raw_svs; m__io__raw_svs = 0; + } + if (m_svs) { + for (std::vector::iterator it = m_svs->begin(); it != m_svs->end(); ++it) { + delete *it; + } + delete m_svs; m_svs = 0; + } +} + +ubx_t::nav_sat_t::nav_t::nav_t(kaitai::kstream* p__io, ubx_t::nav_sat_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { + m__parent = p__parent; + m__root = p__root; + + try { + _read(); + } catch(...) { + _clean_up(); + throw; + } +} + +void ubx_t::nav_sat_t::nav_t::_read() { + m_gnss_id = static_cast(m__io->read_u1()); + m_sv_id = m__io->read_u1(); + m_cno = m__io->read_u1(); + m_elev = m__io->read_s1(); + m_azim = m__io->read_s2le(); + m_pr_res = m__io->read_s2le(); + m_flags = m__io->read_u4le(); +} + +ubx_t::nav_sat_t::nav_t::~nav_t() { + _clean_up(); +} + +void ubx_t::nav_sat_t::nav_t::_clean_up() { +} + ubx_t::nav_pvt_t::nav_pvt_t(kaitai::kstream* p__io, ubx_t* p__parent, ubx_t* p__root) : kaitai::kstruct(p__io) { m__parent = p__parent; m__root = p__root; diff --git a/system/ubloxd/generated/ubx.h b/system/ubloxd/generated/ubx.h index 6be4ce8c4b..022108489f 100644 --- a/system/ubloxd/generated/ubx.h +++ b/system/ubloxd/generated/ubx.h @@ -16,6 +16,7 @@ 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; @@ -42,7 +43,7 @@ public: class rxm_rawx_t : public kaitai::kstruct { public: - class meas_t; + class measurement_t; rxm_rawx_t(kaitai::kstream* p__io, ubx_t* p__parent = 0, ubx_t* p__root = 0); @@ -53,18 +54,18 @@ public: public: ~rxm_rawx_t(); - class meas_t : public kaitai::kstruct { + class measurement_t : public kaitai::kstruct { public: - meas_t(kaitai::kstream* p__io, ubx_t::rxm_rawx_t* p__parent = 0, ubx_t* p__root = 0); + 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: - ~meas_t(); + ~measurement_t(); private: double m_pr_mes; @@ -110,11 +111,11 @@ public: uint8_t m_num_meas; uint8_t m_rec_stat; std::string m_reserved1; - std::vector* m_measurements; + std::vector* m_meas; ubx_t* m__root; ubx_t* m__parent; - std::vector* m__raw_measurements; - std::vector* m__io__raw_measurements; + std::vector* m__raw_meas; + std::vector* m__io__raw_meas; public: double rcv_tow() const { return m_rcv_tow; } @@ -123,11 +124,11 @@ public: 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* measurements() const { return m_measurements; } + std::vector* meas() const { return m_meas; } ubx_t* _root() const { return m__root; } ubx_t* _parent() const { return m__parent; } - std::vector* _raw_measurements() const { return m__raw_measurements; } - std::vector* _io__raw_measurements() const { return m__io__raw_measurements; } + 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 { @@ -170,6 +171,79 @@ public: 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: diff --git a/system/ubloxd/tests/ublox.py b/system/ubloxd/tests/ublox.py index 80abc64e2c..0d271ad785 100644 --- a/system/ubloxd/tests/ublox.py +++ b/system/ubloxd/tests/ublox.py @@ -48,6 +48,7 @@ MSG_NAV_TIMEGPS = 0x20 MSG_NAV_TIMEUTC = 0x21 MSG_NAV_CLOCK = 0x22 MSG_NAV_SVINFO = 0x30 +MSG_NAV_SAT = 0x35 MSG_NAV_AOPSTATUS = 0x60 MSG_NAV_DGPS = 0x31 MSG_NAV_DOP = 0x04 diff --git a/system/ubloxd/ublox_msg.cc b/system/ubloxd/ublox_msg.cc index 811492ba9b..a01187d560 100644 --- a/system/ubloxd/ublox_msg.cc +++ b/system/ubloxd/ublox_msg.cc @@ -434,7 +434,7 @@ kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { mr.setGpsWeek(msg->week()); auto mb = mr.initMeasurements(msg->num_meas()); - auto measurements = *msg->measurements(); + 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()); @@ -463,6 +463,22 @@ kj::Array UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) { 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(); diff --git a/system/ubloxd/ublox_msg.h b/system/ubloxd/ublox_msg.h index a6e0623aa0..a52a7db3e5 100644 --- a/system/ubloxd/ublox_msg.h +++ b/system/ubloxd/ublox_msg.h @@ -102,6 +102,7 @@ class UbloxMsgParser { 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(); diff --git a/system/ubloxd/ubx.ksy b/system/ubloxd/ubx.ksy index 6945680d32..02c757fe71 100644 --- a/system/ubloxd/ubx.ksy +++ b/system/ubloxd/ubx.ksy @@ -17,6 +17,7 @@ seq: 0x0215: rxm_rawx 0x0a09: mon_hw 0x0a0b: mon_hw2 + 0x0135: nav_sat instances: checksum: pos: length + 6 @@ -142,13 +143,13 @@ types: type: u1 - id: reserved1 size: 3 - - id: measurements - type: meas + - id: meas + type: measurement size: 32 repeat: expr repeat-expr: num_meas types: - meas: + measurement: seq: - id: pr_mes type: f8 @@ -179,6 +180,39 @@ types: 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: