Ublox tow continuity check (#27001)

* add continuity check

* simplify

* remove pair

* update refs

* bump laika update refs

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: 669becadde

laika_repo

@@ -1 +1 @@
-Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d
+Subproject commit 2e5c7b8a85cbaebda7fa715fe53d5e6ecbd62b0a

selfdrive/locationd/ublox_msg.cc

@@ -170,9 +170,18 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
       kaitai::kstream stream(subframe_data);
       gps_t subframe(&stream);
       int subframe_id = subframe.how()->subframe_id();
+      int sv_id = msg->sv_id();
+      uint64_t tow_counter = subframe.how()->tow_count();
 
-      if (subframe_id == 1) gps_subframes[msg->sv_id()].clear();
-      gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
+      bool clear_buffer = subframe_id == 1;
+      if (gps_sat_tow_count.count(sv_id) != 0) {
+        int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id];
+        clear_buffer |= counter_diff != 1 && counter_diff != -100798;
+      }
+      if (clear_buffer) gps_subframes[sv_id].clear();
+
+      gps_subframes[sv_id][subframe_id] = subframe_data;
+      gps_sat_tow_count[sv_id] = tow_counter;
     }
 
     if (gps_subframes[msg->sv_id()].size() == 5) {

selfdrive/locationd/ublox_msg.h

@@ -103,6 +103,7 @@ class UbloxMsgParser {
     inline bool valid_so_far();
 
     std::unordered_map<int, std::unordered_map<int, std::string>> gps_subframes;
+    std::unordered_map<int, uint64_t> gps_sat_tow_count;
 
     size_t bytes_in_parse_buf = 0;
     uint8_t msg_parse_buf[ublox::UBLOX_HEADER_SIZE + ublox::UBLOX_MAX_MSG_SIZE];

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-18a70665bdb6b6aee4a224e826417049415e8290
+acc5655633af6ade096ad41f680fd8a28c2e3790