diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index dd3690db0e..4ac038de20 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -15,6 +15,7 @@ #include #include #include +#include #include @@ -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 last_recv_time; + std::unordered_map 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){ - if (ignition) { - LOGW("received invalid ublox message while onroad, resetting panda GPS"); - need_reset = true; - } - } else { - pigeon_publish_raw(pm, recv); + + // Parse message header + if (ignition && recv.length() >= 3) { + if (recv[0] == (char)ublox::PREAMBLE1 && recv[1] == (char)ublox::PREAMBLE2){ + const char msg_cls = recv[2]; + last_recv_time[msg_cls] = nanos_since_boot(); + } + } + + // 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;