Implement TripleDipper GPS fallback

common/gps.py

@@ -1,8 +1,25 @@
+from __future__ import annotations
+
+from cereal import car
 from openpilot.common.params import Params
 
 
-def get_gps_location_service(params: Params) -> str:
-  if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
+def gm_car_params_present(params: Params, CP: car.CarParams | None = None) -> bool:
+  if CP is not None and getattr(CP, "brand", None):
+    return CP.brand == "gm"
+  try:
+    raw_car_params = params.get("CarParams")
+    if raw_car_params is None:
+      return False
+    with car.CarParams.from_bytes(raw_car_params) as parsed_cp:
+      return parsed_cp.brand == "gm"
+  except Exception:
+    return False
+
+
+def get_gps_location_service(params: Params, CP: car.CarParams | None = None) -> str:
+  # GM arbitrates device/PPS/OnStar through gpsLocationExternal.
+  if gm_car_params_present(params, CP) or params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"):
     return "gpsLocationExternal"
   else:
     return "gpsLocation"

opendbc_repo/opendbc/car/gm/carstate.py

@@ -1,5 +1,7 @@
 import copy
 import math
+from datetime import UTC, datetime, timedelta
+from collections.abc import Mapping
 from cereal import custom
 from opendbc.can import CANDefine, CANParser
 from opendbc.car import Bus, create_button_events, structs
@@ -8,12 +10,10 @@ from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.gps import get_car_gps_config
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.gm.values import (
-  ALT_ACCS,
   ASCM_INT,
   CAMERA_ACC_CAR,
   CAR,
   CC_ONLY_CAR,
-  CC_REGEN_PADDLE_CAR,
   DBC,
   AccState,
   CanBus,
@@ -38,6 +38,112 @@ BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.D
 HARD_BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise}
 NORMAL_CRUISE_BUTTONS = (CruiseButtons.RES_ACCEL, CruiseButtons.DECEL_SET)
 
+# Optional ~10 Hz CT6 PPS GPS messages on the powertrain bus.
+PPS_GPS_MESSAGES = (
+  "PPS_ElevHdSpd_FO",
+  "PPS_PosLat_FO",
+  "PPS_PosLong_FO",
+  "PPS_Time_FO",
+  "PPS_QualMetrics_FO",
+)
+# PPS_SigAcqTime_FO is omitted: its validity bit stays 1 even during valid fixes.
+
+
+def pps_checksum_ok(data: bytes) -> bool:
+  """Validate the 11-bit checksum used by the observed PPS frames."""
+  if len(data) < 2:
+    return False
+  received = ((data[-2] & 0x07) << 8) | data[-1]
+  expected = sum(data[:-2]) + (data[-2] >> 3) + 0x4C
+  return (expected & 0x7FF) == received
+
+
+def decode_gm_pps_gps(values: Mapping[str, Mapping[str, float]], raw: Mapping[str, bytes],
+                      timestamp_nanos: int) -> dict | None:
+  """Decode one coherent PPS bundle into the existing car-GPS sample shape."""
+  if any(not pps_checksum_ok(raw.get(name, b"")) for name in PPS_GPS_MESSAGES):
+    return None
+
+  try:
+    pos_lat = values["PPS_PosLat_FO"]
+    pos_long = values["PPS_PosLong_FO"]
+    timestamp_values = values["PPS_Time_FO"]
+    quality = values["PPS_QualMetrics_FO"]
+    if (int(pos_lat.get("PPSLatV", 1)) != 0 or
+        int(pos_long.get("PPSLongV", 1)) != 0 or
+        int(quality.get("PPS2DAbsPosErrEstmtV", 1)) != 0 or
+        int(quality.get("PPSMdV", 1)) != 0 or
+        int(quality.get("PPSPstnDilPrcsV", 1)) != 0 or
+        int(timestamp_values.get("PPSTmdayV", 1)) != 0 or
+        int(timestamp_values.get("PPSCldrDayV", 1)) != 0 or
+        int(timestamp_values.get("PPSCldrYrV", 1)) != 0):
+      return None
+    # Reject mode 6 (dead reckoning only without GNSS).
+    if int(quality["PPSMd"]) == 6:
+      return None
+
+    latitude = float(pos_lat["PPSLat"]) / 3_600_000.0
+    longitude = float(pos_long["PPSLong"]) / 3_600_000.0
+    if not (math.isfinite(latitude) and math.isfinite(longitude) and
+            -90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and
+            (latitude != 0.0 or longitude != 0.0)):
+      return None
+
+    year = int(timestamp_values["PPSCldrYr"])
+    day_of_year = int(timestamp_values["PPSCldrDay"])
+    millis_of_day = int(timestamp_values["PPSTmday"])
+    if not 2014 <= year <= 2141 or day_of_year < 1 or not 0 <= millis_of_day < 86_400_000:
+      return None
+    timestamp = datetime(year, 1, 1, tzinfo=UTC) + timedelta(days=day_of_year - 1, milliseconds=millis_of_day)
+    if timestamp.year != year:
+      return None
+
+    elev = values["PPS_ElevHdSpd_FO"]
+    speed = float(elev["PPSVel"]) * CV.KPH_TO_MS
+    if int(elev.get("PPSVelV", 1)) != 0 or not math.isfinite(speed) or not 0.0 <= speed <= 200.0:
+      speed = 0.0
+    heading = float(elev["PPSHedng"])
+    if (int(elev.get("PPSHedngV", 1)) != 0 or
+        not math.isfinite(heading) or not 0.0 <= heading < 360.0):
+      heading = 0.0
+
+    altitude = float(elev["PPSElvtn"]) / 100.0
+    if int(elev.get("PPSElvtnV", 1)) != 0 or not math.isfinite(altitude):
+      altitude = 0.0
+
+    horizontal_accuracy = float(quality["PPS2DAbsPosErrEstmt"])
+    if not math.isfinite(horizontal_accuracy) or horizontal_accuracy < 0.0:
+      horizontal_accuracy = 0.0
+
+    vertical_accuracy = float(quality["PPS3DAbsPosErrEstmt"])
+    if int(quality.get("PPS3DAbsPosErrEstmtV", 1)) != 0 or not math.isfinite(vertical_accuracy) or vertical_accuracy < 0.0:
+      vertical_accuracy = 0.0
+
+    bearing_accuracy = float(quality["PPSAbsHdngErrEstmt"])
+    if int(quality.get("PPSAbsHdngErrEstmtV", 1)) != 0 or not math.isfinite(bearing_accuracy) or bearing_accuracy < 0.0:
+      bearing_accuracy = 180.0
+  except (KeyError, TypeError, ValueError, OverflowError, AttributeError):
+    return None
+
+  heading_rad = math.radians(heading)
+  return {
+    "timestamp_nanos": timestamp_nanos,
+    "latitude": latitude,
+    "longitude": longitude,
+    "altitude": altitude,
+    "speed": speed,
+    "bearingDeg": heading,
+    "horizontalAccuracy": horizontal_accuracy,
+    "unixTimestampMillis": round(timestamp.timestamp() * 1000),
+    "verticalAccuracy": vertical_accuracy,
+    "bearingAccuracyDeg": bearing_accuracy,
+    # Velocity error units are undocumented in DBC; omit conversion.
+    "speedAccuracy": 0.0,
+    "hasFix": True,
+    "satelliteCount": 0,
+    "vNED": [speed * math.cos(heading_rad), speed * math.sin(heading_rad), 0.0],
+  }
+
 
 def get_hard_cruise_buttons(steering_button_msg: dict) -> int:
   return steering_button_msg.get("ACCButtonsHard", CruiseButtons.INIT)
