Laikad preperation (#26800)

* laikad update, renaming

* update locationd

* address PR comments

* draft to fix replay

* fix process relay to allow no response for messages

* bump cereal

* update process replay ref commit

* move laikad helpers to laika

* fix ublox test

* update refs

* add proper qcom replay support

* fix gnss support if both is available

* update refs

* move laika back to master

* move cereal back to master

Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>

release/files_common

@@ -233,7 +233,6 @@ selfdrive/locationd/generated/gps.cpp
 selfdrive/locationd/generated/gps.h
 
 selfdrive/locationd/laikad.py
-selfdrive/locationd/laikad_helpers.py
 selfdrive/locationd/locationd.h
 selfdrive/locationd/locationd.cc
 selfdrive/locationd/paramsd.py

selfdrive/locationd/laikad.py

@@ -20,7 +20,7 @@ from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse
 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
-from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun
+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
 from selfdrive.locationd.models.gnss_kf import GNSSKalman
 from selfdrive.locationd.models.gnss_kf import States as GStates
@@ -58,9 +58,9 @@ class Laikad:
     self.load_cache()
 
     self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)}
-    self.last_pos_fix = []
-    self.last_pos_residual = []
-    self.last_pos_fix_t = None
+    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
 
@@ -92,20 +92,23 @@ class Laikad:
       cloudlog.debug("Cache saved")
       self.last_cached_t = t
 
-  def get_est_pos(self, t, processed_measurements):
-    if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2:
-      min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 5
-      pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements)
-      if len(pos_fix) > 0:
-        self.last_pos_fix_t = t
-        residual_median = np.median(np.abs(pos_fix_residual))
-        if np.median(np.abs(pos_fix_residual)) < POS_FIX_RESIDUAL_THRESHOLD:
-          cloudlog.debug(f"Pos fix is within threshold with median: {residual_median.round()}")
-          self.last_pos_fix = pos_fix[:3]
-          self.last_pos_residual = pos_fix_residual
-        else:
-          cloudlog.debug(f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}")
-    return self.last_pos_fix
+  def get_lsq_fix(self, t, measurements):
+    if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
+      min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 5
+      position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
+
+      if len(position_solution) < 3:
+        return None
+      position_estimate = position_solution[:3]
+      #TODO median abs residual is decent estimate of std, can be improved with measurements stds and/or DOP
+      position_std = np.median(np.abs(pr_residuals)) * np.ones(3)
+      velocity_solution, prr_residuals = 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 = np.median(np.abs(prr_residuals)) * np.ones(3)
+      return position_estimate, position_std, velocity_estimate, velocity_std
 
   def is_good_report(self, gnss_msg):
     if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
@@ -148,11 +151,36 @@ class Laikad:
     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
+  def process_report(self, new_meas, t):
+    # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
+    new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
+    processed_measurements = process_measurements(new_meas, self.astro_dog)
 
+    instant_fix = self.get_lsq_fix(t, processed_measurements)
+    if instant_fix is None:
+      return None
+    else:
+      position_estimate, position_std, velocity_estimate, velocity_std = instant_fix
+      self.last_fix_t = t
+      self.last_fix_pos = position_estimate
+      self.lat_fix_pos_std = position_std
+
+    corrected_measurements = correct_measurements(processed_measurements, position_estimate, self.astro_dog)
+    if (t*1e9) % 10 == 0:
+      cloudlog.debug(f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}")
+    return position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, processed_measurements
+
+  def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
+    if self.is_ephemeris(gnss_msg):
+      self.read_ephemeris(gnss_msg)
+      return None
+    elif self.is_good_report(gnss_msg):
+
+      week, tow, new_meas = self.read_report(gnss_msg)
+      if len(new_meas) == 0:
+        return None
+
+      self.gps_week = week
       t = gnss_mono_time * 1e-9
       if week > 0:
         self.got_first_gnss_msg = True
@@ -160,44 +188,38 @@ class Laikad:
         if self.auto_fetch_orbits:
           self.fetch_orbits(latest_msg_t, block)
 
-      # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites
-      new_meas = [m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7]
-
-      processed_measurements = process_measurements(new_meas, self.astro_dog)
-      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 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)
-      kf_valid = all(self.kf_valid(t))
-      ecef_pos = self.gnss_kf.x[GStates.ECEF_POS]
-      ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY]
-
-      p = self.gnss_kf.P.diagonal()
-      pos_std = np.sqrt(p[GStates.ECEF_POS])
-      vel_std = np.sqrt(p[GStates.ECEF_VELOCITY])
+      output = self.process_report(new_meas, t)
+      if output is None:
+        return None
+      position_estimate, position_std, velocity_estimate, velocity_std, corrected_measurements, _ = output
 
