diff --git a/common/gps.py b/common/gps.py index 4d1a95c6a..d9e91b546 100644 --- a/common/gps.py +++ b/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" diff --git a/opendbc_repo/opendbc/car/gm/carstate.py b/opendbc_repo/opendbc/car/gm/carstate.py index 9e0f3c122..19afac654 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 @@ -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 diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index ca34e609c..2e91a556b 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/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", diff --git a/opendbc_repo/opendbc/car/gps.py b/opendbc_repo/opendbc/car/gps.py index 750d9ebf8..b507583f7 100644 --- a/opendbc_repo/opendbc/car/gps.py +++ b/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: diff --git a/scripts/host_tool_runner.sh b/scripts/host_tool_runner.sh index e4537d83d..aa5dc5b81 100755 --- a/scripts/host_tool_runner.sh +++ b/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 \ diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8e761c203..1b27cab2c 100644 --- a/selfdrive/car/card.py +++ b/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: diff --git a/selfdrive/car/tests/test_gm_gps.py b/selfdrive/car/tests/test_gm_gps.py new file mode 100644 index 000000000..5352a279b --- /dev/null +++ b/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 diff --git a/selfdrive/controls/tests/test_starpilot_planner.py b/selfdrive/controls/tests/test_starpilot_planner.py index df00fd98d..0228d1331 100644 --- a/selfdrive/controls/tests/test_starpilot_planner.py +++ b/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() + diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index bf40fa3ff..5e443969d 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/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 = { diff --git a/starpilot/starpilot_process.py b/starpilot/starpilot_process.py index 0db249cac..33acf60ef 100644 --- a/starpilot/starpilot_process.py +++ b/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() diff --git a/starpilot/system/speed_limit_filler.py b/starpilot/system/speed_limit_filler.py index f172c697a..6ba0ea56d 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([ + "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 diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 7f8a16213..7d7fd9fda 100644 --- a/system/manager/process_config.py +++ b/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") diff --git a/system/manager/test/test_process_config.py b/system/manager/test/test_process_config.py index 2088385bd..883d7da94 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, @@ -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()) diff --git a/system/timed.py b/system/timed.py index 64fd59252..550a2f035 100644 --- a/system/timed.py +++ b/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() diff --git a/system/ubloxd/tests/test_gps_routing.py b/system/ubloxd/tests/test_gps_routing.py new file mode 100644 index 000000000..f5af7fb5d --- /dev/null +++ b/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] diff --git a/system/ubloxd/ubloxd.py b/system/ubloxd/ubloxd.py index 6882ad095..fc8a3b50e 100644 --- a/system/ubloxd/ubloxd.py +++ b/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: