diff --git a/laika_repo b/laika_repo index 73bf110ae0..2e5c7b8a85 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 73bf110ae0093ad86755bf5eb6a03e46ff5c239d +Subproject commit 2e5c7b8a85cbaebda7fa715fe53d5e6ecbd62b0a diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 6431e9d48b..8bd9985170 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -170,9 +170,18 @@ kj::Array 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) { diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 542e72816b..919e9963f1 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -103,6 +103,7 @@ class UbloxMsgParser { inline bool valid_so_far(); std::unordered_map> gps_subframes; + std::unordered_map 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]; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a173cf5205..1aa3aa6f1e 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -18a70665bdb6b6aee4a224e826417049415e8290 +acc5655633af6ade096ad41f680fd8a28c2e3790 \ No newline at end of file