+      self.update_localizer(position_estimate, t, corrected_measurements)
       meas_msgs = [create_measurement_msg(m) for m in corrected_measurements]
-      dat = messaging.new_message("gnssMeasurements")
+      msg = messaging.new_message("gnssMeasurements")
       measurement_msg = log.LiveLocationKalman.Measurement.new_message
-      dat.gnssMeasurements = {
+
+      P_diag = self.gnss_kf.P.diagonal()
+      kf_valid = all(self.kf_valid(t))
+      msg.gnssMeasurements = {
         "gpsWeek": week,
         "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),
-        # TODO std is incorrectly the dimension of the measurements and not 3D
-        "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
-        "ubloxMonoTime": gnss_mono_time,
+        "kalmanPositionECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_POS].tolist(),
+                                        std=np.sqrt(P_diag[GStates.ECEF_POS]).tolist(),
+                                        valid=kf_valid),
+        "kalmanVelocityECEF": measurement_msg(value=self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist(),
+                                        std=np.sqrt(P_diag[GStates.ECEF_VELOCITY]).tolist(),
+                                        valid=kf_valid),
+        "positionECEF": measurement_msg(value=position_estimate, std=position_std.tolist(), valid=bool(self.last_fix_t == t)),
+        "velocityECEF": measurement_msg(value=velocity_estimate, std=velocity_std.tolist(), valid=bool(self.last_fix_t == t)),
+
+        "measTime": gnss_mono_time,
         "correctedMeasurements": meas_msgs
       }
-      return dat
-    elif self.is_ephemeris(gnss_msg):
-      self.read_ephemeris(gnss_msg)
+      return msg
 
     #elif gnss_msg.which() == 'ionoData':
-    # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them.
+    # 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
@@ -349,9 +371,23 @@ class EphemerisSourceType(IntEnum):
   qcom = 3
 
 
-def main(sm=None, pm=None):
+def process_msg(laikad, gnss_msg, mono_time, block=False):
+  # 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']:
+      return None
+
+    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)
+      return None
+
+  return laikad.process_gnss_msg(gnss_msg, mono_time, block=block)
+
+
+def main(sm=None, pm=None, qc=None):
   use_qcom = not Params().get_bool("UbloxAvailable", block=True)
-  if use_qcom:
+  if use_qcom or (qc is not None and qc):
     raw_gnss_socket = "qcomGnss"
   else:
     raw_gnss_socket = "ubloxGnss"
@@ -371,19 +407,13 @@ 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
+      msg = process_msg(laikad, gnss_msg, sm.logMonoTime[raw_gnss_socket], replay)
+      if msg is None:
+        msg = messaging.new_message("gnssMeasurements")
+        msg.valid = False
 
-        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
+      pm.send('gnssMeasurements', msg)
 
-      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_gnss_msg and sm.updated['clocks']:
       clocks_msg = sm['clocks']
       t = GPSTime.from_datetime(datetime.utcfromtimestamp(clocks_msg.wallTimeNanos * 1E-9))

selfdrive/locationd/laikad_helpers.py

