Support quectel gps (#25299)

* qcom clock gets corrected randomly it seems

* Use quectel gps

* fix small laikad bugs

* Support both

* Support ublox and qcom clock model

* fix laikad test

* fix typo

* Back to original value

* More typos

Co-authored-by: Comma Device <device@comma.ai>

selfdrive/locationd/laikad.py

@@ -19,7 +19,7 @@ from laika.downloader import DownloadFailed
 from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_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
+from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
 from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun
 from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
 from selfdrive.locationd.models.gnss_kf import GNSSKalman
@@ -36,7 +36,7 @@ POS_FIX_RESIDUAL_THRESHOLD = 100.0
 class Laikad:
   def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False,
                valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV),
-               save_ephemeris=False):
+               save_ephemeris=False, use_qcom=False):
     """
     valid_const: GNSS constellation which can be used
     auto_fetch_orbits: If true fetch orbits from internet when needed
@@ -45,14 +45,14 @@ class Laikad:
     save_ephemeris: If true saves and loads nav and orbit ephemeris to cache.
     """
     self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER)
-    self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True)
+    self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom)
 
     self.auto_fetch_orbits = auto_fetch_orbits
     self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None
     self.orbit_fetch_future: Optional[Future] = None
 
     self.last_fetch_orbits_t = None
-    self.got_first_ublox_msg = False
+    self.got_first_gnss_msg = False
     self.last_cached_t = None
     self.save_ephemeris = save_ephemeris
     self.load_cache()
@@ -61,6 +61,7 @@ class Laikad:
     self.last_pos_fix = []
     self.last_pos_residual = []
     self.last_pos_fix_t = None
+    self.use_qcom = use_qcom
 
   def load_cache(self):
     if not self.save_ephemeris:
@@ -105,17 +106,40 @@ class Laikad:
           cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}")
     return self.last_pos_fix
 
-  def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False):
+  def is_good_report(self, gnss_msg):
-    if ublox_msg.which == 'measurementReport':
+    if gnss_msg.which == 'drMeasurementReport' and self.use_qcom:
-      t = ublox_mono_time * 1e-9
+      constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
-      report = ublox_msg.measurementReport
+      # TODO support GLONASS
-      if report.gpsWeek > 0:
+      return constellation_id in [ConstellationId.GPS, ConstellationId.SBAS]
-        self.got_first_ublox_msg = True
+    elif gnss_msg.which == 'measurementReport' and not self.use_qcom:
-        latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow)
+      return True
+    else:
+      return False
+
+  def read_report(self, gnss_msg):
+    if self.use_qcom:
+      report = gnss_msg.drMeasurementReport
+      week = report.gpsWeek
+      tow = report.gpsMilliseconds / 1000.0
+      new_meas = read_raw_qcom(report)
+    else:
+      report = gnss_msg.measurementReport
+      week = report.gpsWeek
+      tow = report.rcvTow
+      new_meas = read_raw_ublox(report)
+    return week, tow, new_meas
+
+  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)
+
+      t = gnss_mono_time * 1e-9
+      if week > 0:
+        self.got_first_gnss_msg = True
+        latest_msg_t = GPSTime(week, tow)
         if self.auto_fetch_orbits:
           self.fetch_orbits(latest_msg_t, block)
 
-      new_meas = read_raw_ublox(report)
       # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
       new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
 
@@ -123,7 +147,7 @@ class Laikad:
       est_pos = self.get_est_pos(t, processed_measurements)
 
       corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else []
-      if ublox_mono_time % 10 == 0:
+      if gnss_mono_time % 10 == 0:
         cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
 
       self.update_localizer(est_pos, t, corrected_measurements)
@@ -139,20 +163,21 @@ class Laikad:
       dat = messaging.new_message("gnssMeasurements")
       measurement_msg = log.LiveLocationKalman.Measurement.new_message
       dat.gnssMeasurements = {
-        "gpsWeek": report.gpsWeek,
+        "gpsWeek": week,
-        "gpsTimeOfWeek": report.rcvTow,
+        "gpsTimeOfWeek": tow,
         "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid),
         "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid),
         "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
-        "ubloxMonoTime": ublox_mono_time,
+        "ubloxMonoTime": gnss_mono_time,
         "correctedMeasurements": meas_msgs
       }
       return dat
