rawgpsd: publish ephemerides (#25931)

* add svpoly parsing

* Publish poly

* add source check

* add safety check for invalid gpsWeek values

* address PR comments

* add qcom ephemeris source type

* bump cereal and laika

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

cereal

@@ -1 +1 @@
-Subproject commit 5ba96b6ded57bcd91e60140ce0036f61370f8512
+Subproject commit b29717c4c328d5cf34d46f682f25267150f82637

laika_repo

@@ -1 +1 @@
-Subproject commit c8bc1fa01be9f22592efb991ee52d3d965d21968
+Subproject commit e1049cde0a68f7d4a70b1ebd76befdc0e163ad55

selfdrive/locationd/laikad.py

@@ -16,7 +16,7 @@ from common.params import Params, put_nonblocking
 from laika import AstroDog
 from laika.constants import SECS_IN_HR, SECS_IN_MIN
 from laika.downloader import DownloadFailed
-from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem
+from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse_qcom_ephem
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
@@ -61,6 +61,7 @@ class Laikad:
     self.last_pos_fix = []
     self.last_pos_residual = []
     self.last_pos_fix_t = None
+    self.gps_week = None
     self.use_qcom = use_qcom
 
   def load_cache(self):
@@ -107,11 +108,11 @@ class Laikad:
     return self.last_pos_fix
 
   def is_good_report(self, gnss_msg):
-    if gnss_msg.which == 'drMeasurementReport' and self.use_qcom:
+    if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
       constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
       # TODO support GLONASS
       return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
-    elif gnss_msg.which == 'measurementReport' and not self.use_qcom:
+    elif gnss_msg.which() == 'measurementReport' and not self.use_qcom:
       return True
     else:
       return False
@@ -129,9 +130,28 @@ class Laikad:
       new_meas = read_raw_ublox(report)
     return week, tow, new_meas
 
+  def is_ephemeris(self, gnss_msg):
+    if self.use_qcom:
+      return gnss_msg.which() == 'drSvPoly'
+    else:
+      return gnss_msg.which() == 'ephemeris'
+
+  def read_ephemeris(self, gnss_msg):
+    # TODO this only works on GLONASS
+    if self.use_qcom:
+      # TODO this is not robust to gps week rollover
+      if self.gps_week is None:
+        return
+      ephem = parse_qcom_ephem(gnss_msg.drSvPoly, self.gps_week)
+    else:
+      ephem = convert_ublox_ephem(gnss_msg.ephemeris)
+    self.astro_dog.add_navs({ephem.prn: [ephem]})
+    self.cache_ephemeris(t=ephem.epoch)
+
   def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
     if self.is_good_report(gnss_msg):
       week, tow, new_meas = self.read_report(gnss_msg)
+      self.gps_week = week
 
       t = gnss_mono_time * 1e-9
       if week > 0:
@@ -172,12 +192,10 @@ class Laikad:
         "correctedMeasurements": meas_msgs
       }
       return dat
-    # TODO this only works on GLONASS, qcom needs live ephemeris parsing too
-    elif gnss_msg.which == 'ephemeris':
-      ephem = convert_ublox_ephem(gnss_msg.ephemeris)
-      self.astro_dog.add_navs({ephem.prn: [ephem]})
-      self.cache_ephemeris(t=ephem.epoch)
-    #elif gnss_msg.which == 'ionoData':
+    elif self.is_ephemeris(gnss_msg):
+      self.read_ephemeris(gnss_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]):
@@ -265,9 +283,11 @@ def create_measurement_msg(meas: GNSSMeasurement):
   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
-    week, time_of_week = -1, -1
+  elif ephem.eph_type == EphemerisType.QCOM_POLY:
+    source_type = EphemerisSourceType.qcom
   else:
     assert ephem.file_epoch is not None
     week = ephem.file_epoch.week
@@ -325,6 +345,7 @@ class EphemerisSourceType(IntEnum):
   nav = 0
   nasaUltraRapid = 1
   glonassIacUltraRapid = 2
+  qcom = 3
 
 
 def main(sm=None, pm=None):
@@ -348,6 +369,17 @@ def main(sm=None, pm=None):
 
     if sm.updated[raw_gnss_socket]:
       gnss_msg = sm[raw_gnss_socket]
+
+      # TODO: Understand and use remaining unknown constellations
+      if gnss_msg.which() == "drMeasurementReport":
+        if getattr(gnss_msg, gnss_msg.which()).source not in ['glonass', 'gps', 'beidou', 'sbas']:
+          continue
+
+        if getattr(gnss_msg, gnss_msg.which()).gpsWeek > np.iinfo(np.int16).max:
+          # gpsWeek 65535 is received rarely from quectel, this cannot be
+          # passed to GnssMeasurements's gpsWeek (Int16)
+          continue
+
       msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay)
       if msg is not None:
         pm.send('gnssMeasurements', msg)

