Ubloxd: gps add iodc check (#27162)

* gps add iodc check

* add test

* simplify

* update ref

---------

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

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()

selfdrive/locationd/ublox_msg.cc

@@ -166,26 +166,25 @@ kj::Array<capnp::word> 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;
+    int subframe_id = subframe.how()->subframe_id();
-    if (gps_sat_tow_count.count(sv_id) != 0) {
+    if (subframe_id > 3) {
-      int64_t counter_diff = tow_counter - gps_sat_tow_count[sv_id];
+      // dont parse almanac subframes
-      clear_buffer |= counter_diff != 1 && counter_diff != -100798;
+      return kj::Array<capnp::word>();
     }
-    if (clear_buffer) gps_subframes[sv_id].clear();
+    gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
-
-    gps_subframes[sv_id][subframe_id] = subframe_data;
-    gps_sat_tow_count[sv_id] = tow_counter;
   }
 
-  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<capnp::word> 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<capnp::word> 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<capnp::word> 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
+    gps_subframes[msg->sv_id()].clear();
-    {
+    if (iodc_lsb != iode_s2 || iodc_lsb != iode_s3) {
-      kaitai::kstream stream(gps_subframes[msg->sv_id()][4]);
+      // data set cutover, reject ephemeris
-      gps_t subframe(&stream);
+      return kj::Array<capnp::word>();
-      gps_t::subframe_4_t* subframe_4 = static_cast<gps_t::subframe_4_t*>(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<gps_t::subframe_4_t::ionosphere_data_t*>(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});
-      }
     }
-
     return capnp::messageToFlatArray(msg_builder);
   }
   return kj::Array<capnp::word>();

selfdrive/locationd/ublox_msg.h

@@ -105,7 +105,6 @@ class UbloxMsgParser {
     kj::Array<capnp::word> parse_gps_ephemeris(ubx_t::rxm_sfrbx_t *msg);
 
     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 @@
-9ef210f7e473fa46dd43337b5f09eeabebc694b7
+e825c953316277a342284a1f10b8dab88e95fc31