Boardd: Track ublox message frequency and re-init (#20278)

* track message frequency

* also keep old reset trigger

* cleanup

* improve comments

* Add 10 seconds

* these are now signed

* also check ignition last

* cleanup

* correct include

* lower threshold

* dont clear map

selfdrive/boardd/boardd.cc

@@ -15,6 +15,7 @@
 #include <bitset>
 #include <thread>
 #include <atomic>
+#include <unordered_map>
 
 #include <libusb-1.0/libusb.h>
 
@@ -25,6 +26,7 @@
 #include "common/swaglog.h"
 #include "common/timing.h"
 #include "messaging.hpp"
+#include "locationd/ublox_msg.h"
 
 #include "panda.h"
 #include "pigeon.h"
@@ -482,24 +484,54 @@ void pigeon_thread() {
   Pigeon * pigeon = Pigeon::connect(panda);
 #endif
 
+  std::unordered_map<char, uint64_t> last_recv_time;
+  std::unordered_map<char, int64_t> cls_max_dt = {
+    {(char)ublox::CLASS_NAV, int64_t(500000000ULL)}, // 0.5s - msg is 10Hz
+    {(char)ublox::CLASS_RXM, int64_t(500000000ULL)}, // 0.5s - msg is 10Hz
+    {(char)ublox::CLASS_MON, int64_t(2000000000ULL)}, // 2.0s - msg is 1Hz
+  };
+
   while (!do_exit && panda->connected) {
     bool need_reset = false;
     std::string recv = pigeon->receive();
-    if (recv.length() > 0) {
+
-      if (recv[0] == (char)0x00){
+    // Parse message header
-        if (ignition) {
+    if (ignition && recv.length() >= 3) {
-          LOGW("received invalid ublox message while onroad, resetting panda GPS");
+      if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2){
-          need_reset = true;
+        const char msg_cls = recv[2];
-        }
+        last_recv_time[msg_cls] = nanos_since_boot();
-      } else {
+      }
-        pigeon_publish_raw(pm, recv);
+    }
+
+    // Check based on message frequency
+    for (const auto& [msg_cls, max_dt] : cls_max_dt) {
+      int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
+      if (ignition_last && ignition && dt > max_dt) {
+        LOGE("ublox receive timeout, msg class: 0x%02x, dt %llu, resetting panda GPS", msg_cls, dt);
+        need_reset = true;
       }
     }
 
+    // Check based on null bytes
+    if (ignition && recv.length() > 0 && recv[0] == (char)0x00){
+      need_reset = true;
+      LOGW("received invalid ublox message while onroad, resetting panda GPS");
+    }
+
+    if (recv.length() > 0){
+      pigeon_publish_raw(pm, recv);
+    }
+
     // init pigeon on rising ignition edge
     // since it was turned off in low power mode
     if((ignition && !ignition_last) || need_reset) {
       pigeon->init();
+
+      // Set receive times to current time
+      uint64_t t = nanos_since_boot() + 10000000000ULL; // Give ublox 10 seconds to start
+      for (const auto& [msg_cls, dt] : cls_max_dt) {
+        last_recv_time[msg_cls] = t;
+      }
     }
 
     ignition_last = ignition;