Added hw status to ublox test scripts and ubloxGnss packet (#1054)

* Added hw status to ublox test scripts and ubloxGnss packet

* Added extra config to boardd

* Implemented new ublox message handler in ubloxd

* Fixed debug text

* Added some explanation and cereal bump

cereal

@@ -1 +1 @@
-Subproject commit 23abef4fdb18bc2b06281f9937ed3e7fc31e5c37
+Subproject commit ab32956aaf927ee6318bcf639e3a25eea72027a9

selfdrive/boardd/boardd.cc

@@ -789,6 +789,7 @@ void pigeon_init() {
   usleep(100*1000);
 
   // init from ubloxd
+  // To generate this data, run test/ubloxd.py with the print statements enabled in the write function in panda/python/serial.py 
   pigeon_send("\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("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
   pigeon_send("\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");
@@ -807,6 +808,7 @@ void pigeon_init() {
   pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51");
   pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
   pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
+  pigeon_send("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70");
 
   LOGW("panda GPS on");
 }

selfdrive/locationd/test/ublox.py

@@ -475,6 +475,8 @@ msg_types = {
   UBloxDescriptor('AID_ALM', '<II', '_remaining', 'I', ['dwrd']),
   (CLASS_RXM, MSG_RXM_ALM):
   UBloxDescriptor('RXM_ALM', '<II , 8I', ['svid', 'week', 'dwrd[8]']),
+  (CLASS_CFG, MSG_CFG_ANT):
+  UBloxDescriptor('CFG_ANT', '<HH', ['flags', 'pins']),
   (CLASS_CFG, MSG_CFG_ODO):
   UBloxDescriptor('CFG_ODO', '<B3BBB6BBB2BBB2B', [
     'version', 'reserved1[3]', 'flags', 'odoCfg', 'reserverd2[6]', 'cogMaxSpeed',
@@ -494,9 +496,9 @@ msg_types = {
     'reserved12', 'reserved13', 'aopOrbMaxErr', 'reserved3', 'reserved4'
   ]),
   (CLASS_MON, MSG_MON_HW):
-  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB25BHIII', [
+  UBloxDescriptor('MON_HW', '<IIIIHHBBBBIB17BHIII', [
     'pinSel', 'pinBank', 'pinDir', 'pinVal', 'noisePerMS', 'agcCnt', 'aStatus', 'aPower',
-    'flags', 'reserved1', 'usedMask', 'VP[25]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
+    'flags', 'reserved1', 'usedMask', 'VP[17]', 'jamInd', 'reserved3', 'pinInq', 'pullH',
     'pullL'
   ]),
   (CLASS_MON, MSG_MON_HW2):
@@ -827,7 +829,10 @@ class UBlox:
     if not self.read_only:
       if self.use_sendrecv:
         return self.dev.send(buf)
-      return self.dev.write(buf)
+      if type(buf) == str:
+        return self.dev.write(str.encode(buf))
+      else:
+        return self.dev.write(buf)
 
   def read(self, n):
     '''read some bytes'''
@@ -973,7 +978,7 @@ class UBlox:
     payload = struct.pack('<IIIB', clearMask, saveMask, loadMask, deviceMask)
     self.send_message(CLASS_CFG, MSG_CFG_CFG, payload)
 
-  def configure_poll(self, msg_class, msg_id, payload=''):
+  def configure_poll(self, msg_class, msg_id, payload=b''):
     '''poll a configuration message'''
     self.send_message(msg_class, msg_id, payload)
 

selfdrive/locationd/test/ubloxd.py

@@ -72,10 +72,11 @@ def configure_ublox(dev):
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
 
-  # Configure RAW and PVT messages to be sent every solution cycle
+  # Configure RAW, PVT and HW messages to be sent every solution cycle
   dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)
   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)
 
 
 
@@ -222,6 +223,17 @@ def gen_raw(msg):
                 'measurements': measurements_parsed}}
   return log.Event.new_message(ubloxGnss=raw_meas)
 
+def gen_hw_status(msg):
+  msg_data = msg.unpack()[0]
+  ublox_hw_status = {'hwStatus': {
+    'noisePerMS': msg_data['noisePerMS'],
+    'agcCnt': msg_data['agcCnt'],
+    'aStatus': msg_data['aStatus'],
+    'aPower': msg_data['aPower'],
+    'jamInd': msg_data['jamInd']
+  }}
+  return log.Event.new_message(ubloxGnss=ublox_hw_status)
+
 def init_reader():
   port_counter = 0
   while True:
@@ -252,9 +264,12 @@ def handle_msg(dev, msg, nav_frame_buffer):
       if nav is not None:
         nav.logMonoTime = int(realtime.sec_since_boot() * 1e9)
         ubloxGnss.send(nav.to_bytes())
-
+    elif msg.name() == 'MON_HW':
+      hw = gen_hw_status(msg)
+      hw.logMonoTime = int(realtime.sec_since_boot() * 1e9)
+      ubloxGnss.send(hw.to_bytes())
     else:
-      print("UNKNNOWN MESSAGE:", msg.name())
+      print("UNKNOWN MESSAGE:", msg.name())
   except ublox.UBloxError as e:
     print(e)
 

selfdrive/locationd/ublox_msg.cc

@@ -347,6 +347,22 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() {
   return kj::Array<capnp::word>();
 }
 
+kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw() {
+  mon_hw_msg *msg = (mon_hw_msg *)&msg_parse_buf[UBLOX_HEADER_SIZE];
+
+  capnp::MallocMessageBuilder msg_builder;
+  cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto gnss = event.initUbloxGnss();
+  auto hwStatus = gnss.initHwStatus();
+  hwStatus.setNoisePerMS(msg->noisePerMS);
+  hwStatus.setAgcCnt(msg->agcCnt);
+  hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->aStatus);
+  hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->aPower);
+  hwStatus.setJamInd(msg->jamInd);
+  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