@@ -109,11 +215,15 @@ class CarState(CarStateBase):
     self.car_gps_config = get_car_gps_config(CP)
     self.car_gps_supported = self.car_gps_config is not None
     self.car_gps = None
+    self.onstar_gps = None
     self._car_gps_timestamp_nanos = 0
     self._prev_gps_lat = None
     self._prev_gps_lon = None
     self._last_gps_bearing = None
 
+    self.pps_gps = None
+    self._pps_gps_timestamp_nanos = 0
+
   def _update_car_gps(self, cp, v_ego: float = 0.0) -> None:
     if self.car_gps_config is None:
       return
@@ -148,12 +258,47 @@ class CarState(CarStateBase):
       else:
         self._prev_gps_lat = self._prev_gps_lon = None
 
+      self.onstar_gps = gps
       self.car_gps = gps
       self._car_gps_timestamp_nanos = timestamp_nanos
 
-  def get_car_gps(self):
+  def _update_pps_gps(self, cp) -> None:
+    """Decode a complete, checksum-valid PPS burst when one is available."""
+    timestamps = [max(cp.ts_nanos[name].values(), default=0) for name in PPS_GPS_MESSAGES]
+    if not all(timestamps):
+      return
+
+    timestamp_nanos = max(timestamps)
+    if timestamp_nanos <= self._pps_gps_timestamp_nanos:
+      return
+    if timestamp_nanos - min(timestamps) > 100_000_000:
+      return
+
+    vl = cp.vl
+    try:
+      first_id = int(vl["PPS_ElevHdSpd_FO"]["PPSElvHedngSpdBrstID"])
+      if not (first_id == int(vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ==
+              int(vl["PPS_PosLong_FO"]["PPSLongBrstID"]) ==
+              int(vl["PPS_Time_FO"]["PPSTmBrstID"]) ==
+              int(vl["PPS_QualMetrics_FO"]["PPSPosQltyMtcBrstID"])):
+        return
+    except (KeyError, ValueError, TypeError, OverflowError):
+      return
+
+    values = {name: cp.vl[name] for name in PPS_GPS_MESSAGES}
+    raw = {name: cp.vl_raw[name] for name in PPS_GPS_MESSAGES}
+    self.pps_gps = decode_gm_pps_gps(values, raw, timestamp_nanos)
+    self._pps_gps_timestamp_nanos = timestamp_nanos
+
+  def get_car_gps(self) -> dict | None:
     return self.car_gps
 
+  def get_car_gps_sources(self) -> dict[str, dict | None]:
+    return {
+      "pps": self.pps_gps,
+      "onstar": self.onstar_gps,
+    }
+
   def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
     if not self.CP.pcmCruise:
       for b in buttonEvents:
@@ -239,6 +384,9 @@ class CarState(CarStateBase):
                      abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
 
     self._update_car_gps(pt_cp, ret.vEgo)
+    pps_cp = can_parsers.get(Bus.adas)
+    if pps_cp is not None:
+      self._update_pps_gps(pps_cp)
 
     if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
       ret.gearShifter = self.parse_gear_shifter("T")
@@ -588,8 +736,16 @@ class CarState(CarStateBase):
       ("ASCMLKASteeringCmd", 0),
     ]
 
-    return {
+    parsers = {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus.POWERTRAIN),
       Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus.CAMERA),
       Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, CanBus.LOOPBACK),
     }
+    if getattr(CP, "brand", None) == "gm":
+      # Optional CT6 PPS parser on Bus.adas; non-PPS vehicles remain CAN-valid.
+      parsers[Bus.adas] = CANParser(
+        "cadillac_ct6_object",
+        [(name, 0) for name in PPS_GPS_MESSAGES],
+        CanBus.POWERTRAIN,
+      )
+    return parsers

opendbc_repo/opendbc/car/gm/tests/test_gm.py

@@ -1,5 +1,6 @@
 import pytest
 import numpy as np
+from datetime import UTC, datetime
 from types import SimpleNamespace
 from parameterized import parameterized
 