@@ -1,89 +0,0 @@
-import numpy as np
-import sympy
-
-from laika.constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT
-from laika.helpers import ConstellationId
-
-
-def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6):
-  '''
-  Calculates gps fix using gauss newton method
-  To solve the problem a minimal of 4 measurements are required.
-    If Glonass is included 5 are required to solve for the additional free variable.
-  returns:
-  0 -> list with positions
-  '''
-  if x0 is None:
-    x0 = [0, 0, 0, 0, 0]
-  n = len(measurements)
-  if n < min_measurements:
-    return [], []
-
-  Fx_pos = pr_residual(measurements, posfix_functions, signal=signal)
-  x = gauss_newton(Fx_pos, x0)
-  residual, _ = Fx_pos(x, weight=1.0)
-  return x.tolist(), residual.tolist()
-
-
-def pr_residual(measurements, posfix_functions, signal='C1C'):
-  def Fx_pos(inp, weight=None):
-    vals, gradients = [], []
-
-    for meas in measurements:
-      pr = meas.observables[signal]
-      pr += meas.sat_clock_err * SPEED_OF_LIGHT
-
-      w = (1 / meas.observables_std[signal]) if weight is None else weight
-
-      val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w)
-      vals.append(val)
-      gradients.append(gradient)
-    return np.asarray(vals), np.asarray(gradients)
-
-  return Fx_pos
-
-
-def gauss_newton(fun, b, xtol=1e-8, max_n=25):
-  for _ in range(max_n):
-    # Compute function and jacobian on current estimate
-    r, J = fun(b)
-
-    # Update estimate
-    delta = np.linalg.pinv(J) @ r
-    b -= delta
-
-    # Check step size for stopping condition
-    if np.linalg.norm(delta) < xtol:
-      break
-  return b
-
-
-def get_posfix_sympy_fun(constellation):
-  # Unknowns
-  x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
-  bc = sympy.Symbol('bc')
-  bg = sympy.Symbol('bg')
-  var = [x, y, z, bc, bg]
-
-  # Knowns
-  pr = sympy.Symbol('pr')
-  sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
-  weight = sympy.Symbol('weight')
-
-  theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT
-  val = sympy.sqrt(
-    (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
-    (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
-    (sat_z - z) ** 2
-  )
-
-  if constellation == ConstellationId.GLONASS:
-    res = weight * (val - (pr - bc - bg))
-  elif constellation == ConstellationId.GPS:
-    res = weight * (val - (pr - bc))
-  else:
-    raise NotImplementedError(f"Constellation {constellation} not supported")
-
-  res = [res] + [sympy.diff(res, v) for v in var]
-
-  return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res, modules=["numpy"])

selfdrive/locationd/locationd.cc

@@ -25,8 +25,15 @@ const double MAX_FILTER_REWIND_TIME = 0.8; // s
 
 // TODO: GPS sensor time offsets are empirically calculated
 // They should be replaced with synced time from a real clock
-const double GPS_LOCATION_SENSOR_TIME_OFFSET = 0.630; // s
-const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095; // s
+const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
+const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
+const float  GPS_MUL_FACTOR = 10.0;
+const float  GPS_POS_STD_THRESHOLD = 50.0;
+const float  GPS_VEL_STD_THRESHOLD = 5.0;
+const float  GPS_POS_ERROR_RESET_THRESHOLD = 200.0;
+const float  GPS_POS_STD_RESET_THRESHOLD = 5.0;
+const float  GPS_VEL_STD_RESET_THRESHOLD = 0.5;
+const float  GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 5.0;
 
 static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
   VectorXd res(floatlist.size());
@@ -316,8 +323,8 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
   VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
-  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getAccuracy(),2) + std::pow(GPS_MUL_FACTOR * log.getVerticalAccuracy(),2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(GPS_MUL_FACTOR * log.getSpeedAccuracy(), 2)).asDiagonal();
 
   this->unix_timestamp_millis = log.getUnixTimestampMillis();
   double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@@ -348,6 +355,77 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
   this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
 }
 
