Ttff logs (#27610)

* ttff in logs

* More meta info

* cleanup

* passess tests

* unused import

* fix linting

* ttff of 0 is invalid

* ref commit

* bump cereal

* Update ref_commit

* sort ephem status list

* sort ephem status list

* update ref
old-commit-hash: 204f7d8623

cereal

@@ -1 +1 @@
-Subproject commit 80a8eb84593feb08dfc0bd47e5c04e586ea4b7a6
+Subproject commit 7492dc3f45702dfdbe0f353f38844e7fbf39ca71

laika_repo

@@ -1 +1 @@
-Subproject commit 8463fe8fed272f58093406a990532084bc060359
+Subproject commit 719c79a156cd254fa8ebde101c2f48583f91afed

selfdrive/locationd/laikad.py

@@ -18,7 +18,7 @@ from laika.constants import SECS_IN_HR, SECS_IN_MIN
 from laika.downloader import DownloadFailed
 from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
 from laika.gps_time import GPSTime
-from laika.helpers import ConstellationId
+from laika.helpers import ConstellationId, get_sv_id
 from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
 from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
 from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
@@ -33,6 +33,44 @@ CACHE_VERSION = 0.2
 POS_FIX_RESIDUAL_THRESHOLD = 100.0
 
 
+class LogEphemerisType(IntEnum):
+  nav = 0
+  nasaUltraRapid = 1
+  glonassIacUltraRapid = 2
+  qcom = 3
+
+class EphemerisSource(IntEnum):
+  gnssChip = 0
+  internet = 1
+  cache = 2
+  unknown = 3
+
+def get_log_eph_type(ephem):
+  if ephem.eph_type == EphemerisType.NAV:
+    source_type = LogEphemerisType.nav
+  elif ephem.eph_type == EphemerisType.QCOM_POLY:
+    source_type = LogEphemerisType.qcom
+  else:
+    assert ephem.file_epoch is not None
+    file_src = ephem.file_source
+    if file_src == 'igu':  # example nasa: '2214/igu22144_00.sp3.Z'
+      source_type = LogEphemerisType.nasaUltraRapid
+    elif file_src == 'Sta':  # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
+      source_type = LogEphemerisType.glonassIacUltraRapid
+    else:
+      raise Exception(f"Didn't expect file source {file_src}")
+  return source_type
+
+def get_log_eph_source(ephem):
+  if ephem.file_name == 'qcom' or ephem.file_name == 'ublox':
+    source = EphemerisSource.gnssChip
+  elif ephem.file_name == EPHEMERIS_CACHE:
+    source = EphemerisSource.cache
+  else:
+    source = EphemerisSource.internet
+  return source
+
+
 class Laikad:
   def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_navs=True, auto_update=False,
                valid_ephem_types=(EphemerisType.NAV, EphemerisType.QCOM_POLY),
@@ -64,6 +102,8 @@ class Laikad:
     self.last_fix_t = None
     self.gps_week = None
     self.use_qcom = use_qcom
+    self.first_log_time = None
+    self.ttff = -1
 
   def load_cache(self):
     if not self.save_ephemeris:
@@ -76,8 +116,8 @@ class Laikad:
     nav_dict = {}
     try:
       ephem_cache = ephemeris_structs.EphemerisCache.from_bytes(cache_bytes)
-      glonass_navs = [GLONASSEphemeris(data_struct) for data_struct in ephem_cache.glonassEphemerides] 
+      glonass_navs = [GLONASSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.glonassEphemerides] 
-      gps_navs = [GPSEphemeris(data_struct) for data_struct in ephem_cache.gpsEphemerides]
+      gps_navs = [GPSEphemeris(data_struct, file_name=EPHEMERIS_CACHE) for data_struct in ephem_cache.gpsEphemerides]
       for e in sum([glonass_navs, gps_navs], []):
         if e.prn not in nav_dict:
           nav_dict[e.prn] = []
@@ -99,6 +139,22 @@ class Laikad:
       cloudlog.debug("Cache saved")
       self.last_cached_t = self.last_report_time
 
+  def create_ephem_statuses(self):
+    ephemeris_statuses = []
+    prns_to_check = list(self.astro_dog.get_all_ephem_prns())
+    prns_to_check.sort()
+    for prn in prns_to_check:
+      eph = self.astro_dog.get_eph(prn, self.last_report_time)
+      if eph is not None:
+        status = log.GnssMeasurements.EphemerisStatus.new_message()
+        status.constellationId = ConstellationId.from_rinex_char(prn[0]).value
+        status.svId = get_sv_id(prn)
+        status.type = get_log_eph_type(eph).value
+        status.source = get_log_eph_source(eph).value
+        ephemeris_statuses.append(status)
+    return ephemeris_statuses
+
+    
   def get_lsq_fix(self, t, measurements):
     if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
       min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4
@@ -170,10 +226,10 @@ class Laikad:
     else:
       if gnss_msg.which() == 'ephemeris':
         data_struct = ephemeris_structs.Ephemeris.new_message(**gnss_msg.ephemeris.to_dict())
-        ephem = GPSEphemeris(data_struct)
+        ephem = GPSEphemeris(data_struct, file_name='ublox')
       elif gnss_msg.which() == 'glonassEphemeris':
         data_struct = ephemeris_structs.GlonassEphemeris.new_message(**gnss_msg.glonassEphemeris.to_dict())
-        ephem = GLONASSEphemeris(data_struct)
+        ephem = GLONASSEphemeris(data_struct, file_name='ublox')
       else:
         cloudlog.error(f"Unsupported ephemeris type: {gnss_msg.which()}")
         return
@@ -204,6 +260,12 @@ class Laikad:
 
   def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
     out_msg = messaging.new_message("gnssMeasurements")
+    out_msg.gnssMeasurements = {
+        "timeToFirstFix": self.ttff,
+        "ephemerisStatuses" : self.create_ephem_statuses(),
+    }
+    if self.first_log_time is None:
+      self.first_log_time = 1e-9 * gnss_mono_time
     if self.is_ephemeris(gnss_msg):
       self.read_ephemeris(gnss_msg)
       return out_msg
@@ -223,6 +285,8 @@ class Laikad:
       output = self.process_report(new_meas, t)
       if output is None:
         return out_msg
+      if self.ttff <= 0:
+        self.ttff = max(1e-3, t - self.first_log_time)
       position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
 
       self.update_localizer(position_estimate, t, corrected_measurements)
@@ -244,14 +308,12 @@ class Laikad:
         "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)),
 
         "measTime": gnss_mono_time,
-        "correctedMeasurements": meas_msgs
+        "correctedMeasurements": meas_msgs,
+        "timeToFirstFix": self.ttff,
+        "ephemerisStatuses" : self.create_ephem_statuses(),
       }
     return out_msg
 
