From e5f93ef142af706eed05f1d015e1436efa29d1de Mon Sep 17 00:00:00 2001
From: HaraldSchafer <harald.the.engineer@gmail.com>
Date: Fri, 14 May 2021 16:23:27 -0600
Subject: [PATCH] Add ublox interference monitor (#20911)

* add interference monitor

* new thresholds

* correct labels

* always run for test

* new msg

* add flag

* Revert "always run for test"

This reverts commit b04b92ac2d73d996d1526509e26c4e1a4338cbbb.

* add hw status flags
---
 cereal                                   | 2 +-
 selfdrive/boardd/pigeon.cc               | 3 +++
 selfdrive/locationd/test/ublox.py        | 5 +++++
 selfdrive/locationd/test/ubloxd.py       | 7 +++++++
 selfdrive/locationd/ublox_msg.cc         | 1 +
 selfdrive/test/process_replay/ref_commit | 2 +-
 6 files changed, 18 insertions(+), 2 deletions(-)

diff --git a/cereal b/cereal
index 6561fda4..2acf89ed 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 6561fda4bbc105a0dd1f745aee1f8f596f4e3099
+Subproject commit 2acf89ed6fb6444dce2bc738739270bdf6ccb5e4
diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc
index 9b3bcd31..937925e4 100644
--- a/selfdrive/boardd/pigeon.cc
+++ b/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\xFF\xAD\x62\xAD\x1E\x63\x00\x00\x83\x0C"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;
   }
diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py
index 006382db..04422a81 100644
--- a/selfdrive/locationd/test/ublox.py
+++ b/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,10 @@ msg_types = {
     'portID', 'reserved0', 'txReady', 'mode', 'baudRate', 'inProtoMask', 'outProtoMask',
     'reserved4', 'reserved5'
   ]),
+  (CLASS_CFG, MSG_CFG_ITMF):
+  UBloxDescriptor('CFG_ITMF', '<II', [
+    'config', 'config2'
+  ]),
   (CLASS_CFG, MSG_CFG_CFG):
   UBloxDescriptor('CFG_CFG', '<III,B',
                   ['clearMask', 'saveMask', 'loadMask', 'deviceMask']),
diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py
index 81bee2fc..871d7b44 100755
--- a/selfdrive/locationd/test/ubloxd.py
+++ b/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 = '10101101011000101010110111111111'
+  #bits_ITMF_config2 = '00000000000000000110001100011110'
+  ITMF_config1 = 2908925439
+  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)
diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc
index 0eb08a01..064d4f90 100644
--- a/selfdrive/locationd/ublox_msg.cc
+++ b/selfdrive/locationd/ublox_msg.cc
@@ -303,6 +303,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_mon_hw(ubx_t::mon_hw_t *msg) {
   MessageBuilder msg_builder;
   auto hwStatus = msg_builder.initEvent().initUbloxGnss().initHwStatus();
   hwStatus.setNoisePerMS(msg->noise_per_ms());
+  hwStatus.setFlags(msg->flags());
   hwStatus.setAgcCnt(msg->agc_cnt());
   hwStatus.setAStatus((cereal::UbloxGnss::HwStatus::AntennaSupervisorState) msg->a_status());
   hwStatus.setAPower((cereal::UbloxGnss::HwStatus::AntennaPowerStatus) msg->a_power());
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 6c404dcb..bf3374fa 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-9a12b34e64f9fc0016b8b35a2bf8e11f273ab8d2
\ No newline at end of file
+cd48ec4946068b94bf86b67af4a76c812480ea1d
\ No newline at end of file