-    elif ublox_msg.which == 'ephemeris':
+    # TODO this only works on GLONASS, qcom needs live ephemeris parsing too
-      ephem = convert_ublox_ephem(ublox_msg.ephemeris)
+    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 ublox_msg.which == 'ionoData':
+    #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]):
@@ -303,24 +328,30 @@ class EphemerisSourceType(IntEnum):
 
 
 def main(sm=None, pm=None):
+  use_qcom = os.path.isfile("/persist/comma/use-quectel-rawgps")
+  if use_qcom:
+    raw_gnss_socket = "qcomGnss"
+  else:
+    raw_gnss_socket = "ubloxGnss"
+
   if sm is None:
-    sm = messaging.SubMaster(['ubloxGnss', 'clocks'])
+    sm = messaging.SubMaster([raw_gnss_socket, 'clocks'])
   if pm is None:
     pm = messaging.PubMaster(['gnssMeasurements'])
 
   replay = "REPLAY" in os.environ
   use_internet = "LAIKAD_NO_INTERNET" not in os.environ
-  laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet)
+  laikad = Laikad(save_ephemeris=not replay, auto_fetch_orbits=use_internet, use_qcom=use_qcom)
 
   while True:
     sm.update()
 
-    if sm.updated['ubloxGnss']:
+    if sm.updated[raw_gnss_socket]:
-      ublox_msg = sm['ubloxGnss']
+      gnss_msg = sm[raw_gnss_socket]
-      msg = laikad.process_ublox_msg(ublox_msg, sm.logMonoTime['ubloxGnss'], block=replay)
+      msg = laikad.process_gnss_msg(gnss_msg, sm.logMonoTime[raw_gnss_socket], block=replay)
       if msg is not None:
         pm.send('gnssMeasurements', msg)
-    if not laikad.got_first_ublox_msg and sm.updated['clocks']:
+    if not laikad.got_first_gnss_msg and sm.updated['clocks']:
       clocks_msg = sm['clocks']
       t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))
       if laikad.auto_fetch_orbits:

selfdrive/locationd/locationd.cc

@@ -443,6 +443,8 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   this->time_check(t);
   if (log.isSensorEvents()) {
     this->handle_sensors(t, log.getSensorEvents());
+  } else if (log.isGpsLocation()) {
+    this->handle_gps(t, log.getGpsLocation());
   } else if (log.isGpsLocationExternal()) {
     this->handle_gps(t, log.getGpsLocationExternal());
   } else if (log.isCarState()) {
@@ -490,11 +492,17 @@ void Localizer::determine_gps_mode(double current_time) {
 }
 
 int Localizer::locationd_thread() {
-  const std::initializer_list<const char *> service_list = {"gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
+  const char* gps_location_socket;
+  if (util::file_exists("/persist/comma/use-quectel-rawgps")) {
+    gps_location_socket = "gpsLocation";
+  } else {
+    gps_location_socket = "gpsLocationExternal";
+  }
+  const std::initializer_list<const char *> service_list = {gps_location_socket, "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
   PubMaster pm({"liveLocationKalman"});
 
   // TODO: remove carParams once we're always sending at 100Hz
-  SubMaster sm(service_list, {}, nullptr, {"gpsLocationExternal", "carParams"});
+  SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"});
 
   uint64_t cnt = 0;
   bool filterInitialized = false;

selfdrive/locationd/models/gnss_kf.py

@@ -39,12 +39,6 @@ class GNSSKalman():
                        1e14, (100)**2, (0.2)**2,
                        (10)**2, (1)**2])
 
-  # process noise
-  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
-               3**2, 3**2, 3**2,
-               (.1)**2, (0)**2, (0.005)**2,
-               .1**2, (.01)**2])
-
   maha_test_kinds: List[int] = []  # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS]
 
   @staticmethod
