add interference monitor

selfdrive/boardd/pigeon.cc

@@ -99,15 +99,18 @@ void Pigeon::init() {
     if (!send_with_ack("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10"s)) continue;
     if (!send_with_ack("\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"s)) continue;
     if (!send_with_ack("\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"s)) continue;
+    if (!send_with_ack("\xB5\x62\x06\x39\x08\x00\x33\xAC\x62\xAD\x1E\x63\x00\x00\xB6\xA5"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x24\x00\x00\x2A\x84"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x23\x00\x00\x29\x81"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x1E\x00\x00\x24\x72"s)) continue;
+    if (!send_with_ack("\xB5\x62\x06\x39\x00\x00\x3F\xC3"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x09\x01\x1E\x70"s)) continue;
     if (!send_with_ack("\xB5\x62\x06\x01\x03\x00\x0A\x0B\x01\x20\x74"s)) continue;
 
+
     LOGW("panda GPS on");
     return;
   }

selfdrive/locationd/test/ublox.py

@@ -96,6 +96,7 @@ MSG_CFG_NMEA = 0x17
 MSG_CFG_NVS = 0x22
 MSG_CFG_PM2 = 0x3B
 MSG_CFG_PM = 0x32
+MSG_CFG_ITMF = 0x39
 MSG_CFG_RINV = 0x34
 MSG_CFG_RST = 0x04
 MSG_CFG_RXM = 0x11
@@ -365,6 +366,11 @@ msg_types = {
     'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
     'reserved4', 'reserved5'
   ]),
+  (CLASS_CFG, MSG_CFG_ITMF):
+  UBloxDescriptor('CFG_ITMF', '<I', [
+    'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
+    'reserved4', 'reserved5'
+  ]),
   (CLASS_CFG, MSG_CFG_CFG):
   UBloxDescriptor('CFG_CFG', '<III,B',
                   ['clearMask', 'saveMask', 'loadMask', 'deviceMask']),

selfdrive/locationd/test/ubloxd.py

@@ -35,6 +35,12 @@ def configure_ublox(dev):
                                             0, 0, 0, 0, 0,
                                             0, 0, 0, 0, 0)
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO, payload)
+  #bits_ITMF_config1 = '10101101011000101010110000110011'
+  #bits_ITMF_config2 = '00000000000000000110001100011110'
+  ITMF_config1 = 2908924979
+  ITMF_config2 = 25374
+  payload = struct.pack('<II', ITMF_config1, ITMF_config2)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF, payload)
   #payload = struct.pack('<HHIBBBBBBBBBBH6BBB2BH4B3BB', 0, 8192, 0, 0, 0,
   #                                                     0, 0, 0, 0, 0, 0,
   #                                                     0, 0, 0, 0, 0, 0,
@@ -46,6 +52,7 @@ def configure_ublox(dev):
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAV5)
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_NAVX5)
   dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ODO)
+  dev.configure_poll(ublox.CLASS_CFG, ublox.MSG_CFG_ITMF)
 
   # Configure RAW, PVT and HW messages to be sent every solution cycle
   dev.configure_message_rate(ublox.CLASS_NAV, ublox.MSG_NAV_PVT, 1)

selfdrive/locationd/ublox_msg.cc

@@ -302,6 +302,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_rawx(ubx_t::rxm_rawx_t *msg) {
 kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
   MessageBuilder msg_builder;
   auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
+  std::cout << int(msg->flags()) << "\n ------- \n";
   hwStatus.setNoisePerMS(msg->noise_per_ms());
   hwStatus.setAgcCnt(msg->agc_cnt());
   hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status());