DRV ECU

selfdrive/modeld/modeld.py

@@ -4,7 +4,7 @@ import ctypes
 from functools import cached_property
 import os
 import struct
-from openpilot.system.hardware import TICI
+from openpilot.system.hardware import HARDWARE, TICI
 os.environ['GMMU'] = '0'
 os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
 from tinygrad.device import Device
@@ -77,6 +77,10 @@ def _should_publish_model_output(model_output, vipc_dropped_frames: int, externa
 MIN_LAT_CONTROL_SPEED = 0.3
 BIG_MODEL_LOAD_WAIT_TIMEOUT_MS = 30000
 BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000
+EXTERNAL_GPU_POWER_READY_MV = 13000
+EXTERNAL_GPU_EGMP_READY_MV = 12500
+EXTERNAL_GPU_POWER_STABLE_SECONDS = 3.0
+EXTERNAL_GPU_POWER_LOG_INTERVAL_SECONDS = 10.0
 LAT_SMOOTH_BP = [2.0, 8.0]
 
 
@@ -89,6 +93,86 @@ def _set_hcq_wait_timeout(timeout_ms: int) -> None:
   getenv.cache_clear()
 
 
+def _external_gpu_power_voltage(device_type: str, panda_states, peripheral_state) -> int | None:
+  if device_type == "tici":
+    voltage = int(peripheral_state.voltage)
+    return voltage if peripheral_state.pandaType != log.PandaState.PandaType.unknown and voltage > 0 else None
+
+  voltages = [
+    int(state.voltage) for state in panda_states
+    if state.pandaType != log.PandaState.PandaType.unknown and int(state.voltage) > 0
+  ]
+  return max(voltages, default=None)
+
+
+def _external_gpu_power_ready(voltage: int | None, now: float, stable_since: float | None,
+                              minimum_voltage: int = EXTERNAL_GPU_POWER_READY_MV) -> tuple[bool, float | None]:
+  if voltage is None or voltage < minimum_voltage:
+    return False, None
+
+  stable_since = now if stable_since is None else stable_since
+  return now - stable_since >= EXTERNAL_GPU_POWER_STABLE_SECONDS, stable_since
+
+
+def _egmp_ready_bus(CP) -> int | None:
+  if CP is None or CP.brand != "hyundai":
+    return None
+
+  # These platforms use the accessory-mode ECU-disable startup sequence.
+  from opendbc.car.hyundai.hyundaicanfd import CanBus
+  from opendbc.car.hyundai.values import CAR
+  if CP.carFingerprint not in (CAR.HYUNDAI_IONIQ_5_PE, CAR.HYUNDAI_IONIQ_6, CAR.KIA_EV9):
+    return None
+  return CanBus(CP).ECAN
+
+
+def _egmp_vehicle_ready(can_messages, bus: int) -> bool:
+  return any(
+    msg.address == 0x35 and msg.src == bus and len(msg.dat) > 3 and bytes(msg.dat)[3] & 0x40
+    for msg in can_messages
+  )
+
+
+def wait_for_external_gpu_power_ready(CP=None) -> None:
+  """Wait out vehicle startup power transitions before initializing Chestnut."""
+  device_type = HARDWARE.get_device_type()
+  egmp_bus = _egmp_ready_bus(CP)
+  services = ["pandaStates", "peripheralState"] + (["can"] if egmp_bus is not None else [])
+  sm = SubMaster(services)
+  vehicle_ready = egmp_bus is None
+  stable_since = None
+  last_log = 0.0
+
+  while True:
+    sm.update(1000)
+    now = time.monotonic()
+    if egmp_bus is not None and sm.updated["can"] and _egmp_vehicle_ready(sm["can"], egmp_bus):
+      if not vehicle_ready:
+        cloudlog.warning("e-GMP vehicle entered READY; waiting for external GPU power to stabilize")
+      vehicle_ready = True
+
+    voltage = _external_gpu_power_voltage(device_type, sm["pandaStates"], sm["peripheralState"])
+    minimum_voltage = EXTERNAL_GPU_EGMP_READY_MV if egmp_bus is not None else EXTERNAL_GPU_POWER_READY_MV
+    ready, stable_since = _external_gpu_power_ready(
+      voltage,
+      now,
+      stable_since if vehicle_ready else None,
+      minimum_voltage,
+    )
+    if vehicle_ready and ready:
+      cloudlog.warning(f"vehicle power stable at {voltage / 1000:.2f} V; starting external GPU load")
+      return
+
+    if now - last_log >= EXTERNAL_GPU_POWER_LOG_INTERVAL_SECONDS:
+      detail = "unavailable" if voltage is None else f"{voltage / 1000:.2f} V"
+      if not vehicle_ready:
+        cloudlog.warning(f"external GPU load deferred: vehicle power is {detail}; waiting for e-GMP READY")
+      else:
+        cloudlog.warning(f"external GPU load deferred: vehicle power is {detail}; waiting for " +
+                         f"{minimum_voltage / 1000:.1f} V to remain stable")
+      last_log = now
+
+
 def get_lateral_smooth_seconds(v_ego: float, maximum: float = 0.0) -> float:
   return float(np.interp(v_ego, LAT_SMOOTH_BP, [maximum, 0.0]))
 
@@ -629,10 +713,13 @@ def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_
     return ModelState(cam_w, cam_h, False)
 
 
-def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str) -> ModelState | None:
+def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str,
+                             CP=None, demo: bool = False) -> ModelState | None:
   """Load and warm the USB-GPU model without running another tinygrad model concurrently."""
   candidate = None
   try:
+    if not demo:
+      wait_for_external_gpu_power_ready(CP)
     _set_hcq_wait_timeout(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
     wait_usbgpu_link()
     candidate = ModelState(
@@ -702,11 +789,19 @@ def main(demo=False):
   model = None
   small_model = None
   big_model = None
+  CP = None
   if external_gpu_requested:
+    if demo:
+      CP = get_demo_car_params()
+    else:
+      CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+
     big_model = _load_external_gpu_model(
       vipc_client_main.width,
       vipc_client_main.height,
       selected_model,
+      CP,
+      demo,
     )
 
     small_model = ModelState(
@@ -754,10 +849,11 @@ def main(demo=False):
   camera_offset.set_target(params.get_float("CameraOffset", return_default=True))
 
 
-  if demo:
-    CP = get_demo_car_params()
-  else:
-    CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
+  if CP is None:
+    if demo:
+      CP = get_demo_car_params()
+    else:
+      CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
   cloudlog.info("modeld got CarParams: %s", CP.brand)
 
   lat_smooth_seconds = _model_smooth_seconds(params, "LatSmoothSeconds", LAT_SMOOTH_SECONDS)

selfdrive/modeld/tests/test_usbgpu_helpers.py

@@ -38,6 +38,51 @@ def test_external_gpu_uses_a_longer_load_watchdog():
   assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
 
 
+def test_external_gpu_voltage_uses_hardware_specific_source():
+  panda_type = modeld.log.PandaState.PandaType
+  panda_states = [SimpleNamespace(pandaType=panda_type.dos, voltage=230)]
+  peripheral_state = SimpleNamespace(pandaType=panda_type.dos, voltage=13550)
+
+  assert modeld._external_gpu_power_voltage("tici", panda_states, peripheral_state) == 13550
+
+  panda_states = [SimpleNamespace(pandaType=panda_type.tres, voltage=14100)]
+  peripheral_state = SimpleNamespace(pandaType=panda_type.tres, voltage=12800)
+  assert modeld._external_gpu_power_voltage("tizi", panda_states, peripheral_state) == 14100
+
+  panda_states = [SimpleNamespace(pandaType=panda_type.cuatro, voltage=13200)]
+  peripheral_state = SimpleNamespace(pandaType=panda_type.cuatro, voltage=12800)
+  assert modeld._external_gpu_power_voltage("mici", panda_states, peripheral_state) == 13200
+
+
+def test_external_gpu_power_must_remain_stable():
+  ready, stable_since = modeld._external_gpu_power_ready(12800, 10.0, None)
+  assert not ready
+  assert stable_since is None
+
+  ready, stable_since = modeld._external_gpu_power_ready(14100, 11.0, stable_since)
+  assert not ready
+  assert stable_since == 11.0
+
+  ready, stable_since = modeld._external_gpu_power_ready(14100, 13.9, stable_since)
+  assert not ready
+  ready, stable_since = modeld._external_gpu_power_ready(14100, 14.0, stable_since)
+  assert ready
+
+  ready, stable_since = modeld._external_gpu_power_ready(11900, 15.0, stable_since)
+  assert not ready
+  assert stable_since is None
+
+
+def test_egmp_ready_uses_accelerator_ready_bit():
+  bus = 1
+  not_ready = SimpleNamespace(address=0x35, src=bus, dat=bytes([0, 0, 0, 0x00]))
+  wrong_bus = SimpleNamespace(address=0x35, src=0, dat=bytes([0, 0, 0, 0x40]))
+  ready = SimpleNamespace(address=0x35, src=bus, dat=bytes([0, 0, 0, 0x40]))
+
+  assert not modeld._egmp_vehicle_ready([not_ready, wrong_bus], bus)
+  assert modeld._egmp_vehicle_ready([not_ready, ready], bus)
+
+
 def test_external_gpu_signal_wait_yields_between_usb_polls(monkeypatch):
   from tinygrad.runtime import ops_amd
 
@@ -139,6 +184,7 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
       calls.append("warmup")
 
   monkeypatch.setattr(modeld, "wait_usbgpu_link", lambda: calls.append("link"))
+  monkeypatch.setattr(modeld, "wait_for_external_gpu_power_ready", lambda CP: calls.append(("power", CP)))
   monkeypatch.setattr(modeld, "_set_hcq_wait_timeout", lambda timeout: calls.append(("timeout", timeout)))
   monkeypatch.setattr(modeld, "_close_tinygrad_disk_cache_connection", lambda: calls.append("close_cache"))
   monkeypatch.setattr(modeld, "ModelState", FakeModelState)
@@ -148,10 +194,11 @@ def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
     lambda *_args: (_ for _ in ()).throw(AssertionError("runtime must not change tinygrad's process-global DEV")),
   )
 
-  loaded = modeld._load_external_gpu_model(1928, 1208, "big-model")
+  loaded = modeld._load_external_gpu_model(1928, 1208, "big-model", "car-params")
 
   assert isinstance(loaded, FakeModelState)
   assert calls == [
+    ("power", "car-params"),
     ("timeout", modeld.BIG_MODEL_LOAD_WAIT_TIMEOUT_MS),
     "link",
     ("model", 1928, 1208, True, "big-model", False),