@@ -120,7 +114,14 @@ class GNSSKalman():
 
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds)
 
-  def __init__(self, generated_dir, cython=False):
+  def __init__(self, generated_dir, cython=False, erratic_clock=False):
+    # process noise
+    clock_error_drift = 100.0 if erratic_clock else 0.1
+    self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
+                      3**2, 3**2, 3**2,
+                      (clock_error_drift)**2, (0)**2, (0.005)**2,
+                      .1**2, (.01)**2])
+
     self.dim_state = self.x_initial.shape[0]
 
     # init filter

selfdrive/locationd/models/loc_kf.py

@@ -91,22 +91,6 @@ class LocKalman():
                        0.05**2, 0.05**2, 0.05**2,
                        0.01**2, 0.01**2, 0.01**2])
 
-  # process noise
-  Q = np.diag([0.03**2, 0.03**2, 0.03**2,
-               0.0**2, 0.0**2, 0.0**2,
-               0.0**2, 0.0**2, 0.0**2,
-               0.1**2, 0.1**2, 0.1**2,
-               (.1)**2, (0.0)**2,
-               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
-               (0.02 / 100)**2,
-               3**2, 3**2, 3**2,
-               0.001**2,
-               (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
-               (.1)**2, (.01)**2,
-               0.005**2,
-               (0.02 / 100)**2,
-               (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
-               (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
 
   # measurements that need to pass mahalanobis distance outlier rejector
   maha_test_kinds = [ObservationKind.ORB_FEATURES, ObservationKind.ORB_FEATURES_WIDE]  # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
@@ -345,9 +329,29 @@ class LocKalman():
       msckf_params = None
     gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, msckf_params, maha_test_kinds)
 
-  def __init__(self, generated_dir, N=4):
+  def __init__(self, generated_dir, N=4, erratic_clock=False):
     name = f"{self.name}_{N}"
 
+
+    # process noise
+    clock_error_drift = 100.0 if erratic_clock else 0.1
+    self.Q = np.diag([0.03**2, 0.03**2, 0.03**2,
+                      0.0**2, 0.0**2, 0.0**2,
+                      0.0**2, 0.0**2, 0.0**2,
+                      0.1**2, 0.1**2, 0.1**2,
+                      (clock_error_drift)**2, (0)**2,
+                      (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+                      (0.02 / 100)**2,
+                      3**2, 3**2, 3**2,
+                      0.001**2,
+                      (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
+                      (.1)**2, (.01)**2,
+                      0.005**2,
+                      (0.02 / 100)**2,
+                      (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
+                      (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
+
+
     self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
                       ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
                       ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),

selfdrive/locationd/test/test_laikad.py

@@ -28,7 +28,7 @@ def get_log(segs=range(0)):
 def verify_messages(lr, laikad, return_one_success=False):
   good_msgs = []
   for m in lr:
-    msg = laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True)
+    msg = laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
     if msg is not None and len(msg.gnssMeasurements.correctedMeasurements) > 0:
       good_msgs.append(msg)
       if return_one_success:
@@ -152,7 +152,7 @@ class TestLaikad(unittest.TestCase):
     self.assertFalse(all(laikad.kf_valid(m.logMonoTime * 1e-9)))
     kf_valid = False
     for m in self.logs:
-      laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=True)
+      laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=True)
       kf_valid = all(laikad.kf_valid(m.logMonoTime * 1e-9))
       if kf_valid:
         break
@@ -201,7 +201,7 @@ class TestLaikad(unittest.TestCase):
     laikad = Laikad(auto_update=False)
     has_orbits = False
     for m in self.logs:
-      laikad.process_ublox_msg(m.ubloxGnss, m.logMonoTime, block=False)
+      laikad.process_gnss_msg(m.ubloxGnss, m.logMonoTime, block=False)
       if laikad.orbit_fetch_future is not None:
         laikad.orbit_fetch_future.result()
       vals = laikad.astro_dog.orbits.values()