Laikad upgrades: prep for laikad lld (#29545)

* Laikad upgrades: prep for laikad lld

* Update ref commit

selfdrive/locationd/laikad.py

@@ -18,7 +18,8 @@ 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, get_sv_id
-from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
+from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
+from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports
 from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
 from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
 from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman
@@ -102,9 +103,10 @@ class Laikad:
     self.use_qcom = use_qcom
     self.first_log_time = None
     self.ttff = -1
+    self.measurement_lag = 0.630 if self.use_qcom else 0.095
 
     # qcom specific stuff
-    self.qcom_reports_received = 1
+    self.qcom_reports_received = 4
     self.qcom_reports = []
 
   def load_cache(self):
@@ -161,47 +163,31 @@ class Laikad:
   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
+
       position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
       if len(position_solution) < 3:
         return None
       position_estimate = position_solution[:3]
-
-      position_std_residual = np.median(np.abs(pr_residuals))
-      position_std = np.median(np.abs(pos_std))/10
-      position_std = max(position_std_residual, position_std) * np.ones(3)
+      position_std = pos_std[:3]
 
       velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements)
       if len(velocity_solution) < 3:
         return None
       velocity_estimate = velocity_solution[:3]
-
-      velocity_std_residual = np.median(np.abs(prr_residuals))
-      velocity_std = np.median(np.abs(vel_std))/10
-      velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3)
+      velocity_std = vel_std[:3]
 
       return position_estimate, position_std, velocity_estimate, velocity_std
 
-  def gps_time_from_qcom_report(self, gnss_msg):
-    report = gnss_msg.drMeasurementReport
-    if report.source == log.QcomGnss.MeasurementSource.gps:
-      report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
-    elif report.source == log.QcomGnss.MeasurementSource.sbas:
-      report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
-    elif report.source == log.QcomGnss.MeasurementSource.glonass:
-      report_time = GPSTime.from_glonass(report.glonassYear,
-                                            report.glonassDay,
-                                            report.glonassMilliseconds / 1000.0)
-    else:
-      raise NotImplementedError(f'Unknownconstellation {report.source}')
-    return report_time
-
   def is_good_report(self, gnss_msg):
-    if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
+    if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom:
       # TODO: Understand and use remaining unknown constellations
       try:
-        constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
+        if gnss_msg.which() == 'drMeasurementReport':
+          constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
+        else:
+          constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source)
         good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS]
-        report_time = self.gps_time_from_qcom_report(gnss_msg)
+        report_time = gps_time_from_qcom_report(gnss_msg)
       except NotImplementedError:
         return False
       # Garbage timestamps with week > 32767 are sometimes sent by module.
@@ -216,21 +202,20 @@ class Laikad:
   def read_report(self, gnss_msg):
     if self.use_qcom:
       # QCOM reports are per constellation, so we need to aggregate them
-      report = gnss_msg.drMeasurementReport
-      report_time = self.gps_time_from_qcom_report(gnss_msg)
-
+      # Additionally, the pseudoranges are broken in the measurementReports
+      # and the doppler filteredSpeed is broken in the drMeasurementReports
+      report_time = gps_time_from_qcom_report(gnss_msg)
       if report_time - self.last_report_time > 0:
         self.qcom_reports_received = max(1, len(self.qcom_reports))
-        self.qcom_reports = [report]
+        self.qcom_reports = [gnss_msg]
       else:
-        self.qcom_reports.append(report)
+        self.qcom_reports.append(gnss_msg)
       self.last_report_time = report_time
 
-      new_meas = []
       if len(self.qcom_reports) == self.qcom_reports_received:
-        for report in self.qcom_reports:
-          new_meas.extend(read_raw_qcom(report))
-
+        new_meas = get_measurements_from_qcom_reports(self.qcom_reports)
+      else:
+        new_meas = []
     else:
       report = gnss_msg.measurementReport
       self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow)
@@ -300,7 +285,7 @@ class Laikad:
   def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
     out_msg = messaging.new_message("gnssMeasurements")
     t = gnss_mono_time * 1e-9
-    msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time}
+    msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)}
     if self.first_log_time is None:
       self.first_log_time = 1e-9 * gnss_mono_time
     if self.is_ephemeris(gnss_msg):

selfdrive/locationd/test/test_laikad.py

@@ -177,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 = 56 if use_qcom else 560
-      valid_fix_expected = 56 if use_qcom else 560
+      correct_msgs_expected = 55 if use_qcom else 560
+      valid_fix_expected = 55 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 @@
-72e3d7b660ee92f5adcc249112cf04c703f4bf9e
+4846f6f416128ba74837829643aa3fe0897c5621