@@ -8,7 +9,14 @@ from opendbc.can import CANPacker, CANParser
 from opendbc.car import Bus, DT_CTRL, structs
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.gm import gmcan
-from opendbc.car.gm.carstate import CarState as GMCarState, get_hard_cruise_buttons, update_auto_hold_drive_timers
+from opendbc.car.gm.carstate import (
+  CarState as GMCarState,
+  PPS_GPS_MESSAGES,
+  decode_gm_pps_gps,
+  get_hard_cruise_buttons,
+  pps_checksum_ok,
+  update_auto_hold_drive_timers,
+)
 from opendbc.car.gm.carcontroller import (
   VisualAlert,
   get_acc_dashboard_always_one,
@@ -95,6 +103,146 @@ class TestBoltGps:
     assert gps["verticalAccuracy"] == 10.0
     assert gps["speedAccuracy"] == 0.5
 
+
+class TestPpsGps:
+  _frames = [
+    (0x260, bytes.fromhex("10ddac000d831277"), 0),
+    (0x261, bytes.fromhex("08386fce09ca"), 0),
+    (0x262, bytes.fromhex("6d98820341de"), 0),
+    (0x264, bytes.fromhex("0018f90578b5eaac"), 0),
+    (0x265, bytes.fromhex("1a0000800a0258fd"), 0),
+  ]
+
+  def test_observed_bundle_checksum_and_conversion(self):
+    parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
+    parser.update([(1_000_000_000, self._frames)])
+
+    assert all(pps_checksum_ok(parser.vl_raw[name]) for name in PPS_GPS_MESSAGES)
+    gps = decode_gm_pps_gps(
+      {name: parser.vl[name] for name in PPS_GPS_MESSAGES},
+      {name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES},
+      1_000_000_000,
+    )
+    assert gps is not None
+    assert gps["hasFix"]
+    assert gps["latitude"] == pytest.approx(38.3101, abs=1e-4)
+    assert gps["longitude"] == pytest.approx(-85.7701, abs=1e-4)
+    assert gps["altitude"] == pytest.approx(106.9)
+    assert gps["bearingDeg"] == pytest.approx(56.748)
+    assert gps["horizontalAccuracy"] == pytest.approx(1.0)
+    assert gps["unixTimestampMillis"] == 1788655668655
+
+  @pytest.fixture
+  def bundle(self):
+    parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
+    parser.update([(1_000_000_000, self._frames)])
+    return ({name: dict(parser.vl[name]) for name in PPS_GPS_MESSAGES},
+            {name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES})
+
+  @pytest.mark.parametrize("year,day,date", [
+    (2025, 1, "2025-01-01"), (2025, 365, "2025-12-31"), (2025, 366, None),
+    (2024, 366, "2024-12-31"), (2024, 367, None), (2025, 0, None),
+  ])
+  def test_one_based_day_of_year(self, bundle, year, day, date):
+    values, raw = bundle
+    values["PPS_Time_FO"].update(PPSCldrYr=year, PPSCldrDay=day, PPSTmday=1234)
+    gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
+    if date is None:
+      assert gps is None
+    else:
+      expected = int(datetime.fromisoformat(date).replace(tzinfo=UTC).timestamp() * 1000) + 1234
+      assert gps["unixTimestampMillis"] == expected
+
+  @pytest.mark.parametrize("bad_data", [b"", b"\x01"])
+  def test_checksum_short_input(self, bad_data):
+    assert not pps_checksum_ok(bad_data)
+
+  @pytest.mark.parametrize("lat,lon,valid", [(0.0, 10.0 * 3_600_000, True), (10.0 * 3_600_000, 0.0, True), (0.0, 0.0, False)])
+  def test_coordinate_axes(self, bundle, lat, lon, valid):
+    values, raw = bundle
+    values["PPS_PosLat_FO"]["PPSLat"] = lat
+    values["PPS_PosLong_FO"]["PPSLong"] = lon
+    gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
+    if valid:
+      assert gps is not None
+      assert gps["hasFix"]
+    else:
+      assert gps is None
+
+  def test_get_car_gps_sources_shape(self):
+    cs = GMCarState.__new__(GMCarState)
+    cs.pps_gps = {"hasFix": True}
+    cs.onstar_gps = None
+    sources = cs.get_car_gps_sources()
+    assert sources == {"pps": {"hasFix": True}, "onstar": None}
+
+  @pytest.mark.parametrize("message,signal,value", [
+    ("PPS_PosLat_FO", "PPSLatV", 1),
+    ("PPS_PosLong_FO", "PPSLongV", 1),
+    ("PPS_QualMetrics_FO", "PPS2DAbsPosErrEstmtV", 1),
+    ("PPS_PosLat_FO", "PPSLat", float("nan")),
+    ("PPS_PosLong_FO", "PPSLong", 181 * 3_600_000),
+    ("PPS_QualMetrics_FO", "PPSMd", 6),
+    ("PPS_Time_FO", "PPSTmdayV", 1),
+  ])
+  def test_unusable_position_rejected(self, bundle, message, signal, value):
+    values, raw = bundle
+    values[message][signal] = value
+    assert decode_gm_pps_gps(values, raw, 1_000_000_000) is None
+
+  @pytest.mark.parametrize("invalidity", ["checksum", "position-validity"])
+  def test_burst_cache_and_explicit_invalidation(self, invalidity):
+    parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0)
+    cs = GMCarState.__new__(GMCarState)
+    cs.pps_gps = None
+    cs._pps_gps_timestamp_nanos = 0
+    parser.update([(1_000_000_000, self._frames[:-1])])
+    cs._update_pps_gps(parser)
+    assert cs.pps_gps is None  # Incomplete startup burst.
+    parser.update([(1_000_000_000, self._frames[-1:])])
+    cs._update_pps_gps(parser)
+    good = cs.pps_gps
+    assert good is not None
+    # No complete new burst: keep its original timestamp for freshness.
+    parser.update([(2_000_000_000, self._frames[:1])])
+    cs._update_pps_gps(parser)
+    assert cs.pps_gps is good
+    parser.update([(2_100_000_000, self._frames)])
+    parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"] = int(parser.vl["PPS_PosLat_FO"]["PPSLatBrstID"]) ^ 1
+    cs._update_pps_gps(parser)
+    assert cs.pps_gps is good  # A mismatched burst must not refresh the fix.
+    bad_frames = ([(addr, data[:-1] + bytes([data[-1] ^ 1]), bus) for addr, data, bus in self._frames]
+                  if invalidity == "checksum" else self._frames)
+    parser.update([(3_000_000_000, bad_frames)])
+    if invalidity == "position-validity":
+      parser.vl["PPS_PosLat_FO"]["PPSLatV"] = 1
+    cs._update_pps_gps(parser)
+    assert cs.pps_gps is None
+    parser.update([(4_000_000_000, self._frames)])
+    cs._update_pps_gps(parser)
+    assert cs.pps_gps is not None
+
+  @pytest.mark.parametrize("speed_bad,heading_bad,elevation_bad,vertical_bad,bearing_bad", [
+    (0, 0, 0, 0, 0), (1, 0, 0, 0, 0), (0, 1, 0, 0, 0), (0, 0, 1, 0, 0),
+    (0, 0, 0, 1, 0), (0, 0, 0, 0, 1), (1, 1, 1, 1, 1),
+  ], ids=["valid", "speed", "heading", "elevation", "vertical-accuracy", "bearing-accuracy", "all-invalid"])
+  def test_optional_field_fallbacks(self, bundle, speed_bad, heading_bad, elevation_bad, vertical_bad, bearing_bad):
+    values, raw = bundle
+    values["PPS_ElevHdSpd_FO"].update(PPSVel=36, PPSVelV=speed_bad, PPSHedng=90, PPSHedngV=heading_bad,
+                                     PPSElvtn=12345, PPSElvtnV=elevation_bad)
+    values["PPS_QualMetrics_FO"].update(PPS2DAbsPosErrEstmt=3.2, PPS3DAbsPosErrEstmt=4.5, PPS3DAbsPosErrEstmtV=vertical_bad,
+                                        PPSAbsHdngErrEstmt=6.0, PPSAbsHdngErrEstmtV=bearing_bad)
+    gps = decode_gm_pps_gps(values, raw, 1_000_000_000)
+    assert gps is not None
+    assert gps["hasFix"]
+    assert gps["speed"] == pytest.approx(0.0 if speed_bad else 10.0)
+    assert gps["bearingDeg"] == (0 if heading_bad else 90)
+    assert gps["altitude"] == pytest.approx(0.0 if elevation_bad else 123.45)
+    assert gps["vNED"] == pytest.approx([gps["speed"], 0, 0] if heading_bad else [0, gps["speed"], 0])
+    assert gps["horizontalAccuracy"] == pytest.approx(3.2)
+    assert gps["verticalAccuracy"] == pytest.approx(0.0 if vertical_bad else 4.5)
+    assert gps["bearingAccuracyDeg"] == pytest.approx(180.0 if bearing_bad else 6.0)
+
   def test_bolt_gps_heading_and_speed_derivation(self):
     cp = SimpleNamespace(
       brand="gm",

opendbc_repo/opendbc/car/gps.py

@@ -6,8 +6,10 @@ from collections.abc import Callable, Mapping
 from typing import Any
 
 from opendbc.car.common.conversions import Conversions as CV
+from opendbc.can.dbc import DBC as DBC_FILE
+from opendbc.car import Bus
 from opendbc.car.ford.values import CAR as FORD_CAR
-from opendbc.car.gm.values import CAR as GM_CAR
+from opendbc.car.gm.values import CAR as GM_CAR, DBC as GM_DBC
 
 
 CarGpsSample = dict[str, Any]
@@ -158,8 +160,25 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = {
 
 
 def get_car_gps_config(CP) -> CarGpsConfig | None:
+  cp_brand = getattr(CP, "brand", None)
   config = CAR_GPS_CONFIGS.get(CP.carFingerprint)
-  return config if config is not None and config.brand == CP.brand else None
+  if config is not None and config.brand == cp_brand:
+    return config
+
+  # Enable OnStar GPS for GM cars whose powertrain DBC defines it.
+  if cp_brand == "gm":
+    try:
+      dbc_name = GM_DBC[CP.carFingerprint][Bus.pt]
+      if "TCICOnStarGPSPosition" in DBC_FILE(dbc_name).name_to_msg:
+        return CarGpsConfig(
+          brand="gm",
+          messages=CHEVROLET_BOLT_GPS_MESSAGES,
+          decoder=parse_chevrolet_bolt_can_gps,
+        )
+    except (KeyError, OSError, TypeError, RuntimeError):
+      pass
+
+  return None
 
 
 def car_gps_available(CP) -> bool:

scripts/host_tool_runner.sh

@@ -284,14 +284,27 @@ ensure_host_python_extensions() {
 }
 
 sync_host_generated_headers() {
-  if ! command -v capnpc >/dev/null 2>&1; then
+  local capnp_bin=""
+  local candidate=""
+  for candidate in "${HOST_VENV}"/lib/python*/site-packages/capnproto/install/bin; do
+    if [[ -x "${candidate}/capnpc" ]]; then
+      capnp_bin="${candidate}"
+      break
+    fi
+  done
+
+  local capnpc_cmd="capnpc"
+  if [[ -n "${capnp_bin}" ]]; then
+    capnpc_cmd="${capnp_bin}/capnpc"
+    export PATH="${capnp_bin}:${PATH}"
+  elif ! command -v capnpc >/dev/null 2>&1; then
     return
   fi
 
   (
     cd "${WORK_DIR}"
     mkdir -p cereal/gen/cpp
-    capnpc --src-prefix=cereal \
+    "${capnpc_cmd}" --src-prefix=cereal \
       cereal/log.capnp \
       cereal/car.capnp \
       cereal/legacy.capnp \

selfdrive/car/card.py

@@ -43,6 +43,8 @@ REDNECK_DECREASE_LOOKAHEAD_POINTS = 10
 SLC_SOURCE_NONE = "None"
 EventName = log.OnroadEvent.EventName
 
+GM_GPS_SOURCES = (("device", 2.0), ("pps", 1.0), ("onstar", 2.5))
+
 # forward
 carlog.addHandler(ForwardingHandler(cloudlog))
 
@@ -76,6 +78,22 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket
   return can_recv, can_send
 
 
+def _gps_sample_is_healthy(sample: dict) -> bool:
+  if not sample.get("hasFix", False):
+    return False
+  try:
+    latitude = float(sample["latitude"])
+    longitude = float(sample["longitude"])
+    return (math.isfinite(latitude) and math.isfinite(longitude) and
+            -90.0 <= latitude <= 90.0 and -180.0 <= longitude <= 180.0 and
+            (latitude != 0.0 or longitude != 0.0) and
+            math.isfinite(float(sample["altitude"])) and
+            math.isfinite(float(sample["speed"])) and
+            math.isfinite(float(sample["bearingDeg"])))
+  except (KeyError, TypeError, ValueError):
+    return False
+
+
 class Car:
   CI: CarInterfaceBase
   RI: RadarInterfaceBase
@@ -85,7 +103,9 @@ class Car:
 
   def __init__(self, CI=None, RI=None) -> None:
     self.can_sock = messaging.sub_sock('can', timeout=20)
-    self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan'])
+    self.sm = messaging.SubMaster([
+      'pandaStates', 'carControl', 'onroadEvents', 'radarState', 'longitudinalPlan', 'gpsLocation',
+    ])
     self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput', 'liveTracks'])
     self.gps_pm = None
 
@@ -93,6 +113,8 @@ class Car:
     self._last_car_gps_timestamp_nanos = 0
     self._last_car_gps_received_monotonic = 0.0
     self._last_car_gps_publish_monotonic = 0.0
+    self._gm_gps = dict.fromkeys(source for source, _ in GM_GPS_SOURCES)
+    self._gm_gps_had_fix = False
 
     self.CC_prev = car.CarControl.new_message()
     self.CS_prev = car.CarState.new_message()
@@ -141,8 +163,9 @@ class Car:
       self.RI = RI
 
     car_gps_supported = bool(getattr(self.CI.CS, 'car_gps_supported', False))
+    self.gm_gps_supported = self.CP.brand == "gm"
     self.params.put_bool("CarGpsAvailable", car_gps_supported)
-    if car_gps_supported:
+    if car_gps_supported or self.gm_gps_supported:
       self.gps_pm = messaging.PubMaster(['gpsLocationExternal'])
 
     aol_available = always_on_lateral_available(self.CP)
@@ -349,39 +372,80 @@ class Car:
     FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles)
     return CS, RD, FPCS
 
+  def _publish_gm_gps(self, now: float) -> None:
+    if self.sm.updated.get("gpsLocation", False):
+      self._gm_gps["device"] = (self.sm["gpsLocation"].to_dict(), now) if self.sm.valid["gpsLocation"] else None
+
+    for source, sample in self.CI.CS.get_car_gps_sources().items():
+      previous = self._gm_gps[source]
+      if sample is None:
+        self._gm_gps[source] = None
+      elif previous is None or sample["timestamp_nanos"] > previous[0]["timestamp_nanos"]:
+        self._gm_gps[source] = (sample.copy(), now)
+
+    # Device > PPS > OnStar, with each source's own freshness window.
+    selected = None
+    for source, timeout in GM_GPS_SOURCES:
+      candidate = self._gm_gps[source]
+      if candidate is not None and now - candidate[1] <= timeout and _gps_sample_is_healthy(candidate[0]):
+        selected = candidate[0]
+        break
+
+    if selected is None:
+      if not self._gm_gps_had_fix:
+        return
+    elif self._gm_gps_had_fix and (now - self._last_car_gps_publish_monotonic < 0.2):
+      return
+
+    gps_send = messaging.new_message('gpsLocationExternal', valid=selected is not None)
+    if selected is not None:
+      gps_send.gpsLocationExternal = {key: value for key, value in selected.items() if key != "timestamp_nanos"}
+      if source != "device":
+        gps_send.gpsLocationExternal.source = "car"
+    else:
+      # Clear subscribers' cached fix once, then let the service become stale.
+      gps_send.gpsLocationExternal.hasFix = False
+    assert self.gps_pm is not None
+    self.gps_pm.send('gpsLocationExternal', gps_send)
+    self._gm_gps_had_fix = selected is not None
+    self._last_car_gps_publish_monotonic = now
+
   def state_publish(self, CS: car.CarState, RD: structs.RadarDataT | None, FPCS: custom.StarPilotCarState):
     """carState and carParams publish loop"""
 
-    get_car_gps = getattr(self.CI.CS, 'get_car_gps', None)
-    car_gps = get_car_gps() if get_car_gps is not None else None
     now = time.monotonic()
-    if car_gps is not None and car_gps['timestamp_nanos'] > self._last_car_gps_timestamp_nanos:
-      self._last_car_gps_timestamp_nanos = car_gps['timestamp_nanos']
-      self._last_car_gps_received_monotonic = now
+    if self.gm_gps_supported:
+      self._publish_gm_gps(now)
+    else:
+      get_car_gps = getattr(self.CI.CS, 'get_car_gps', None)
+      car_gps = get_car_gps() if get_car_gps is not None else None
+      if car_gps is not None and car_gps['timestamp_nanos'] > self._last_car_gps_timestamp_nanos:
+        self._last_car_gps_timestamp_nanos = car_gps['timestamp_nanos']
+        self._last_car_gps_received_monotonic = now
 
-    if car_gps is not None and self._last_car_gps_received_monotonic > 0.0 and \
-        now - self._last_car_gps_received_monotonic <= 2.5 and \
-        now - self._last_car_gps_publish_monotonic >= 0.2:
-      gps_send = messaging.new_message('gpsLocationExternal', valid=True)
-      gps = gps_send.gpsLocationExternal
-      gps.flags = 0
-      gps.latitude = car_gps['latitude']
-      gps.longitude = car_gps['longitude']
-      gps.altitude = car_gps['altitude']
-      gps.speed = car_gps['speed']
-      gps.bearingDeg = car_gps['bearingDeg']
-      gps.horizontalAccuracy = car_gps['horizontalAccuracy']
-      gps.unixTimestampMillis = car_gps['unixTimestampMillis']
-      gps.source = log.GpsLocationData.SensorSource.car
-      gps.vNED = car_gps['vNED']
-      gps.verticalAccuracy = car_gps['verticalAccuracy']
-      gps.bearingAccuracyDeg = car_gps['bearingAccuracyDeg']
-      gps.speedAccuracy = car_gps['speedAccuracy']
-      gps.hasFix = car_gps['hasFix']
-      gps.satelliteCount = car_gps['satelliteCount']
-      assert self.gps_pm is not None
-      self.gps_pm.send('gpsLocationExternal', gps_send)
-      self._last_car_gps_publish_monotonic = now
+      if car_gps is not None and self._last_car_gps_received_monotonic > 0.0 and \
+          now - self._last_car_gps_received_monotonic <= 2.5 and \
+          now - self._last_car_gps_publish_monotonic >= 0.2:
+        gps_send = messaging.new_message('gpsLocationExternal', valid=True)
+        gps = gps_send.gpsLocationExternal
+        gps.flags = 0
+        gps.latitude = car_gps['latitude']
+        gps.longitude = car_gps['longitude']
+        gps.altitude = car_gps['altitude']
+        gps.speed = car_gps['speed']
+        gps.bearingDeg = car_gps['bearingDeg']
+        gps.horizontalAccuracy = car_gps['horizontalAccuracy']
+        gps.unixTimestampMillis = car_gps['unixTimestampMillis']
+        gps.source = log.GpsLocationData.SensorSource.car
+        gps.vNED = car_gps['vNED']
+        gps.verticalAccuracy = car_gps['verticalAccuracy']
+        gps.bearingAccuracyDeg = car_gps['bearingAccuracyDeg']
+        gps.speedAccuracy = car_gps['speedAccuracy']
+        gps.hasFix = car_gps['hasFix']
+        gps.satelliteCount = car_gps['satelliteCount']
+        assert self.gps_pm is not None
+        self.gps_pm.send('gpsLocationExternal', gps_send)
+        self._last_car_gps_publish_monotonic = now
 
     # carParams - logged every 50 seconds (> 1 per segment)
     if self.sm.frame % int(50. / DT_CTRL) == 0:

selfdrive/car/tests/test_gm_gps.py

@@ -0,0 +1,169 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import messaging
+from openpilot.selfdrive.car.card import Car, GM_GPS_SOURCES
+
+
+def sample():
+  gps = messaging.new_message("gpsLocation", valid=True).gpsLocation
+  gps.hasFix = True
+  gps.latitude = 38.3
+  gps.longitude = -85.7
+  gps.source = "ublox"
+  return dict(gps.to_dict(), timestamp_nanos=1)
+
+
+@pytest.fixture
+def selector():
+  card = Car.__new__(Car)
+  card._gm_gps = dict.fromkeys(source for source, _ in GM_GPS_SOURCES)
+  card._gm_gps_had_fix = False
+  card._last_car_gps_publish_monotonic = 0.0
+  card.gps_pm = SimpleNamespace(sent=[])
+  card.gps_pm.send = lambda service, message: card.gps_pm.sent.append((service, message))
+  card.sm = SimpleNamespace(updated={"gpsLocation": False})
+  card.CI = SimpleNamespace(CS=SimpleNamespace(get_car_gps_sources=dict))
+  return card
+
+
+@pytest.mark.parametrize("states,winner", [
+  (("good", None, None), "device"),
+  (("bad", "good", None), "pps"),
+  (("stale", "good", None), "pps"),
+  (("bad", "bad", "good"), "onstar"),
+  (("good", "good", "good"), "device"),
+  (("bad", "stale", None), None),
+  (("stale", "stale", "good"), "onstar"),
+  (("bad", "bad", "bad"), None),
+  (("stale", "stale", "stale"), None),
+])
+def test_priority(selector, states, winner):
+  for index, ((source, timeout), state) in enumerate(zip(GM_GPS_SOURCES, states, strict=True)):
+    if state is not None:
+      fix = sample() | {"latitude": 38.0 + index, "hasFix": state != "bad"}
+      selector._gm_gps[source] = (fix, 10.0 - timeout - 0.01 if state == "stale" else 10.0)
+  selector._publish_gm_gps(10.0)
+  if winner is None:
+    assert not selector.gps_pm.sent
+  else:
+    assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == selector._gm_gps[winner][0]["latitude"]
+
+
+@pytest.mark.parametrize("source,timeout", [("device", 2.0), ("pps", 1.0), ("onstar", 2.5)])
+def test_freshness_and_recovery(selector, source, timeout):
+  selector._gm_gps[source] = (sample(), 10.0)
+  selector._publish_gm_gps(10.0 + timeout)
+  assert selector.gps_pm.sent[-1][1].valid
+  selector._publish_gm_gps(10.01 + timeout)
+  assert not selector.gps_pm.sent[-1][1].valid
+  selector._gm_gps[source] = (sample(), 20.0)
+  selector._publish_gm_gps(20.0)
+  assert selector.gps_pm.sent[-1][1].valid
+
+
+@pytest.mark.parametrize("source", ["device", "pps"])
+def test_upward_recovery(selector, source):
+  selector._gm_gps["onstar"] = (sample() | {"latitude": 40.0}, 10.0)
+  selector._publish_gm_gps(10.0)
+  assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == 40.0
+  selector._gm_gps[source] = (sample(), 10.21)
+  selector._publish_gm_gps(10.21)
+  assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == 38.3
+
+
+@pytest.mark.parametrize("changes,expected", [
+  ({"latitude": float("nan")}, False), ({"longitude": float("inf")}, False),
+  ({"latitude": 91}, False), ({"longitude": -181}, False), ({"longitude": 181}, False),
+  ({"latitude": 0, "longitude": 0}, False), ({"speed": float("nan")}, False),
+  ({"latitude": 0, "longitude": 10}, True), ({"latitude": 10, "longitude": 0}, True),
+  ({"horizontalAccuracy": 500.0}, True),
+])
+def test_health(selector, changes, expected):
+  selector._gm_gps = {source: (sample() | {"latitude": 40.0}, 10.0) for source, _ in GM_GPS_SOURCES}
+  selector._gm_gps["device"] = (sample() | changes, 10.0)
+  selector._publish_gm_gps(10.0)
+  gps = selector.gps_pm.sent[-1][1].gpsLocationExternal
+  assert gps.latitude == (selector._gm_gps["device"][0]["latitude"] if expected else 40.0)
+  assert gps.source == ("ublox" if expected else "car")
+
+
+@pytest.mark.parametrize("source", [source for source, _ in GM_GPS_SOURCES])
+def test_selected_fix_fields_are_preserved(selector, source):
+  fix = sample() | {"altitude": 123.0, "speed": 10.0, "vNED": [0.0, 10.0, 0.0], "bearingDeg": 90.0,
+                    "horizontalAccuracy": 3.0, "verticalAccuracy": 5.0, "bearingAccuracyDeg": 2.0}
+  if source == "device":
+    selector._gm_gps[source] = (fix, 10.0)
+  else:
+    selector.CI.CS.get_car_gps_sources = lambda: {source: fix}
+  selector._publish_gm_gps(10.0)
+  service, message = selector.gps_pm.sent[-1]
+  assert service == "gpsLocationExternal"
+  assert message.valid
+  expected = {key: value for key, value in fix.items() if key != "timestamp_nanos"}
+  expected["source"] = "ublox" if source == "device" else "car"
+  assert message.gpsLocationExternal.to_dict() == expected
+
+
+@pytest.mark.parametrize("source", ["pps", "onstar"])
+def test_can_cache_freshness_and_explicit_invalidation(selector, source):
+  sources = {"pps": sample(), "onstar": sample() | {"latitude": 40.0}}
+  selector.CI.CS.get_car_gps_sources = lambda: sources
+  selector._publish_gm_gps(10.0)
+  for timestamp in (1, 0):
+    sources[source] = sources[source] | {"timestamp_nanos": timestamp}
+    selector._publish_gm_gps(10.5)
+    assert selector._gm_gps[source][1] == 10.0
+  sources[source] = sources[source] | {"timestamp_nanos": 2}
+  selector._publish_gm_gps(10.6)
+  assert selector._gm_gps[source][1] == 10.6
+  sources[source] = None
+  selector._publish_gm_gps(10.9)
+  assert selector._gm_gps[source] is None
+  assert selector.gps_pm.sent[-1][1].gpsLocationExternal.latitude == (40.0 if source == "pps" else 38.3)
+
+
+def test_no_source_publication_transition(selector):
+  selector._publish_gm_gps(10.0)
+  assert not selector.gps_pm.sent
+  selector._gm_gps["device"] = (sample(), 10.0)
+  selector._publish_gm_gps(10.0)
+  valid = selector.gps_pm.sent[-1][1]
+  assert valid.valid and valid.gpsLocationExternal.hasFix
+  assert valid.gpsLocationExternal.latitude == 38.3
+  selector._publish_gm_gps(10.1)
+  assert len(selector.gps_pm.sent) == 1
+  selector._publish_gm_gps(12.1)
+  invalid = selector.gps_pm.sent[-1][1]
+  assert not invalid.valid and not invalid.gpsLocationExternal.hasFix
+  for now in (12.12, 12.13, 12.14):
+    selector._publish_gm_gps(now)
+  assert len(selector.gps_pm.sent) == 2
+  selector._gm_gps["device"] = (sample(), 12.15)
+  selector._publish_gm_gps(12.15)  # Recovery bypasses the 200 ms healthy cadence.
+  assert len(selector.gps_pm.sent) == 3
+  assert selector.gps_pm.sent[-1][1].gpsLocationExternal.hasFix
+  for now in (15.0, 20.0, 30.0):
+    selector._publish_gm_gps(now)
+  assert len(selector.gps_pm.sent) == 4  # One more loss marker, then sustained silence.
+
+
+def test_device_updates_honor_event_validity(selector):
+  message = messaging.new_message("gpsLocation", valid=True)
+  message.gpsLocation = {key: value for key, value in sample().items() if key != "timestamp_nanos"}
+  class DeviceMessages(dict):
+    updated = {"gpsLocation": True}
+    valid = {"gpsLocation": True}
+  selector.sm = DeviceMessages(gpsLocation=message.gpsLocation)
+  selector._publish_gm_gps(10.0)
+  assert selector.gps_pm.sent[-1][1].valid
+  selector.sm.updated["gpsLocation"] = False
+  selector._publish_gm_gps(10.05)
+  assert selector._gm_gps["device"][1] == 10.0
+  selector.sm.updated["gpsLocation"] = True
+  selector.sm.valid["gpsLocation"] = False
+  selector._publish_gm_gps(10.1)
+  assert selector._gm_gps["device"] is None
+  assert len(selector.gps_pm.sent) == 2  # Loss bypasses the healthy cadence too.
+  assert not selector.gps_pm.sent[-1][1].gpsLocationExternal.hasFix

selfdrive/controls/tests/test_starpilot_planner.py

@@ -37,7 +37,7 @@ def make_toggles(**overrides):
 
 def test_force_stop_jerk_scale_is_platform_specific():
   assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="HYUNDAI_ELANTRA_2021")) == 0.80
-  assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.32
+  assert get_force_stop_jerk_scale(SimpleNamespace(carFingerprint="OTHER_CAR")) == 0.20
 
 
 def test_lead_follow_jerk_scale_is_platform_specific():
@@ -255,3 +255,18 @@ def test_untracked_vision_lead_still_uses_strict_entry_gate():
     assert not planner.update_lead_status(16.8)
   finally:
     planner.shutdown()
+
+
+def test_gps_location_service_updates_on_carparams(monkeypatch):
+  planner = make_planner(monkeypatch)
+  try:
+    sm = make_sm(planner, frame=1, v_ego=20.0, left_blinker=False)
+    sm.updated = {"carParams": True}
+    sm["carParams"] = SimpleNamespace(brand="gm")
+    sm["gpsLocationExternal"] = SimpleNamespace(latitude=2.0, longitude=2.0, bearingDeg=45.0, hasFix=True)
+    planner.update(0.0, False, sm, make_toggles())
+    assert planner.gps_location_service == "gpsLocationExternal"
+    assert planner.gps_position["latitude"] == 2.0
+  finally:
+    planner.shutdown()
+

starpilot/controls/starpilot_planner.py

@@ -126,6 +126,10 @@ class StarPilotPlanner:
       v_cruise_kph += starpilot_toggles.set_speed_offset
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
     v_ego = max(sm["carState"].vEgo, 0)
+    updated = getattr(sm, "updated", None)
+    if updated is not None and updated.get("carParams", False):
+      cp = sm["carParams"] if "carParams" in sm else None
+      self.gps_location_service = get_gps_location_service(self.params, cp)
 
     gps_location = sm[self.gps_location_service]
     self.gps_position = {

starpilot/starpilot_process.py

@@ -310,6 +310,9 @@ def starpilot_thread():
   while True:
     sm.update()
 
+    if sm.updated["carParams"]:
+      gps_location_service = get_gps_location_service(params, sm["carParams"])
+
     now = datetime.datetime.now(datetime.timezone.utc)
     monotonic_now = time.monotonic()
 

starpilot/system/speed_limit_filler.py

@@ -55,7 +55,10 @@ class MapSpeedLogger:
 
     self.gps_location_service = get_gps_location_service(self.params)
 
-    self.sm = messaging.SubMaster(["deviceState", "starpilotCarState", "starpilotPlan", self.gps_location_service, "mapdOut", "modelV2"])
+    self.sm = messaging.SubMaster([
+      "carParams", "deviceState", "starpilotCarState", "starpilotPlan", "gpsLocation", "gpsLocationExternal",
+      "mapdOut", "modelV2",
+    ])
 
   @property
   def can_make_overpass_request(self):
@@ -252,6 +255,8 @@ class MapSpeedLogger:
     return relevant_segments
 
   def log_speed_limit(self):
+    if self.sm.updated["carParams"]:
+      self.gps_location_service = get_gps_location_service(self.params, self.sm["carParams"])
     if not self.sm.updated[self.gps_location_service]:
       return
 

system/manager/process_config.py

@@ -1,3 +1,5 @@
+from __future__ import annotations
+
 import os
 import operator
 import platform
@@ -6,6 +8,7 @@ import sys
 from types import SimpleNamespace
 
 from cereal import car
+from openpilot.common.gps import gm_car_params_present
 from openpilot.common.params import Params
 from opendbc.car.gps import car_gps_available
 from openpilot.system.hardware import HARDWARE, PC, TICI
@@ -36,9 +39,12 @@ def update_car_gps_param(params: Params) -> bool | None:
   car_params = params.get("CarParams")
   if car_params is None:
     return None
+  try:
+    with car.CarParams.from_bytes(car_params) as parsed_cp:
+      available = car_gps_available(parsed_cp)
+  except Exception:
+    return None
 
-  with car.CarParams.from_bytes(car_params) as CP:
-    available = car_gps_available(CP)
   if available != params.get_bool("CarGpsAvailable"):
     params.put_bool("CarGpsAvailable", available)
   return available
@@ -48,7 +54,9 @@ def ublox(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: S
   use_ublox = ublox_available()
   if use_ublox != params.get_bool("UbloxAvailable"):
     params.put_bool("UbloxAvailable", use_ublox)
-  return started and use_ublox and car_gps is False
+  is_gm = gm_car_params_present(params)
+  # On GM, card arbitrates u-blox and CAN GPS; wait for CarParams.
+  return started and use_ublox and car_gps is not None and (not car_gps or is_gm)
 
 def joystick(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool:
   return started and params.get_bool("JoystickDebugMode")

system/manager/test/test_process_config.py

@@ -4,6 +4,7 @@ import pytest
 
 from cereal import car
 from opendbc.car.ford.values import CAR as FORD_CAR
+from opendbc.car.gm.values import CAR as GM_CAR
 import openpilot.system.manager.process_config as process_config
 from openpilot.system.manager.process_config import (
   allow_uploads,
@@ -152,25 +153,33 @@ class GpsParams:
     self.values[key] = value
 
 
-def test_ublox_waits_for_current_carparams(monkeypatch):
-  monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
-  params = GpsParams()
+@pytest.mark.parametrize("brand,fingerprint,persisted,live_brand,expected,car_gps", [
+  ("gm", "", False, "gm", False, False),
+  ("mock", "", False, "mock", False, False),
+  ("ford", "", False, "ford", False, False),
+  ("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, False, "gm", False, False),
+  ("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, True, "gm", True, True),
+  ("gm", GM_CAR.CHEVROLET_BOLT_CC_2018_2021, True, "", True, True),
+  ("gm", "unknown GM", True, "gm", True, False),
+  ("mock", "mock", True, "", True, False),
+  ("ford", FORD_CAR.FORD_MUSTANG_MACH_E_MK1, True, "", False, True),
+  ("gm", "", "corrupt", "", False, False),
+], ids=["missing-gm", "missing-mock", "missing-ford", "stale-live-cp", "persisted-gm",
+        "empty-live-cp", "unknown-gm", "non-car-gps", "vehicle-gps", "corrupt-cp"])
+def test_ublox_startup(monkeypatch, brand, fingerprint, persisted, live_brand, expected, car_gps):
+  monkeypatch.setattr(process_config, "ublox_available", lambda: True)
+  current = car.CarParams.new_message(brand=brand, carFingerprint=fingerprint)
+  params = GpsParams(current if persisted is True else None)
+  if persisted == "corrupt":
+    params.values["CarParams"] = b"invalid_corrupt_data"
+  live_cp = car.CarParams.new_message(brand=live_brand, carFingerprint=fingerprint if live_brand else "")
 
-  assert not ublox(True, params, car.CarParams.new_message(), SimpleNamespace())
+  assert ublox(True, params, live_cp, SimpleNamespace()) is expected
   assert params.get_bool("UbloxAvailable")
-
-
-@pytest.mark.parametrize("car_gps,expected", [(False, True), (True, False)])
-def test_ublox_has_single_external_gps_publisher(monkeypatch, car_gps, expected):
-  monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True)
-  CP = car.CarParams.new_message()
-  if car_gps:
-    CP.brand = "ford"
-    CP.carFingerprint = FORD_CAR.FORD_MUSTANG_MACH_E_MK1
-  else:
-    CP.brand = "mock"
-    CP.carFingerprint = "mock"
-  params = GpsParams(CP)
-
-  assert ublox(True, params, car.CarParams.new_message(), SimpleNamespace()) is expected
   assert params.get_bool("CarGpsAvailable") is car_gps
+  assert not ublox(False, params, live_cp, SimpleNamespace())
+  if persisted is False and fingerprint:
+    params.values["CarParams"] = current.to_bytes()
+    assert ublox(True, params, live_cp, SimpleNamespace())  # Same stale live CP, now with current persisted CP.
+  monkeypatch.setattr(process_config, "ublox_available", lambda: False)
+  assert not ublox(True, params, live_cp, SimpleNamespace())

system/timed.py

@@ -63,7 +63,8 @@ def main() -> NoReturn:
   gps_location_service = get_gps_location_service(params)
 
   pm = messaging.PubMaster(['clocks'])
-  sm = messaging.SubMaster([gps_location_service])
+  # Subscribe to both services to support dynamic GM GPS routing.
+  sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal'])
 
   # StarPilot variables
   tf = TimezoneFinder() if TimezoneFinder is not None else None
@@ -75,6 +76,7 @@ def main() -> NoReturn:
 
   while True:
     sm.update(1000)
+    gps_location_service = get_gps_location_service(params)
 
     msg = messaging.new_message('clocks')
     msg.valid = system_time_valid()

system/ubloxd/tests/test_gps_routing.py

@@ -0,0 +1,30 @@
+from types import SimpleNamespace
+
+import pytest
+
+from cereal import car, messaging
+from openpilot.system.ubloxd import ubloxd
+
+
+@pytest.mark.parametrize("brand,service", [("gm", "gpsLocation"), ("mock", "gpsLocationExternal")])
+def test_main_routes_every_parsed_fix(monkeypatch, brand, service):
+  cp = car.CarParams.new_message(brand=brand)
+  monkeypatch.setattr(ubloxd, "Params", lambda: SimpleNamespace(get=lambda key: cp.to_bytes()))
+  sent = []
+  pm = SimpleNamespace(send=lambda service, message: sent.append((service, message)))
+  monkeypatch.setattr(messaging, "PubMaster", lambda services: pm)
+  monkeypatch.setattr(messaging, "sub_sock", lambda *args, **kwargs: None)
+  # Several fixes in one receive cycle must all reach the publisher.
+  incoming = iter([SimpleNamespace(ubloxRaw=b"", logMonoTime=1)])
+  monkeypatch.setattr(messaging, "recv_one", lambda sock: next(incoming))
+  parser = ubloxd.UbloxMsgParser(service)
+  fixes = [messaging.new_message(service, valid=True) for _ in range(5)]
+  monkeypatch.setattr(parser.framer, "add_data", lambda *args: range(5))
+  monkeypatch.setattr(parser, "parse_frame", lambda index: (parser.gps_service, fixes[index]))
+  def make_parser(actual_service):
+    assert actual_service == service
+    return parser
+  monkeypatch.setattr(ubloxd, "UbloxMsgParser", make_parser)
+  with pytest.raises(StopIteration):
+    ubloxd.main()
+  assert sent == [(service, fix) for fix in fixes]

system/ubloxd/ubloxd.py

@@ -6,8 +6,11 @@ import numpy as np
 from collections import defaultdict
 from dataclasses import dataclass
 
+from cereal import car
 from cereal import log
 from cereal import messaging
+from openpilot.common.gps import gm_car_params_present
+from openpilot.common.params import Params
 from openpilot.system.ubloxd.generated.ubx import Ubx
 from openpilot.system.ubloxd.generated.gps import Gps
 from openpilot.system.ubloxd.generated.glonass import Glonass
@@ -103,7 +106,8 @@ class UbloxMsgParser:
     11: 64, 12: 128, 13: 256, 14: 512, 15: 1024,
   }
 
-  def __init__(self) -> None:
+  def __init__(self, gps_service: str = 'gpsLocationExternal') -> None:
+    self.gps_service = gps_service
     self.framer = UbxFramer()
     self.caches = EphemerisCaches(
       gps_subframes=defaultdict(dict),
@@ -159,10 +163,10 @@ class UbloxMsgParser:
       return self._gen_nav_sat(body)
     return None
 
-  # NAV-PVT -> gpsLocationExternal
+  # NAV-PVT -> the selected canonical GPS service
   def _gen_nav_pvt(self, msg: Ubx.NavPvt) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder]:
-    dat = messaging.new_message('gpsLocationExternal', valid=True)
-    gps = dat.gpsLocationExternal
+    dat = messaging.new_message(self.gps_service, valid=True)
+    gps = getattr(dat, self.gps_service)
     gps.source = log.GpsLocationData.SensorSource.ublox
     gps.flags = msg.flags
     gps.hasFix = (msg.flags % 2) == 1
@@ -191,7 +195,7 @@ class UbloxMsgParser:
     gps.verticalAccuracy = msg.v_acc * 1e-03
     gps.speedAccuracy = msg.s_acc * 1e-03
     gps.bearingAccuracyDeg = msg.head_acc * 1e-05
-    return ('gpsLocationExternal', dat)
+    return (self.gps_service, dat)
 
   # RXM-SFRBX dispatch to GPS or GLONASS ephemeris
   def _gen_rxm_sfrbx(self, msg) -> tuple[str, capnp.lib.capnp._DynamicStructBuilder] | None:
@@ -493,8 +497,11 @@ class UbloxMsgParser:
 
 
 def main():
-  parser = UbloxMsgParser()
-  pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal'])
+  params = Params()
+  gps_service = 'gpsLocation' if gm_car_params_present(params) else 'gpsLocationExternal'
+
+  parser = UbloxMsgParser(gps_service)
+  pm = messaging.PubMaster(['ubloxGnss', gps_service])
   sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False)
 
   while True: