From 87b961b282a7b0459e1de033ccaea24ee625d644 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Mon, 7 Sep 2026 14:04:09 -0400 Subject: [PATCH] wip --- common/gps.py | 16 +- opendbc_repo/opendbc/car/gm/carstate.py | 190 +++++++++++++++- opendbc_repo/opendbc/car/gm/tests/test_gm.py | 47 +++- opendbc_repo/opendbc/car/gps.py | 31 ++- selfdrive/car/card.py | 227 ++++++++++++++++--- starpilot/starpilot_process.py | 2 + starpilot/system/speed_limit_filler.py | 6 +- system/manager/process_config.py | 9 +- system/manager/test/test_process_config.py | 12 + system/timed.py | 6 +- system/ubloxd/ubloxd.py | 34 ++- 11 files changed, 533 insertions(+), 47 deletions(-) diff --git a/common/gps.py b/common/gps.py index 4d1a95c6a..64de746c6 100644 --- a/common/gps.py +++ b/common/gps.py @@ -1,8 +1,22 @@ +from cereal import car from openpilot.common.params import Params +def _gm_car_params_present(params: Params) -> bool: + try: + raw_car_params = params.get("CarParams") + if raw_car_params is None: + return False + with car.CarParams.from_bytes(raw_car_params) as CP: + return CP.brand == "gm" + except Exception: + return False + + def get_gps_location_service(params: Params) -> str: - if params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"): + # GM card publishes the selected device/PPS/OnStar result through the + # existing external service. Keep every other platform on its prior rule. + if _gm_car_params_present(params) or params.get_bool("UbloxAvailable") or params.get_bool("CarGpsAvailable"): return "gpsLocationExternal" else: return "gpsLocation" diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 9e0f3c122..f0a4ad74d 100644 --- a/opendbc_repo/opendbc/car/gm/carstate.py +++ b/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 @@ -38,6 +40,140 @@ 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) +# The CT6 DBC describes the PPS receiver output. These messages have been +# observed on the GM powertrain bus at approximately 10 Hz; they remain an +# optional parser so a vehicle which does not provide them is unaffected. +PPS_GPS_MESSAGES = ( + "PPS_ElevHdSpd_FO", + "PPS_PosLat_FO", + "PPS_PosLong_FO", + "PPS_Time_FO", + "PPS_QualMetrics_FO", +) +# PPS_SigAcqTime_FO is intentionally not a health gate: on the observed GM +# route its validity bit remains 1 even while the position bundle is valid. +PPS_GPS_VALIDITY_SIGNALS = ( + "PPSLatV", "PPSLongV", "PPS2DAbsPosErrEstmtV", "PPSMdV", "PPSPstnDilPrcsV", + "PPSTmdayV", "PPSCldrDayV", "PPSCldrYrV", +) +PPS_GPS_VALIDITY_MESSAGES = { + "PPSLatV": "PPS_PosLat_FO", + "PPSLongV": "PPS_PosLong_FO", + "PPS2DAbsPosErrEstmtV": "PPS_QualMetrics_FO", + "PPSMdV": "PPS_QualMetrics_FO", + "PPSPstnDilPrcsV": "PPS_QualMetrics_FO", + "PPSTmdayV": "PPS_Time_FO", + "PPSCldrDayV": "PPS_Time_FO", + "PPSCldrYrV": "PPS_Time_FO", +} + + +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 _invalid_pps_sample(timestamp_nanos: int) -> dict: + return { + "timestamp_nanos": timestamp_nanos, + "latitude": 0.0, + "longitude": 0.0, + "altitude": 0.0, + "speed": 0.0, + "bearingDeg": 0.0, + "horizontalAccuracy": 0.0, + "unixTimestampMillis": 0, + "verticalAccuracy": 0.0, + "bearingAccuracyDeg": 180.0, + "speedAccuracy": 0.0, + "hasFix": False, + "satelliteCount": 0, + "vNED": [0.0, 0.0, 0.0], + } + + +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: + if any(int(values[PPS_GPS_VALIDITY_MESSAGES[signal]].get(signal, 1)) != 0 for signal in PPS_GPS_VALIDITY_SIGNALS): + return None + # Mode 6 is explicitly "DR ONLY" in the DBC and is not an independent GPS + # position. All other advertised modes include GNSS. + if int(values["PPS_QualMetrics_FO"]["PPSMd"]) == 6: + return None + + latitude = float(values["PPS_PosLat_FO"]["PPSLat"]) + longitude = float(values["PPS_PosLong_FO"]["PPSLong"]) + if not (math.isfinite(latitude) and math.isfinite(longitude) and + -90.0 <= latitude / 3_600_000.0 <= 90.0 and + -180.0 <= longitude / 3_600_000.0 <= 180.0): + return None + latitude /= 3_600_000.0 + longitude /= 3_600_000.0 + if latitude == 0.0 and longitude == 0.0: + return None + + year = int(values["PPS_Time_FO"]["PPSCldrYr"]) + day_of_year = int(values["PPS_Time_FO"]["PPSCldrDay"]) + millis_of_day = int(values["PPS_Time_FO"]["PPSTmday"]) + if not 2014 <= year <= 2141 or not 0 <= day_of_year <= 365 or not 0 <= millis_of_day < 86_400_000: + return None + timestamp = datetime(year, 1, 1, tzinfo=UTC) + timedelta(days=day_of_year, milliseconds=millis_of_day) + + speed = float(values["PPS_ElevHdSpd_FO"]["PPSVel"]) * CV.KPH_TO_MS + heading = float(values["PPS_ElevHdSpd_FO"]["PPSHedng"]) + if int(values["PPS_ElevHdSpd_FO"].get("PPSVelV", 1)) != 0 or not math.isfinite(speed) or not 0.0 <= speed <= 200.0: + speed = 0.0 + if (int(values["PPS_ElevHdSpd_FO"].get("PPSHedngV", 1)) != 0 or + not math.isfinite(heading) or not 0.0 <= heading < 360.0): + heading = 0.0 + + altitude = float(values["PPS_ElevHdSpd_FO"]["PPSElvtn"]) + if int(values["PPS_ElevHdSpd_FO"].get("PPSElvtnV", 1)) != 0 or not math.isfinite(altitude): + altitude = 0.0 + else: + altitude /= 100.0 + + horizontal_accuracy = float(values["PPS_QualMetrics_FO"]["PPS2DAbsPosErrEstmt"]) + vertical_accuracy = float(values["PPS_QualMetrics_FO"]["PPS3DAbsPosErrEstmt"]) + bearing_accuracy = float(values["PPS_QualMetrics_FO"]["PPSAbsHdngErrEstmt"]) + if not math.isfinite(horizontal_accuracy) or horizontal_accuracy < 0.0: + horizontal_accuracy = 0.0 + if not math.isfinite(vertical_accuracy) or vertical_accuracy < 0.0: + vertical_accuracy = 0.0 + if not math.isfinite(bearing_accuracy) or bearing_accuracy < 0.0: + bearing_accuracy = 180.0 + except (KeyError, TypeError, ValueError, OverflowError): + 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": int(timestamp.timestamp() * 1000), + "verticalAccuracy": vertical_accuracy, + "bearingAccuracyDeg": bearing_accuracy, + # The CT6 DBC does not document the velocity-error units, so do not expose + # a made-up conversion as a speed accuracy value. + "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 +245,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 +288,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 _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 + + burst_ids = ( + int(cp.vl["PPS_ElevHdSpd_FO"]["PPSElvHedngSpdBrstID"]), + int(cp.vl["PPS_PosLat_FO"]["PPSLatBrstID"]), + int(cp.vl["PPS_PosLong_FO"]["PPSLongBrstID"]), + int(cp.vl["PPS_Time_FO"]["PPSTmBrstID"]), + int(cp.vl["PPS_QualMetrics_FO"]["PPSPosQltyMtcBrstID"]), + ) + if len(set(burst_ids)) != 1: + return + + values = {name: cp.vl[name] for name in PPS_GPS_MESSAGES} + raw = {name: cp.vl_raw[name] for name in PPS_GPS_MESSAGES} + gps = decode_gm_pps_gps(values, raw, timestamp_nanos) + self.pps_gps = gps if gps is not None else _invalid_pps_sample(timestamp_nanos) + self._pps_gps_timestamp_nanos = timestamp_nanos + def get_car_gps(self): return self.car_gps + def get_car_gps_sources(self): + 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 +414,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 +766,18 @@ 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": + # Keep the CT6 object DBC isolated from the active vehicle DBC. Every + # signal is optional, so a GM vehicle without PPS frames stays can-valid + # and follows the existing device/OnStar behavior. + parsers[Bus.adas] = CANParser( + "cadillac_ct6_object", + [(name, 0) for name in PPS_GPS_MESSAGES], + CanBus.POWERTRAIN, + ) + return parsers diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index ca34e609c..ad5cc12f2 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -8,7 +8,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 +102,44 @@ 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["unixTimestampMillis"] == 1788742068655 + + def test_invalid_position_validity_is_rejected(self): + parser = CANParser("cadillac_ct6_object", [(name, 0) for name in PPS_GPS_MESSAGES], 0) + parser.update([(1_000_000_000, self._frames)]) + values = {name: dict(parser.vl[name]) for name in PPS_GPS_MESSAGES} + values["PPS_PosLat_FO"]["PPSLatV"] = 1 + gps = decode_gm_pps_gps( + values, + {name: parser.vl_raw[name] for name in PPS_GPS_MESSAGES}, + 1_000_000_000, + ) + assert gps is None + def test_bolt_gps_heading_and_speed_derivation(self): cp = SimpleNamespace( brand="gm", diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py index 750d9ebf8..92c9d08c9 100644 --- a/opendbc_repo/opendbc/car/gps.py +++ b/opendbc_repo/opendbc/car/gps.py @@ -3,11 +3,14 @@ import math from dataclasses import dataclass from datetime import UTC, datetime from collections.abc import Callable, Mapping +from functools import cache 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] @@ -157,9 +160,33 @@ CAR_GPS_CONFIGS: dict[str, CarGpsConfig] = { } +@cache +def _gm_dbc_has_onstar_gps(dbc_name: str) -> bool: + return "TCICOnStarGPSPosition" in DBC_FILE(dbc_name).name_to_msg + + 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 + + # The OnStar position message is shared by the GM powertrain DBCs. Keep the + # existing explicit map for known vehicles, but enable the same decoder for + # other GM fingerprints only when their active DBC actually defines it. + if cp_brand == "gm": + try: + dbc_name = GM_DBC[CP.carFingerprint][Bus.pt] + if _gm_dbc_has_onstar_gps(dbc_name): + 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: diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8e761c203..5d8420811 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -43,6 +43,13 @@ REDNECK_DECREASE_LOOKAHEAD_POINTS = 10 SLC_SOURCE_NONE = "None" EventName = log.OnroadEvent.EventName +GM_GPS_SOURCE_ORDER = ("device", "pps", "onstar") +GM_GPS_TIMEOUTS = { + "device": 2.0, + "pps": 1.0, + "onstar": 1.0, +} + # forward carlog.addHandler(ForwardingHandler(cloudlog)) @@ -76,6 +83,44 @@ def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket return can_recv, can_send +def _gps_sample_from_message(gps, timestamp_nanos: int, received_monotonic: float) -> dict: + return { + "timestamp_nanos": timestamp_nanos, + "received_monotonic": received_monotonic, + "flags": int(getattr(gps, "flags", 0)), + "latitude": float(gps.latitude), + "longitude": float(gps.longitude), + "altitude": float(gps.altitude), + "speed": float(gps.speed), + "bearingDeg": float(gps.bearingDeg), + "horizontalAccuracy": float(gps.horizontalAccuracy), + "unixTimestampMillis": int(gps.unixTimestampMillis), + "verticalAccuracy": float(gps.verticalAccuracy), + "bearingAccuracyDeg": float(gps.bearingAccuracyDeg), + "speedAccuracy": float(gps.speedAccuracy), + "hasFix": bool(gps.hasFix), + "satelliteCount": int(gps.satelliteCount), + "source": str(gps.source), + "vNED": [float(v) for v in gps.vNED], + } + + +def _gps_sample_is_healthy(sample: dict | None, now: float, timeout: float) -> bool: + if sample is None or not sample.get("hasFix", False): + return False + if now - float(sample.get("received_monotonic", 0.0)) > timeout: + 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 + all(math.isfinite(float(sample[key])) for key in ("altitude", "speed", "bearingDeg"))) + except (KeyError, TypeError, ValueError): + return False + + class Car: CI: CarInterfaceBase RI: RadarInterfaceBase @@ -85,7 +130,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 +140,13 @@ 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_device_gps = None + self._gm_device_gps_timestamp_nanos = 0 + self._gm_car_gps = {"pps": None, "onstar": None} + self._gm_car_gps_timestamps = {"pps": 0, "onstar": 0} + self._gm_selected_source = None + self._gm_source_good_counts = dict.fromkeys(GM_GPS_SOURCE_ORDER, 0) + self._gm_source_last_timestamps = dict.fromkeys(GM_GPS_SOURCE_ORDER, 0) self.CC_prev = car.CarControl.new_message() self.CS_prev = car.CarState.new_message() @@ -141,8 +195,9 @@ class Car: self.RI = RI car_gps_supported = bool(getattr(self.CI.CS, 'car_gps_supported', False)) - self.params.put_bool("CarGpsAvailable", car_gps_supported) - if car_gps_supported: + self.gm_gps_supported = self.CP.brand == "gm" + self.params.put_bool("CarGpsAvailable", car_gps_supported or self.gm_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 +404,149 @@ class Car: FPCS = self.starpilot_card.update(CS, FPCS, self.sm, self.starpilot_toggles) return CS, RD, FPCS + def _update_gm_gps_candidates(self, now: float) -> None: + if self.sm.updated.get("gpsLocation", False): + timestamp_nanos = int(self.sm.logMonoTime["gpsLocation"]) + if timestamp_nanos > self._gm_device_gps_timestamp_nanos: + self._gm_device_gps_timestamp_nanos = timestamp_nanos + self._gm_device_gps = _gps_sample_from_message(self.sm["gpsLocation"], timestamp_nanos, now) + + get_sources = getattr(self.CI.CS, "get_car_gps_sources", None) + sources = get_sources() if get_sources is not None else {} + for source in ("pps", "onstar"): + sample = sources.get(source) if sources is not None else None + if sample is None: + continue + timestamp_nanos = int(sample.get("timestamp_nanos", 0)) + if timestamp_nanos > self._gm_car_gps_timestamps[source]: + candidate = dict(sample) + candidate["received_monotonic"] = now + candidate["source"] = log.GpsLocationData.SensorSource.car + self._gm_car_gps[source] = candidate + self._gm_car_gps_timestamps[source] = timestamp_nanos + + def _select_gm_gps_source(self, now: float) -> dict | None: + candidates = { + "device": self._gm_device_gps, + "pps": self._gm_car_gps["pps"], + "onstar": self._gm_car_gps["onstar"], + } + healthy = { + source: _gps_sample_is_healthy(sample, now, GM_GPS_TIMEOUTS[source]) + for source, sample in candidates.items() + } + + for source in GM_GPS_SOURCE_ORDER: + sample = candidates[source] + timestamp_nanos = int(sample.get("timestamp_nanos", 0)) if sample is not None else 0 + if timestamp_nanos != self._gm_source_last_timestamps[source]: + self._gm_source_last_timestamps[source] = timestamp_nanos + self._gm_source_good_counts[source] = self._gm_source_good_counts[source] + 1 if healthy[source] else 0 + elif not healthy[source]: + self._gm_source_good_counts[source] = 0 + + current = self._gm_selected_source + if current is not None and healthy.get(current, False): + current_index = GM_GPS_SOURCE_ORDER.index(current) + # Keep a recovered higher-priority source from replacing a live source + # on one transient sample; fallback from a failed source is immediate. + for source in GM_GPS_SOURCE_ORDER[:current_index]: + if healthy[source] and self._gm_source_good_counts[source] >= 2: + self._gm_selected_source = source + return candidates[source] + return candidates[current] + + for source in GM_GPS_SOURCE_ORDER: + if healthy[source]: + self._gm_selected_source = source + return candidates[source] + + self._gm_selected_source = None + return None + + def _publish_gm_gps(self, now: float) -> None: + self._update_gm_gps_candidates(now) + selected = self._select_gm_gps_source(now) + if now - self._last_car_gps_publish_monotonic < 0.2: + return + + has_fix = selected is not None + if selected is None: + selected = { + "flags": 0, + "latitude": 0.0, + "longitude": 0.0, + "altitude": 0.0, + "speed": 0.0, + "bearingDeg": 0.0, + "horizontalAccuracy": 0.0, + "unixTimestampMillis": 0, + "verticalAccuracy": 0.0, + "bearingAccuracyDeg": 180.0, + "speedAccuracy": 0.0, + "hasFix": False, + "satelliteCount": 0, + "source": log.GpsLocationData.SensorSource.car, + "vNED": [0.0, 0.0, 0.0], + } + + gps_send = messaging.new_message('gpsLocationExternal', valid=has_fix) + gps = gps_send.gpsLocationExternal + gps.flags = selected.get('flags', 0) + gps.latitude = selected['latitude'] + gps.longitude = selected['longitude'] + gps.altitude = selected['altitude'] + gps.speed = selected['speed'] + gps.bearingDeg = selected['bearingDeg'] + gps.horizontalAccuracy = selected['horizontalAccuracy'] + gps.unixTimestampMillis = selected['unixTimestampMillis'] + gps.source = selected.get('source', log.GpsLocationData.SensorSource.car) + gps.vNED = selected['vNED'] + gps.verticalAccuracy = selected['verticalAccuracy'] + gps.bearingAccuracyDeg = selected['bearingAccuracyDeg'] + gps.speedAccuracy = selected['speedAccuracy'] + gps.hasFix = selected['hasFix'] + gps.satelliteCount = selected['satelliteCount'] + assert self.gps_pm is not None + self.gps_pm.send('gpsLocationExternal', gps_send) + 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: diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index 0db249cac..432f8198e 100644 --- a/starpilot/starpilot_process.py +++ b/starpilot/starpilot_process.py @@ -310,6 +310,8 @@ def starpilot_thread(): while True: sm.update() + gps_location_service = get_gps_location_service(params) + now = datetime.datetime.now(datetime.timezone.utc) monotonic_now = time.monotonic() diff --git a/starpilot/system/speed_limit_filler.py b/starpilot/system/speed_limit_filler.py index f172c697a..fabfc00b1 100644 --- a/starpilot/system/speed_limit_filler.py +++ b/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([ + "deviceState", "starpilotCarState", "starpilotPlan", "gpsLocation", "gpsLocationExternal", + "mapdOut", "modelV2", + ]) @property def can_make_overpass_request(self): @@ -252,6 +255,7 @@ class MapSpeedLogger: return relevant_segments def log_speed_limit(self): + self.gps_location_service = get_gps_location_service(self.params) if not self.sm.updated[self.gps_location_service]: return diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 7f8a16213..fe1fc76dc 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -38,7 +38,10 @@ def update_car_gps_param(params: Params) -> bool | None: return None with car.CarParams.from_bytes(car_params) as CP: - available = car_gps_available(CP) + # GM uses the car-state publisher as the source selector even when a + # particular vehicle has neither CAN fallback receiver. Other brands + # retain the existing decoder-based parameter semantics. + available = car_gps_available(CP) or CP.brand == "gm" if available != params.get_bool("CarGpsAvailable"): params.put_bool("CarGpsAvailable", available) return available @@ -48,7 +51,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 + # On GM, u-blox is routed into gpsLocation so card can publish the selected + # device/PPS/OnStar result through the existing external interface. + return started and use_ublox and (car_gps is False or CP.brand == "gm") def joystick(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: return started and params.get_bool("JoystickDebugMode") diff --git a/system/manager/test/test_process_config.py b/system/manager/test/test_process_config.py index 2088385bd..2a016077e 100644 --- a/system/manager/test/test_process_config.py +++ b/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, @@ -174,3 +175,14 @@ def test_ublox_has_single_external_gps_publisher(monkeypatch, car_gps, expected) assert ublox(True, params, car.CarParams.new_message(), SimpleNamespace()) is expected assert params.get_bool("CarGpsAvailable") is car_gps + + +def test_gm_routes_device_gps_through_selector(monkeypatch): + monkeypatch.setattr("openpilot.system.manager.process_config.ublox_available", lambda: True) + CP = car.CarParams.new_message() + CP.brand = "gm" + CP.carFingerprint = GM_CAR.CHEVROLET_BOLT_CC_2018_2021 + params = GpsParams(CP) + + assert ublox(True, params, CP, SimpleNamespace()) + assert params.get_bool("CarGpsAvailable") diff --git a/system/timed.py b/system/timed.py index 64fd59252..4d8ed79bc 100644 --- a/system/timed.py +++ b/system/timed.py @@ -63,7 +63,10 @@ def main() -> NoReturn: gps_location_service = get_gps_location_service(params) pm = messaging.PubMaster(['clocks']) - sm = messaging.SubMaster([gps_location_service]) + # Keep both canonical services subscribed. GM card selects between the + # device feed and CAN fallbacks at the external publisher; this lets timed + # follow the GM route when its current CarParams are written. + sm = messaging.SubMaster(['gpsLocation', 'gpsLocationExternal']) # StarPilot variables tf = TimezoneFinder() if TimezoneFinder is not None else None @@ -75,6 +78,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() diff --git a/system/ubloxd/ubloxd.py b/system/ubloxd/ubloxd.py index 6882ad095..96720f60d 100644 --- a/system/ubloxd/ubloxd.py +++ b/system/ubloxd/ubloxd.py @@ -3,11 +3,14 @@ import math import capnp import calendar import numpy as np +import time from collections import defaultdict from dataclasses import dataclass +from cereal import car from cereal import log from cereal import messaging +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,9 +497,20 @@ class UbloxMsgParser: def main(): - parser = UbloxMsgParser() - pm = messaging.PubMaster(['ubloxGnss', 'gpsLocationExternal']) + gps_service = 'gpsLocationExternal' + car_params = Params().get("CarParams") + if car_params is not None: + try: + with car.CarParams.from_bytes(car_params) as CP: + if CP.brand == "gm": + gps_service = 'gpsLocation' + except Exception: + pass + + parser = UbloxMsgParser(gps_service) + pm = messaging.PubMaster(['ubloxGnss', gps_service]) sock = messaging.sub_sock('ubloxRaw', timeout=100, conflate=False) + last_device_gps_publish = 0.0 while True: msg = messaging.recv_one(sock) @@ -513,6 +528,11 @@ def main(): if not res: continue service, dat = res + if service == 'gpsLocation': + now = time.monotonic() + if now - last_device_gps_publish < 0.9: + continue + last_device_gps_publish = now pm.send(service, dat) if __name__ == '__main__':