implement ublox MON_HW2 message (#19962)

* implement MON_HW2 message

* didn't mean to remove this

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");
 }

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):

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):

selfdrive/locationd/ublox_msg.cc

@@ -335,6 +335,40 @@ kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
   return capnp::messageToFlatArray(msg_builder);
 }
 
+kj::Array<capnp::word> 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) {

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<capnp::word> gen_solution();
       kj::Array<capnp::word> gen_raw();
       kj::Array<capnp::word> gen_mon_hw();
+      kj::Array<capnp::word> gen_mon_hw2();
 
       kj::Array<capnp::word> gen_nav_data();
       bool add_data(const uint8_t *incoming_data, uint32_t incoming_data_len, size_t &bytes_consumed);

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());
           }