mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 21:14:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ublox tow continuity check (#27001)

Browse Source 
* 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
This commit is contained in:
Kurt Nistelberger
2023-01-18 19:57:54 -07:00
committed by GitHub
parent fa47048e44
commit 6509690ac3
4 changed files with 14 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
laika_repo

Submodule laika_repo updated: 73bf110ae0...2e5c7b8a85

13
selfdrive/locationd/ublox_msg.cc
View File

@@ -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) {

1
selfdrive/locationd/ublox_msg.h
View File

@@ -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];

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
18a70665bdb6b6aee4a224e826417049415e8290
acc5655633af6ade096ad41f680fd8a28c2e3790