+void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
+
+  this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid();
+  if (!this->gps_valid) {
+    this->determine_gps_mode(current_time);
+    return;
+  }
+  this->gps_mode = true;
+
+  double sensor_time = log.getMeasTime() * 1e-9;
+  if (ublox_available)
+    sensor_time -= GPS_UBLOX_SENSOR_TIME_OFFSET;
+  else
+    sensor_time -= GPS_QUECTEL_SENSOR_TIME_OFFSET;
+
+  auto ecef_pos_v = log.getPositionECEF().getValue();
+  VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_pos_std = log.getPositionECEF().getStd()[0];
+  MatrixXdr ecef_pos_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_pos_std, 2)).asDiagonal();
+
+  auto ecef_vel_v = log.getVelocityECEF().getValue();
+  VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
+
+  // indexed at 0 cause all std values are the same MAE
+  auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
+  MatrixXdr ecef_vel_R = Vector3d::Constant(pow(GPS_MUL_FACTOR*ecef_vel_std, 2)).asDiagonal();
+
+  double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
+
+  VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
+  VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
+
+  LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
+  ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
+  VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
+  double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
+
+  VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
+  VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
+  for (int i = 0; i < orientation_error.size(); i++) {
+    orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
+    if (orientation_error(i) < 0.0) {
+      orientation_error(i) += 2.0 * M_PI;
+    }
+    orientation_error(i) -= M_PI;
+  }
+  VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
+
+  if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
+    this->gps_valid = false;
+    this->determine_gps_mode(current_time);
+    return;
+  }
+
+  if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
+    // always reset on first gps message and if the location is off but the accuracy is high
+    LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+  } else if (ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD && orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD) {
+    LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
+    this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
+    this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
+  }
+
+  this->last_gps_msg = current_time;
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+  this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
 void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
   this->car_speed = std::abs(log.getVEgo());
   if (log.getStandstill()) {
@@ -497,9 +575,11 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
   } else if (log.isGyroscope()) {
     this->handle_sensor(t, log.getGyroscope());
   } else if (log.isGpsLocation()) {
-    this->handle_gps(t, log.getGpsLocation(), GPS_LOCATION_SENSOR_TIME_OFFSET);
+    this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
   } else if (log.isGpsLocationExternal()) {
-    this->handle_gps(t, log.getGpsLocationExternal(), GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET);
+    this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+  //} else if (log.isGnssMeasurements()) {
+  //  this->handle_gnss(t, log.getGnssMeasurements());
   } else if (log.isCarState()) {
     this->handle_car_state(t, log.getCarState());
   } else if (log.isCameraOdometry()) {

selfdrive/locationd/locationd.h

@@ -52,6 +52,7 @@ public:
   void handle_msg(const cereal::Event::Reader& log);
   void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
   void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
+  void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
   void handle_car_state(double current_time, const cereal::CarState::Reader& log);
   void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
   void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
@@ -77,6 +78,7 @@ private:
   bool device_fell = false;
   bool gps_mode = false;
   bool gps_valid = false;
+  double last_gps_msg = 0;
   bool ublox_available = true;
   bool observation_timings_invalid = false;
   std::map<std::string, double> observation_values_invalid;  

selfdrive/locationd/test/test_ublox_processing.py

@@ -4,7 +4,8 @@ import numpy as np
 
 from laika import AstroDog
 from laika.helpers import ConstellationId
-from laika.raw_gnss import calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox
+from laika.raw_gnss import correct_measurements, process_measurements, read_raw_ublox
+from laika.opt import calc_pos_fix
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader
 
@@ -54,14 +55,14 @@ class TestUbloxProcessing(unittest.TestCase):
       processed_meas = process_measurements(measurements, dog)
       count_processed_measurements += len(processed_meas)
       pos_fix = calc_pos_fix(processed_meas)
-      if len(pos_fix) > 0 and all(pos_fix[0] != 0):
+      if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
         position_fix_found += 1
 
         corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog)
         count_corrected_measurements += len(corrected_meas)
 
         pos_fix = calc_pos_fix(corrected_meas)
-        if len(pos_fix) > 0 and all(pos_fix[0] != 0):
+        if len(pos_fix) > 0 and all(p != 0 for p in pos_fix[0]):
           pos_ests.append(pos_fix[0])
           position_fix_found_after_correcting += 1
 

selfdrive/test/process_replay/process_replay.py

@@ -251,8 +251,10 @@ def ublox_rcv_callback(msg):
 def laika_rcv_callback(msg, CP, cfg, fsm):
   if msg.which() == 'ubloxGnss' and msg.ubloxGnss.which() == "measurementReport":
     return ["gnssMeasurements"], True
+  elif msg.which() == 'qcomGnss' and msg.qcomGnss.which() == "drMeasurementReport":
+    return ["gnssMeasurements"], True
   else:
-    return [], True
+    return [], False
 
 
 CONFIGS = [
@@ -360,24 +362,11 @@ CONFIGS = [
     tolerance=None,
     fake_pubsubmaster=False,
   ),
-  ProcessConfig(
-    proc_name="laikad",
-    subtest_name="Offline",
-    pub_sub={
-      "ubloxGnss": ["gnssMeasurements"],
-      "clocks": []
-    },
-    ignore=["logMonoTime"],
-    init_callback=get_car_params,
-    should_recv_callback=laika_rcv_callback,
-    tolerance=NUMPY_TOLERANCE,
-    fake_pubsubmaster=True,
-    environ={"LAIKAD_NO_INTERNET": "1"},
-  ),
   ProcessConfig(
     proc_name="laikad",
     pub_sub={
       "ubloxGnss": ["gnssMeasurements"],
+      "qcomGnss": ["gnssMeasurements"],
       "clocks": []
     },
     ignore=["logMonoTime"],
@@ -474,6 +463,12 @@ def python_replay_process(cfg, lr, fingerprint=None):
   all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
   pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
 
+  # laikad needs decision between submaster ubloxGnss and qcomGnss, prio given to ubloxGnss
+  if cfg.proc_name == "laikad":
+    args = (*args, not any(m.which() == "ubloxGnss" for m in pub_msgs))
+    service = "qcomGnss" if args[2] else "ubloxGnss"
+    pub_msgs = [m for m in pub_msgs if m.which() == service or m.which() == 'clocks']
+
   controlsState = None
   initialized = False
   for msg in lr:

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-05ba201bcf97b6c1067dbcde2a60f71f43469c56
+557ffbf5a1c9cafba1ff8d6f3e2642df98b2d6e6