-    #elif gnss_msg.which() == 'ionoData':
-    # TODO: add this, Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
-
-
   def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]):
     # Check time and outputs are valid
     valid = self.kf_valid(t)
@@ -335,31 +397,8 @@ def create_measurement_msg(meas: GNSSMeasurement):
   c.satPos = meas.sat_pos_final.tolist()
   c.satVel = meas.sat_vel.tolist()
   c.satVel = meas.sat_vel.tolist()
-  ephem = meas.sat_ephemeris
-  assert ephem is not None
-  week, time_of_week = -1, -1
-  if ephem.eph_type == EphemerisType.NAV:
-    source_type = EphemerisSourceType.nav
-  elif ephem.eph_type == EphemerisType.QCOM_POLY:
-    source_type = EphemerisSourceType.qcom
-  else:
-    assert ephem.file_epoch is not None
-    week = ephem.file_epoch.week
-    time_of_week = ephem.file_epoch.tow
-    file_src = ephem.file_source
-    if file_src == 'igu':  # example nasa: '2214/igu22144_00.sp3.Z'
-      source_type = EphemerisSourceType.nasaUltraRapid
-    elif file_src == 'Sta':  # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
-      source_type = EphemerisSourceType.glonassIacUltraRapid
-    else:
-      raise Exception(f"Didn't expect file source {file_src}")
-
-  c.ephemerisSource.type = source_type.value
-  c.ephemerisSource.gpsWeek = week
-  c.ephemerisSource.gpsTimeOfWeek = int(time_of_week)
   return c
 
-
 def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]):
   ekf_data = defaultdict(list)
   for m in measurements:
@@ -375,18 +414,6 @@ def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMe
       gnss_kf.predict_and_observe(t, kind, data)
 
 
-class EphemerisSourceType(IntEnum):
-  nav = 0
-  nasaUltraRapid = 1
-  glonassIacUltraRapid = 2
-  qcom = 3
-
-
-def process_msg(laikad, gnss_msg, mono_time, block=False):
-
-  return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
-
-
 def clear_tmp_cache():
   if os.path.exists(DOWNLOADS_CACHE_FOLDER):
     shutil.rmtree(DOWNLOADS_CACHE_FOLDER)

selfdrive/locationd/test/test_laikad.py

@@ -12,11 +12,11 @@ from tqdm import tqdm
 
 from laika.constants import SECS_IN_DAY
 from laika.downloader import DownloadFailed
-from laika.ephemeris import EphemerisType, GPSEphemeris, ephemeris_structs
+from laika.ephemeris import EphemerisType
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, read_raw_ublox, read_raw_qcom
-from selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg
+from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader
 from selfdrive.manager.process_config import managed_processes
@@ -138,9 +138,9 @@ class TestLaikad(unittest.TestCase):
     laikad = Laikad()
     laikad.fetch_navs(gpstime, block=False)
     laikad.orbit_fetch_future.result(30)
+    
     # Get results and save orbits to laikad:
     laikad.fetch_navs(gpstime, block=False)
-
     ephem = laikad.astro_dog.navs['G01'][0]
     self.assertIsNotNone(ephem)
 
@@ -153,6 +153,7 @@ class TestLaikad(unittest.TestCase):
     self.assertIsNotNone(ephem)
     self.assertNotEqual(ephem, ephem2)
 