@@ -85,6 +85,27 @@ typedef struct __attribute__((packed)) {
   uint32_t dwrd;
 } rxm_sfrbx_msg_extra;
 
+// MON_HW
+typedef struct __attribute__((packed)) {
+  uint32_t pinSel;
+  uint32_t pinBank;
+  uint32_t pinDir;
+  uint32_t pinVal;
+  uint16_t noisePerMS;
+  uint16_t agcCnt;
+  uint8_t aStatus;
+  uint8_t aPower;
+  uint8_t flags;
+  uint8_t reserved1;
+  uint32_t usedMask;
+  uint8_t VP[17];
+  uint8_t jamInd;
+  uint8_t reserved2[2];
+  uint32_t pinIrq;
+  uint32_t pullH;
+  uint32_t pullL;
+} mon_hw_msg;
+
 namespace ublox {
   // protocol constants
   const uint8_t PREAMBLE1 = 0xb5;
@@ -93,6 +114,7 @@ namespace ublox {
   // message classes
   const uint8_t CLASS_NAV = 0x01;
   const uint8_t CLASS_RXM = 0x02;
+  const uint8_t CLASS_MON = 0x0A;
 
   // NAV messages
   const uint8_t MSG_NAV_PVT = 0x7;
@@ -101,6 +123,9 @@ namespace ublox {
   const uint8_t MSG_RXM_RAW = 0x15;
   const uint8_t MSG_RXM_SFRBX = 0x13;
 
+  // MON messages
+  const uint8_t MSG_MON_HW = 0x09;
+
   const int UBLOX_HEADER_SIZE = 6;
   const int UBLOX_CHECKSUM_SIZE = 2;
   const int UBLOX_MAX_MSG_SIZE = 65536;
@@ -113,6 +138,7 @@ namespace ublox {
       UbloxMsgParser();
       kj::Array<capnp::word> gen_solution();
       kj::Array<capnp::word> gen_raw();
+      kj::Array<capnp::word> gen_mon_hw();
 
       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

@@ -97,6 +97,17 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
             }
           } else
             LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
+        } else if(parser.msg_class() == CLASS_MON) {
+          if(parser.msg_id() == MSG_MON_HW) {
+            //LOGD("MSG_MON_HW");
+            auto words = parser.gen_mon_hw();
+            if(words.size() > 0) {
+              auto bytes = words.asBytes();
+              send_func(ubloxGnss, bytes.begin(), bytes.size());
+            }
+          } else {
+            LOGW("Unknown mon msg id: 0x%02X", parser.msg_id());
+          }
         } else
           LOGW("Unknown msg class: 0x%02X", parser.msg_class());
         parser.reset();