From 3d98cb72c08ec5d9b9ac5645ea9effecadd6bc38 Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Wed, 1 Feb 2023 15:19:40 -0700 Subject: [PATCH] Ubloxd: gps add iodc check (#27162) * gps add iodc check * add test * simplify * update ref --------- Co-authored-by: Kurt Nistelberger --- .../locationd/test/test_ublox_processing.py | 38 +++++++++++++- selfdrive/locationd/ublox_msg.cc | 52 +++++++------------ selfdrive/locationd/ublox_msg.h | 1 - selfdrive/test/process_replay/ref_commit | 2 +- 4 files changed, 55 insertions(+), 38 deletions(-) diff --git a/selfdrive/locationd/test/test_ublox_processing.py b/selfdrive/locationd/test/test_ublox_processing.py index 7aa588d43e..cd4ce0de04 100755 --- a/selfdrive/locationd/test/test_ublox_processing.py +++ b/selfdrive/locationd/test/test_ublox_processing.py @@ -1,5 +1,5 @@ import unittest - +import time import numpy as np from laika import AstroDog @@ -8,7 +8,8 @@ from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ from laika.opt import calc_pos_fix from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader - +from selfdrive.test.helpers import with_processes +import cereal.messaging as messaging def get_gnss_measurements(log_reader): gnss_measurements = [] @@ -21,6 +22,12 @@ def get_gnss_measurements(log_reader): gnss_measurements.append(read_raw_ublox(report)) return gnss_measurements +def get_ublox_raw(log_reader): + ublox_raw = [] + for msg in log_reader: + if msg.which() == "ubloxRaw": + ublox_raw.append(msg) + return ublox_raw class TestUbloxProcessing(unittest.TestCase): NUM_TEST_PROCESS_MEAS = 10 @@ -30,6 +37,10 @@ class TestUbloxProcessing(unittest.TestCase): lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) cls.gnss_measurements = get_gnss_measurements(lr) + # test gps ephemeris continuity check (drive has ephemeris issues with cutover data) + lr = LogReader(get_url("37b6542f3211019a|2023-01-15--23-45-10", 14)) + cls.ublox_raw = get_ublox_raw(lr) + def test_read_ublox_raw(self): count_gps = 0 count_glonass = 0 @@ -76,6 +87,29 @@ class TestUbloxProcessing(unittest.TestCase): self.assertEqual(count_processed_measurements, 69) self.assertEqual(count_corrected_measurements, 69) + @with_processes(['ubloxd']) + def test_ublox_gps_cutover(self): + time.sleep(2) + ugs = messaging.sub_sock("ubloxGnss", timeout=0.1) + ur_pm = messaging.PubMaster(['ubloxRaw']) + + def replay_segment(): + rcv_msgs = [] + for msg in self.ublox_raw: + ur_pm.send(msg.which(), msg.as_builder()) + time.sleep(0.01) + rcv_msgs += messaging.drain_sock(ugs) + + time.sleep(0.1) + rcv_msgs += messaging.drain_sock(ugs) + return rcv_msgs + + # replay twice to enforce cutover data on rewind + rcv_msgs = replay_segment() + rcv_msgs += replay_segment() + + ephems_cnt = sum(m.ubloxGnss.which() == 'ephemeris' for m in rcv_msgs) + self.assertEqual(ephems_cnt, 15) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 4b31b0101e..127c7e56b6 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -166,26 +166,25 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m { 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(); - 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; + int subframe_id = subframe.how()->subframe_id(); + if (subframe_id > 3) { + // dont parse almanac subframes + return kj::Array(); } - if (clear_buffer) gps_subframes[sv_id].clear(); - - gps_subframes[sv_id][subframe_id] = subframe_data; - gps_sat_tow_count[sv_id] = tow_counter; + gps_subframes[msg->sv_id()][subframe_id] = subframe_data; } - if (gps_subframes[msg->sv_id()].size() == 5) { + // publish if subframes 1-3 have been collected + if (gps_subframes[msg->sv_id()].size() == 3) { MessageBuilder msg_builder; auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); eph.setSvId(msg->sv_id()); + int iode_s2 = 0; + int iode_s3 = 0; + int iodc_lsb = 0; + // Subframe 1 { kaitai::kstream stream(gps_subframes[msg->sv_id()][1]); @@ -199,6 +198,7 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m eph.setAf1(subframe_1->af_1() * pow(2, -43)); eph.setAf0(subframe_1->af_0() * pow(2, -31)); eph.setSvHealth(subframe_1->sv_health()); + iodc_lsb = subframe_1->iodc_lsb(); } // Subframe 2 @@ -215,6 +215,7 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m eph.setCus(subframe_2->c_us() * pow(2, -29)); eph.setA(pow(subframe_2->sqrt_a() * pow(2, -19), 2.0)); eph.setToe(subframe_2->t_oe() * pow(2, 4)); + iode_s2 = subframe_2->iode(); } // Subframe 3 @@ -232,31 +233,14 @@ kj::Array UbloxMsgParser::parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *m eph.setOmegaDot(subframe_3->omega_dot() * pow(2, -43) * gpsPi); eph.setIode(subframe_3->iode()); eph.setIDot(subframe_3->idot() * pow(2, -43) * gpsPi); + iode_s3 = subframe_3->iode(); } - // Subframe 4 - { - kaitai::kstream stream(gps_subframes[msg->sv_id()][4]); - gps_t subframe(&stream); - gps_t::subframe_4_t* subframe_4 = static_cast(subframe.body()); - - // This is page 18, why is the page id 56? - if (subframe_4->data_id() == 1 && subframe_4->page_id() == 56) { - auto iono = static_cast(subframe_4->body()); - double a0 = iono->a0() * pow(2, -30); - double a1 = iono->a1() * pow(2, -27); - double a2 = iono->a2() * pow(2, -24); - double a3 = iono->a3() * pow(2, -24); - eph.setIonoAlpha({a0, a1, a2, a3}); - - double b0 = iono->b0() * pow(2, 11); - double b1 = iono->b1() * pow(2, 14); - double b2 = iono->b2() * pow(2, 16); - double b3 = iono->b3() * pow(2, 16); - eph.setIonoBeta({b0, b1, b2, b3}); - } + gps_subframes[msg->sv_id()].clear(); + if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) { + // data set cutover, reject ephemeris + return kj::Array(); } - return capnp::messageToFlatArray(msg_builder); } return kj::Array(); diff --git a/selfdrive/locationd/ublox_msg.h b/selfdrive/locationd/ublox_msg.h index 736e6d5171..f031313168 100644 --- a/selfdrive/locationd/ublox_msg.h +++ b/selfdrive/locationd/ublox_msg.h @@ -105,7 +105,6 @@ class UbloxMsgParser { kj::Array parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg); 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 959c213a03..a6c8aa682d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9ef210f7e473fa46dd43337b5f09eeabebc694b7 \ No newline at end of file +e825c953316277a342284a1f10b8dab88e95fc31 \ No newline at end of file