From 815654231273eb214876edc71c7e74d65671292c Mon Sep 17 00:00:00 2001 From: robbederks Date: Wed, 3 Feb 2021 13:09:44 +0100 Subject: [PATCH] implement ublox MON_HW2 message (#19962) * implement MON_HW2 message * didn't mean to remove this old-commit-hash: 99d58193e497385b925a215f42b8bc470e328da4 --- cereal | 2 +- selfdrive/boardd/pigeon.cc | 1 + selfdrive/locationd/test/ublox.py | 2 +- selfdrive/locationd/test/ubloxd.py | 1 + selfdrive/locationd/ublox_msg.cc | 34 ++++++++++++++++++++++++++++++ selfdrive/locationd/ublox_msg.h | 16 ++++++++++++++ selfdrive/locationd/ubloxd_main.cc | 7 ++++++ 7 files changed, 61 insertions(+), 2 deletions(-) diff --git a/cereal b/cereal index 2748048829..266fc01950 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 2748048829ac98373fd946d6aca6a3a344e30cc3 +Subproject commit 266fc0195003a50faa95d2905839abf8409b6bcc diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index da04430f17..8cd0790402 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -76,6 +76,7 @@ void Pigeon::init() { send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s); send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s); send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s); + send("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s); LOGW("panda GPS on"); } diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py index 6a28edb1aa..5f561f2efd 100644 --- a/selfdrive/locationd/test/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -729,7 +729,7 @@ class UBlox: class BoarddSerial(): def __init__(self): self.ubloxRaw = messaging.sub_sock('ubloxRaw') - self.buf = "" + self.buf = b"" def read(self, n): for msg in messaging.drain_sock(self.ubloxRaw, len(self.buf) < n): diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 314c80237f..ad0d742a69 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -79,6 +79,7 @@ def configure_ublox(dev): dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_RAW, 1) dev.configure_message_rate(ublox.CLASS_RXM, ublox.MSG_RXM_SFRBX, 1) dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW, 1) + dev.configure_message_rate(ublox.CLASS_MON, ublox.MSG_MON_HW2, 1) def int_to_bool_list(num): diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 4203085b3b..61b0f7dfb2 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -335,6 +335,40 @@ kj::Array UbloxMsgParser::gen_mon_hw() { return capnp::messageToFlatArray(msg_builder); } +kj::Array UbloxMsgParser::gen_mon_hw2() { + mon_hw2_msg *msg = (mon_hw2_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE]; + + MessageBuilder msg_builder; + auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus2(); + hwStatus.setOfsI(msg->ofsI); + hwStatus.setMagI(msg->magI); + hwStatus.setOfsQ(msg->ofsQ); + hwStatus.setMagQ(msg->magQ); + + switch (msg->cfgSource) { + case 114: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::ROM); + break; + case 111: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::OTP); + break; + case 112: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::CONFIGPINS); + break; + case 102: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::FLASH); + break; + default: + hwStatus.setCfgSource(cereal::UbloxGnss::HwStatus2::ConfigSource::UNDEFINED); + break; + } + + hwStatus.setLowLevCfg(msg->lowLevCfg); + hwStatus.setPostStatus(msg->postStatus); + + return capnp::messageToFlatArray(msg_builder); +} + bool UbloxMsgParser::add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed) { int needed = needed_bytes(); if(needed > 0) { diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 5fda805402..2e99e94e15 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -105,6 +105,20 @@ typedef struct __attribute__((packed)) { uint32_t pullL; } mon_hw_msg; +// MON_HW2 +typedef struct __attribute__((packed)) { + int8_t ofsI; + uint8_t magI; + int8_t ofsQ; + uint8_t magQ; + uint8_t cfgSource; + uint8_t reserved1[3]; + uint32_t lowLevCfg; + uint8_t reserved2[8]; + uint32_t postStatus; + uint8_t reserved3[4]; +} mon_hw2_msg; + namespace ublox { // protocol constants const uint8_t PREAMBLE1 = 0xb5; @@ -124,6 +138,7 @@ namespace ublox { // MON messages const uint8_t MSG_MON_HW = 0x09; + const uint8_t MSG_MON_HW2 = 0x0B; const int UBLOX_HEADER_SIZE = 6; const int UBLOX_CHECKSUM_SIZE = 2; @@ -138,6 +153,7 @@ namespace ublox { kj::Array gen_solution(); kj::Array gen_raw(); kj::Array gen_mon_hw(); + kj::Array gen_mon_hw2(); kj::Array gen_nav_data(); bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed); diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index e72ca58f53..51924f6441 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -91,6 +91,13 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) auto bytes = words.asBytes(); pm.send("ubloxGnss", bytes.begin(), bytes.size()); } + } else if(parser.msg_id() == MSG_MON_HW2) { + //LOGD("MSG_MON_HW2"); + auto words = parser.gen_mon_hw2(); + if(words.size() > 0) { + auto bytes = words.asBytes(); + pm.send("ubloxGnss", bytes.begin(), bytes.size()); + } } else { LOGW("Unknown mon msg id: 0x%02X", parser.msg_id()); }