selfdrive/sensord/rawgps/rawgpsd.py

@@ -14,12 +14,13 @@ import cereal.messaging as messaging
 from laika.gps_time import GPSTime
 from system.swaglog import cloudlog
 from selfdrive.sensord.rawgps.modemdiag import ModemDiag, DIAG_LOG_F, setup_logs, send_recv
-from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report,
+from selfdrive.sensord.rawgps.structs import (dict_unpacker, position_report, relist,
                                               gps_measurement_report, gps_measurement_report_sv,
                                               glonass_measurement_report, glonass_measurement_report_sv,
-                                              oemdre_measurement_report, oemdre_measurement_report_sv,
+                                              oemdre_measurement_report, oemdre_measurement_report_sv, oemdre_svpoly_report,
                                               LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
-                                              LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT)
+                                              LOG_GNSS_POSITION_REPORT, LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
+                                              LOG_GNSS_OEMDRE_SVPOLY_REPORT)
 
 DEBUG = int(os.getenv("DEBUG", "0"))==1
 
@@ -28,6 +29,7 @@ LOG_TYPES = [
   LOG_GNSS_GLONASS_MEASUREMENT_REPORT,
   LOG_GNSS_OEMDRE_MEASUREMENT_REPORT,
   LOG_GNSS_POSITION_REPORT,
+  LOG_GNSS_OEMDRE_SVPOLY_REPORT,
 ]
 
 
@@ -146,6 +148,9 @@ def main() -> NoReturn:
   unpack_oemdre_meas, size_oemdre_meas = dict_unpacker(oemdre_measurement_report, True)
   unpack_oemdre_meas_sv, size_oemdre_meas_sv = dict_unpacker(oemdre_measurement_report_sv, True)
 
+  unpack_svpoly, _ = dict_unpacker(oemdre_svpoly_report, True)
+  unpack_position, _ = dict_unpacker(position_report)
+
   unpack_position, _ = dict_unpacker(position_report)
 
   # wait for ModemManager to come up
@@ -258,7 +263,24 @@ def main() -> NoReturn:
 
       pm.send('gpsLocation', msg)
 
-    if log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
+    elif log_type == LOG_GNSS_OEMDRE_SVPOLY_REPORT:
+      msg = messaging.new_message('qcomGnss')
+      dat = unpack_svpoly(log_payload)
+      dat = relist(dat)
+      gnss = msg.qcomGnss
+      gnss.logTs = log_time
+      gnss.init('drSvPoly')
+      poly = gnss.drSvPoly
+      for k,v in dat.items():
+        if k == "version":
+          assert v == 2
+        elif k == "flags":
+          pass
+        else:
+          setattr(poly, k, v)
+      pm.send('qcomGnss', msg)
+
+    elif log_type in [LOG_GNSS_GPS_MEASUREMENT_REPORT, LOG_GNSS_GLONASS_MEASUREMENT_REPORT]:
       msg = messaging.new_message('qcomGnss')
 
       gnss = msg.qcomGnss

selfdrive/sensord/rawgps/structs.py

@@ -56,6 +56,29 @@ oemdre_measurement_report = """
   uint8_t source;
 """
 
+oemdre_svpoly_report = """
+  uint8_t version;
+  uint16_t sv_id;
+  int8_t frequency_index;
+  uint8_t flags;
+  uint16_t iode;
+  double t0;
+  double xyz0[3];
+  double xyzN[9];
+  float other[4];
+  float position_uncertainty;
+  float iono_delay;
+  float iono_dot;
+  float sbas_iono_delay;
+  float sbas_iono_dot;
+  float tropo_delay;
+  float elevation;
+  float elevation_dot;
+  float elevation_uncertainty;
+  double velocity_coeff[12];
+"""
+
+
 oemdre_measurement_report_sv = """
   uint8_t sv_id;
   uint8_t unkn;
@@ -311,3 +334,21 @@ def dict_unpacker(ss, camelcase = False):
     nams = [name_to_camelcase(x) for x in nams]
   sz = calcsize(st)
   return lambda x: dict(zip(nams, unpack_from(st, x))), sz
+
+def relist(dat):
+  list_keys = set()
+  for key in dat.keys():
+    if '[' in key:
+      list_keys.add(key.split('[')[0])
+  list_dict = {}
+  for list_key in list_keys:
+    list_dict[list_key] = []
+    i = 0
+    while True:
+      key = list_key + f'[{i}]'
+      if key not in dat:
+        break
+      list_dict[list_key].append(dat[key])
+      del dat[key]
+      i += 1
+  return {**dat, **list_dict}