+
   def test_fetch_navs_with_wrong_clocks(self):
     laikad = Laikad()
 
@@ -175,35 +176,6 @@ class TestLaikad(unittest.TestCase):
     check_has_navs()
     self.assertEqual(laikad.last_fetch_navs_t, real_current_time)
 
-  def test_ephemeris_source_in_msg(self):
-    dicto = {'svId': 1}
-    data_mock = ephemeris_structs.Ephemeris.new_message(**dicto)
-
-    gpstime = GPS_TIME_PREDICTION_ORBITS_RUSSIAN_SRC
-    laikad = Laikad()
-    laikad.fetch_navs(gpstime, block=True)
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
-    msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
-    # Verify gps satellite returns same source
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['R01'][0])
-    msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
-
-    # Test nasa source by using older date
-    gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
-    laikad = Laikad()
-    laikad.fetch_navs(gpstime, block=True)
-    meas = get_measurement_mock(gpstime, laikad.astro_dog.navs['G01'][0])
-    msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
-
-    # Test nav source type
-    ephem = GPSEphemeris(data_mock)
-    meas = get_measurement_mock(gpstime, ephem)
-    msg = create_measurement_msg(meas)
-    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)
-
   def test_laika_online(self):
     laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
     correct_msgs = verify_messages(self.logs, laikad)
@@ -263,10 +235,14 @@ class TestLaikad(unittest.TestCase):
     self.assertGreater(len(laikad.astro_dog.navs[prn]), 0)
 
   def test_get_navs_in_process(self):
+    for auto_fetch_navs in [True, False]:
       for use_qcom, logs in zip([True, False], [self.logs_qcom, self.logs]):
-      laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=False)
+        laikad = Laikad(auto_update=False, use_qcom=use_qcom, auto_fetch_navs=auto_fetch_navs)
         has_navs = False
         has_fix = False
+        seen_chip_eph = False
+        seen_internet_eph = False
+
         for m in logs:
           gnss_msg = m.qcomGnss if use_qcom else m.ubloxGnss
           out_msg = laikad.process_gnss_msg(gnss_msg, m.logMonoTime, block=False)
@@ -276,11 +252,15 @@ class TestLaikad(unittest.TestCase):
           has_navs = len(vals) > 0 and max([len(v) for v in vals]) > 0
           vals = laikad.astro_dog.qcom_polys.values()
           has_polys = len(vals) > 0 and max([len(v) for v in vals]) > 0
-        if out_msg is not None:
           has_fix = has_fix or out_msg.gnssMeasurements.positionECEF.valid
+          if len(out_msg.gnssMeasurements.ephemerisStatuses):
+            seen_chip_eph = seen_chip_eph or any([x.source == 'gnssChip' for x in out_msg.gnssMeasurements.ephemerisStatuses])
+            seen_internet_eph = seen_internet_eph or any([x.source == 'internet' for x in out_msg.gnssMeasurements.ephemerisStatuses])
             
         self.assertTrue(has_navs or has_polys)
         self.assertTrue(has_fix)
+        self.assertTrue(seen_chip_eph or auto_fetch_navs)
+        self.assertTrue(seen_internet_eph or not auto_fetch_navs)
         self.assertEqual(len(laikad.astro_dog.navs_fetched_times._ranges), 0)
         self.assertEqual(None, laikad.orbit_fetch_future)
 
@@ -303,18 +283,19 @@ class TestLaikad(unittest.TestCase):
       Params().remove(EPHEMERIS_CACHE)
 
       #laikad.astro_dog.get_navs(self.first_gps_time)
-      laikad.last_report_time = GPSTime(2,0)
+      msg = verify_messages(logs, laikad, return_one_success=True)
-      laikad.fetch_navs(self.first_gps_time, block=True)
+      laikad.cache_ephemeris()
-
-      # Wait for cache to save
       wait_for_cache()
 
       # Check both nav and orbits separate
-      laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV, save_ephemeris=True, use_qcom=use_qcom)
+      laikad = Laikad(auto_update=False, valid_ephem_types=EphemerisType.NAV,
+                      save_ephemeris=True, use_qcom=use_qcom, auto_fetch_navs=False)
       # Verify navs are loaded from cache
       self.dict_has_values(laikad.astro_dog.navs)
       # Verify cache is working for only nav by running a segment
       msg = verify_messages(logs, laikad, return_one_success=True)
+      self.assertTrue(len(msg.gnssMeasurements.ephemerisStatuses))
+      self.assertTrue(any([x.source=='cache' for x in msg.gnssMeasurements.ephemerisStatuses]))
       self.assertIsNotNone(msg)
 
 
@@ -349,7 +330,5 @@ class TestLaikad(unittest.TestCase):
   def dict_has_values(self, dct):
     self.assertGreater(len(dct), 0)
     self.assertGreater(min([len(v) for v in dct.values()]), 0)
-
-
 if __name__ == "__main__":
   unittest.main()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-c8ba9450af16f9efc70eb76a5edc205aed53e779
+5775220ec2e62dcdedb92d96270f1380bbd88c39