Qcom rawgps: Timestamp svpolys (#28738)

* Timestamp svpolys

* leap seconds handled fine

* Fix error

* bump laika

* Update test route

* update ref
old-commit-hash: 47ee0932db

selfdrive/locationd/laikad.py

@@ -98,7 +98,6 @@ class Laikad:
     self.velfix_function = get_velfix_sympy_func()
     self.last_fix_pos = None
     self.last_fix_t = None
-    self.gps_week = None
     self.use_qcom = use_qcom
     self.first_log_time = None
     self.ttff = -1
@@ -245,11 +244,8 @@ class Laikad:
 
   def read_ephemeris(self, gnss_msg):
     if self.use_qcom:
-      # TODO this is not robust to gps week rollover
-      if self.gps_week is None:
-        return
       try:
-        ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
+        ephem = parse_qcom_ephem(gnss_msg.drSvPoly)
         self.astro_dog.add_qcom_polys({ephem.prn: [ephem]})
       except Exception:
         cloudlog.exception("Error parsing qcom svPoly ephemeris from qcom module")
@@ -310,7 +306,6 @@ class Laikad:
       self.read_ephemeris(gnss_msg)
     elif self.is_good_report(gnss_msg):
       report_t, new_meas = self.read_report(gnss_msg)
-      self.gps_week = report_t.week
       if report_t.week > 0:
         if self.auto_fetch_navs:
           self.fetch_navs(report_t, block)

selfdrive/locationd/test/test_laikad.py

@@ -20,12 +20,12 @@ from tools.lib.logreader import LogReader
 from selfdrive.test.process_replay.process_replay import get_process_config, replay_process
 
 GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC = GPSTime.from_datetime(datetime(2022, month=1, day=29, hour=12))
+UBLOX_TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
+QCOM_TEST_ROUTE = "616dc83ca1f7f11e|2023-07-11--10-52-31"
 
 
-def get_log_ublox(segs=range(0)):
-  logs = []
-  for i in segs:
-    logs.extend(LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", i)))
+def get_log_ublox():
+  logs = LogReader(get_url(UBLOX_TEST_ROUTE, 0))
 
   ublox_cfg = get_process_config("ubloxd")
   all_logs = replay_process(ublox_cfg, logs)
@@ -47,10 +47,8 @@ def get_log_ublox(segs=range(0)):
   return all_logs, low_gnss
 
 
-def get_log_qcom(segs=range(0)):
-  logs = []
-  for i in segs:
-    logs.extend(LogReader(get_url("b0b3cba7abf862d1|2023-03-11--09-40-33", i)))
+def get_log_qcom():
+  logs = LogReader(get_url(QCOM_TEST_ROUTE, 0))
   all_logs = [m for m in logs if m.which() == 'qcomGnss']
   return all_logs
 
@@ -98,10 +96,10 @@ class TestLaikad(unittest.TestCase):
 
   @classmethod
   def setUpClass(cls):
-    logs, low_gnss = get_log_ublox(range(1))
+    logs, low_gnss = get_log_ublox()
     cls.logs = logs
     cls.low_gnss = low_gnss
-    cls.logs_qcom = get_log_qcom(range(1))
+    cls.logs_qcom = get_log_qcom()
     first_gps_time = get_first_gps_time(logs)
     cls.first_gps_time = first_gps_time
 
@@ -179,8 +177,8 @@ class TestLaikad(unittest.TestCase):
       laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom)
       # Disable fetch_orbits to test NAV only
       correct_msgs = verify_messages(logs, laikad)
-      correct_msgs_expected = 43 if use_qcom else 560
-      valid_fix_expected = 43 if use_qcom else 560
+      correct_msgs_expected = 56 if use_qcom else 560
+      valid_fix_expected = 56 if use_qcom else 560
 
       self.assertEqual(correct_msgs_expected, len(correct_msgs))
       self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-35eb63be5d31ad443da70be29836ce695206d54d
+a683d689bd74ba5ba7c10bfad237aacc04b978c3

selfdrive/test/update_ci_routes.py

@@ -6,6 +6,7 @@ from tqdm import tqdm
 from azure.storage.blob import BlockBlobService  # pylint: disable=import-error
 
 from selfdrive.car.tests.routes import routes as test_car_models_routes
+from selfdrive.locationd.test.test_laikad import UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE
 from selfdrive.test.process_replay.test_processes import source_segments as replay_segments
 from xx.chffr.lib import azureutil  # pylint: disable=import-error
 from xx.chffr.lib.storage import _DATA_ACCOUNT_PRODUCTION, _DATA_ACCOUNT_CI, _DATA_BUCKET_PRODUCTION  # pylint: disable=import-error
@@ -81,6 +82,7 @@ if __name__ == "__main__":
 
   if not len(to_sync):
     # sync routes from the car tests routes and process replay
+    to_sync.extend([UBLOX_TEST_ROUTE, QCOM_TEST_ROUTE])
     to_sync.extend([rt.route for rt in test_car_models_routes])
     to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments])
 

system/sensord/rawgps/rawgpsd.py

@@ -14,7 +14,9 @@ from struct import unpack_from, calcsize, pack
 from cereal import log
 import cereal.messaging as messaging
 from common.gpio import gpio_init, gpio_set
-from laika.gps_time import GPSTime
+from laika.gps_time import GPSTime, utc_to_gpst, get_leap_seconds
+from laika.helpers import get_prn_from_nmea_id
+from laika.constants import SECS_IN_HR, SECS_IN_DAY, SECS_IN_WEEK
 from system.hardware.tici.pins import GPIO
 from system.swaglog import cloudlog
 from system.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
@@ -248,6 +250,7 @@ def main() -> NoReturn:
   signal.signal(signal.SIGTERM, cleanup)
 
   setup_quectel(diag)
+  current_gps_time = utc_to_gpst(GPSTime.from_datetime(datetime.utcnow()))
   cloudlog.warning("quectel setup done")
   gpio_init(GPIO.UBLOX_PWR_EN, True)
   gpio_set(GPIO.UBLOX_PWR_EN, True)
@@ -316,6 +319,8 @@ def main() -> NoReturn:
               setattr(sv.measurementStatus, kk, bool(v & (1<<vv)))
           else:
             setattr(sv, k, v)
+      if report.source == log.QcomGnss.MeasurementSource.gps:
+        current_gps_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
       pm.send('qcomGnss', msg)
     elif log_type == LOG_GNSS_POSITION_REPORT:
       report = unpack_position(log_payload)
@@ -358,6 +363,21 @@ def main() -> NoReturn:
           pass
         else:
           setattr(poly, k, v)
+
+      prn = get_prn_from_nmea_id(poly.svId)
+      if prn[0] == 'R':
+        epoch = GPSTime(current_gps_time.week, (poly.t0 - 3*SECS_IN_HR + SECS_IN_DAY) % (SECS_IN_WEEK) + get_leap_seconds(current_gps_time))
+      else:
+        epoch = GPSTime(current_gps_time.week, poly.t0)
+
+      # handle week rollover
+      if epoch.tow < SECS_IN_DAY and current_gps_time.tow > 6*SECS_IN_DAY:
+        epoch.week += 1
+      elif epoch.tow > 6*SECS_IN_DAY and current_gps_time.tow < SECS_IN_DAY:
+        epoch.week -= 1
+
+      poly.gpsWeek = epoch.week
+      poly.gpsTow = epoch.tow
       pm.send('qcomGnss', msg)
 
     elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]: