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: 3d98cb72c08ec5d9b9ac5645ea9effecadd6bc38

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