Nuts

cereal/log.capnp

@@ -736,6 +736,19 @@ struct PeripheralState {
   }
 }
 
+struct ChestnutState {
+  tempC @0 :Float32;
+  memoryTempC @1 :Float32;
+  powerDrawW @2 :Float32;
+  powerLimitW @3 :Float32;
+  gpuUsagePercent @4 :UInt8;
+  gpuClockMhz @5 :UInt16;
+  fanSpeedRpm @6 :UInt16;
+  pcieLtssm @7 :UInt8;
+  supplyVoltage @8 :UInt16;  # mV
+  supplyCurrent @9 :Int16;  # mA
+}
+
 struct RadarState @0x9a185389d6fdd05f {
   mdMonoTime @6 :UInt64;
   carStateMonoTime @11 :UInt64;
@@ -2692,6 +2705,7 @@ struct Event {
     procLog @33 :ProcLog;
     clocks @35 :Clocks;
     deviceState @6 :DeviceState;
+    chestnutState @152 :ChestnutState;
     logMessage @18 :Text;
     errorLogMessage @85 :Text;
 

cereal/services.py

@@ -29,6 +29,7 @@ _services: dict[str, tuple] = {
   "temperatureSensor": (True, 2., 200),
   "gpsNMEA": (True, 9.),
   "deviceState": (True, 2., 1),
+  "chestnutState": (True, 10., 10),
   "touch": (True, 20., 1),
   "can": (True, 100., 2053, QueueSize.BIG),  # decimation gives ~3 msgs in a full segment
   "controlsState": (True, 100., 10, QueueSize.MEDIUM),

selfdrive/controls/lib/longitudinal_planner.py

@@ -21,6 +21,7 @@ from openpilot.selfdrive.controls.lib.lead_follow_policy import is_nonurgent_dup
 from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
   get_far_follow_output_slew_rates,
   get_follow_prebrake_min_headway,
+  get_force_stop_distance_bias,
   get_force_stop_handoff_distance,
   is_gm_silverado_early_follow_lead,
   is_toyota_rav4_tss2_post_departure_tune,
@@ -2181,7 +2182,10 @@ class LongitudinalPlanner:
     force_stop_x = None
     force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint)
     if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m:
-      force_stop_x = float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE
+      force_stop_x = (
+        float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE +
+        get_force_stop_distance_bias(self.CP.carFingerprint)
+      )
 
     self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
                     sm['starpilotPlan'].dangerFactor, effective_t_follow,

selfdrive/controls/lib/longitudinal_vehicle_tunes.py

@@ -28,6 +28,7 @@ TOYOTA_RAV4_TSS2_EARLY_LEAD_MIN_BRAKE = 0.8
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_BRAKE = 2.0
 TOYOTA_RAV4_TSS2_EARLY_LEAD_MAX_DECEL = 0.5
 TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M = 4.5
+TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M = 2.0
 DEFAULT_FORCE_STOP_HANDOFF_M = 6.0
 
 
@@ -170,3 +171,9 @@ def get_force_stop_handoff_distance(car_fingerprint):
   if str(car_fingerprint) == "TOYOTA_CAMRY_TSS2":
     return TOYOTA_CAMRY_TSS2_FORCE_STOP_HANDOFF_M
   return DEFAULT_FORCE_STOP_HANDOFF_M
+
+
+def get_force_stop_distance_bias(car_fingerprint):
+  if str(car_fingerprint) == "TOYOTA_CAMRY_TSS2":
+    return TOYOTA_CAMRY_TSS2_FORCE_STOP_DISTANCE_BIAS_M
+  return 0.0

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -13,7 +13,10 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import (
   get_lead_veto_distance,
   get_slc_lead_drop_relaxed_target,
 )
-from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_force_stop_handoff_distance
+from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
+  get_force_stop_distance_bias,
+  get_force_stop_handoff_distance,
+)
 from types import SimpleNamespace
 
 
@@ -122,6 +125,11 @@ def test_camry_tss2_uses_closer_force_stop_handoff():
   assert get_force_stop_handoff_distance("TOYOTA_RAV4_TSS2") == pytest.approx(6.0)
 
 
+def test_camry_tss2_gets_forward_force_stop_bias_only():
+  assert get_force_stop_distance_bias("TOYOTA_CAMRY_TSS2") == pytest.approx(2.0)
+  assert get_force_stop_distance_bias("TOYOTA_RAV4_TSS2") == pytest.approx(0.0)
+
+
 def test_curve_speed_controller_holds_target_through_brief_detector_dropout():
   planner, vcruise = make_vcruise()
   sm = make_sm(standstill=False)

selfdrive/modeld/helpers.py

@@ -44,15 +44,11 @@ def _fallback_tg_devices(process_name: str, usbgpu: bool) -> dict[str, str]:
   if process_name == "selfdrive.modeld.dmonitoringmodeld":
     return {"DEV": backend}
 
-  queue_dev = backend
-  if usbgpu:
-    try:
-      available = {name.split(":", 1)[0] for name in Device.get_available_devices()}
-    except Exception:
-      available = set()
-    if "AMD" in available:
-      queue_dev = "AMD"
-  return {"WARP_DEV": backend, "QUEUE_DEV": queue_dev}
+  # The external-GPU profile is only selected after Chestnut has been
+  # recognized. Match upstream's generated device map and select AMD directly;
+  # probing every tinygrad backend opens CL/DSP/CPU devices inside modeld and
+  # can interfere with the on-road QCOM + AMD process.
+  return {"WARP_DEV": backend, "QUEUE_DEV": "AMD" if usbgpu else backend}
 
 
 def get_tg_input_devices(process_name: str, usbgpu: bool) -> dict[str, str]:

selfdrive/modeld/modeld.py

@@ -1,8 +1,11 @@
 #!/usr/bin/env python3
+from functools import cached_property
 import os
+import struct
 from openpilot.system.hardware import TICI
 os.environ['GMMU'] = '0'
 os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
+from tinygrad.device import Device
 from tinygrad.tensor import Tensor
 import threading
 import time
@@ -13,6 +16,7 @@ from cereal import car, log
 from pathlib import Path
 from setproctitle import setproctitle
 from cereal.messaging import PubMaster, SubMaster
+from cereal.services import SERVICE_LIST
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
@@ -70,6 +74,15 @@ BIG_MODEL_RUN_WAIT_TIMEOUT_MS = 3000
 LAT_SMOOTH_BP = [2.0, 8.0]
 
 
+def _set_hcq_wait_timeout(timeout_ms: int) -> None:
+  """Update tinygrad's cached HCQ timeout for the external-GPU load/run phase."""
+  os.environ["HCQDEV_WAIT_TIMEOUT_MS"] = str(timeout_ms)
+  # tinygrad.getenv is cached. Updating os.environ alone leaves the first value
+  # in effect for the lifetime of modeld.
+  from tinygrad.helpers import getenv
+  getenv.cache_clear()
+
+
 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]))
 
@@ -80,6 +93,67 @@ def get_car_lateral_smooth_seconds(brand: str, v_ego: float, maximum: float) ->
   return maximum
 
 
+class ChestnutState:
+  """Publish bounded external-GPU and ASM2464 telemetry from modeld."""
+
+  def __init__(self, pm: PubMaster, big: bool):
+    self.pm = pm
+    self.big = big
+    self.valid = True
+    self.sends = 0
+    self.metrics = {}
+
+  @cached_property
+  def power_limit(self) -> int:
+    smu = Device["AMD"].iface.dev_impl.smu
+    return smu._send_msg(smu.smu_mod.PPSMC_MSG_GetPptLimit, 0, read_back_arg=True, timeout=100)
+
+  def send(self) -> None:
+    msg = messaging.new_message("chestnutState")
+    state = msg.chestnutState
+    self.sends += 1
+
+    # SMU metrics are relatively expensive, so update them at 0.1 Hz while
+    # publishing the cached values with the 10 Hz ASM link telemetry.
+    if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1:
+      try:
+        smu = Device["AMD"].iface.dev_impl.smu
+        smu._send_msg(smu.smu_mod.PPSMC_MSG_TransferTableSmu2Dram, smu.smu_mod.TABLE_SMU_METRICS, timeout=100)
+        metrics = smu.read_table(smu.smu_mod.SmuMetricsExternal_t, smu.smu_mod.TABLE_SMU_METRICS).SmuMetrics
+        self.metrics = {
+          "tempC": metrics.AvgTemperature[smu.smu_mod.TEMP_HOTSPOT],
+          "memoryTempC": metrics.AvgTemperature[smu.smu_mod.TEMP_MEM],
+          "powerDrawW": metrics.AverageSocketPower,
+          "powerLimitW": self.power_limit,
+          "gpuUsagePercent": metrics.AverageGfxActivity,
+          "gpuClockMhz": metrics.AverageGfxclkFrequencyPostDs,
+          "fanSpeedRpm": metrics.AvgFanRpm,
+        }
+        self.valid = True
+      except Exception:
+        if self.valid:
+          cloudlog.exception("chestnut state read failed")
+        self.valid = False
+        self.metrics.clear()
+
+    if self.big:
+      for key, value in self.metrics.items():
+        setattr(state, key, value)
+
+    asm_valid = False
+    if "AMD" in Device._opened_devices:
+      try:
+        asm = Device["AMD"].iface.pci_dev.usb
+        state.pcieLtssm = asm.read(0xB450, 1)[0]
+        state.supplyVoltage, state.supplyCurrent = struct.unpack("<Hh", bytes(asm.usb.control_read(0xC0, 5))[:4])
+        asm_valid = True
+      except Exception:
+        pass
+
+    msg.valid = asm_valid and (not self.big or self.valid)
+    self.pm.send("chestnutState", msg)
+
+
 def _get_param_str(params: Params, key: str, default: str = "") -> str:
   try:
     val = params.get(key)
@@ -571,7 +645,7 @@ def main(demo=False):
     # Loading the large artifact competes with the rest of on-road startup.
     # Keep the short watchdog for inference, but allow tinygrad's normal wait
     # while model weights are being streamed into VRAM.
-    os.environ["HCQDEV_WAIT_TIMEOUT_MS"] = str(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
+    _set_hcq_wait_timeout(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
     from tinygrad.helpers import DEV
     device_config = tinygrad_dev_config(True, TICI)
     DEV.value = device_config
@@ -629,7 +703,7 @@ def main(demo=False):
     loader = threading.Thread(target=load_big_model, name="big_model_loader", daemon=True)
     loader.start()
     loader.join(BIG_MODEL_TIMEOUT)
-    os.environ["HCQDEV_WAIT_TIMEOUT_MS"] = str(BIG_MODEL_RUN_WAIT_TIMEOUT_MS)
+    _set_hcq_wait_timeout(BIG_MODEL_RUN_WAIT_TIMEOUT_MS)
     if loader.is_alive():
       cloudlog.error(f"external GPU model load timed out after {BIG_MODEL_TIMEOUT}s")
     model = big_model
@@ -651,10 +725,14 @@ def main(demo=False):
   cloudlog.warning(f"model loaded in {time.monotonic() - start_time:.1f}s, modeld starting")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"])
+  publish_services = ["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"]
+  if external_gpu_requested:
+    publish_services.append("chestnutState")
+  pm = PubMaster(publish_services)
   sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "starpilotPlan"])
 
   publish_state = PublishState()
+  chestnut_state = ChestnutState(pm, external_gpu_active) if external_gpu_requested else None
   # setup filter to track dropped frames
   frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
   frame_id = 0
@@ -809,6 +887,8 @@ def main(demo=False):
       params.put_bool("UsbGpuActive", False)
       model = small_model
       external_gpu_active = False
+      if chestnut_state is not None:
+        chestnut_state.big = False
       run_count = 0
       model_output = None
     mt2 = time.perf_counter()
@@ -857,6 +937,9 @@ def main(demo=False):
     if sm.updated['starpilotPlan']:
       starpilot_toggles = get_starpilot_toggles(sm)
 
+    if chestnut_state is not None and run_count % round(ModelConstants.MODEL_FREQ / SERVICE_LIST["chestnutState"].frequency) == 0:
+      chestnut_state.send()
+
 if __name__ == "__main__":
   try:
     import argparse

selfdrive/modeld/tests/test_usbgpu_helpers.py

@@ -1,5 +1,6 @@
 import io
 from types import MethodType
+from types import SimpleNamespace
 
 import numpy as np
 
@@ -14,11 +15,66 @@ def test_external_gpu_keeps_the_native_device_available():
   assert tinygrad_dev_config(True, tici=False) == "CPU:LLVM;USB+AMD:LLVM"
 
 
+def test_external_gpu_selects_amd_without_probing_other_backends(monkeypatch, tmp_path):
+  from openpilot.selfdrive.modeld import helpers
+
+  monkeypatch.setattr(helpers, "TG_INPUT_DEVICES_PATH", tmp_path / "missing.json")
+  monkeypatch.setattr(helpers, "_default_tinygrad_backend", lambda: "QCOM")
+  monkeypatch.setattr(
+    helpers.Device,
+    "get_available_devices",
+    lambda: (_ for _ in ()).throw(AssertionError("must not probe every tinygrad backend")),
+  )
+
+  assert helpers.get_tg_input_devices("selfdrive.modeld.modeld", usbgpu=True) == {
+    "WARP_DEV": "QCOM",
+    "QUEUE_DEV": "AMD",
+  }
+
+
 def test_external_gpu_uses_a_longer_load_watchdog():
   assert modeld.BIG_MODEL_LOAD_WAIT_TIMEOUT_MS == 30000
   assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
 
 
+def test_external_gpu_wait_timeout_updates_tinygrad_cache(monkeypatch):
+  from tinygrad.helpers import getenv
+
+  try:
+    monkeypatch.setenv("HCQDEV_WAIT_TIMEOUT_MS", "30000")
+    getenv.cache_clear()
+    assert getenv("HCQDEV_WAIT_TIMEOUT_MS", 0) == 30000
+
+    modeld._set_hcq_wait_timeout(3000)
+    assert getenv("HCQDEV_WAIT_TIMEOUT_MS", 0) == 3000
+  finally:
+    getenv.cache_clear()
+
+
+def test_chestnut_telemetry_is_bounded_when_amd_is_unavailable(monkeypatch):
+  from cereal.services import SERVICE_LIST
+
+  class FakePubMaster:
+    def __init__(self):
+      self.sent = []
+
+    def send(self, service, message):
+      self.sent.append((service, message))
+
+  publisher = FakePubMaster()
+  monkeypatch.setattr(modeld, "Device", SimpleNamespace(_opened_devices=set()))
+
+  telemetry = modeld.ChestnutState(publisher, big=True)
+  telemetry.send()
+
+  assert SERVICE_LIST["chestnutState"].frequency == 10.0
+  assert len(publisher.sent) == 1
+  service, message = publisher.sent[0]
+  assert service == "chestnutState"
+  assert message.which() == "chestnutState"
+  assert not message.valid
+
+
 def test_tinygrad_disk_cache_connection_is_closed_before_thread_handoff(monkeypatch):
   import tinygrad.helpers as tinygrad_helpers
 

starpilot/controls/lib/starpilot_vcruise.py

@@ -8,7 +8,10 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED
 from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedController, is_manual_speed_control
 from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController
-from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_force_stop_handoff_distance
+from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import (
+  get_force_stop_distance_bias,
+  get_force_stop_handoff_distance,
+)
 
 CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS
 CSC_CURVE_RELEASE_HOLD_TIME = 0.75
@@ -320,6 +323,9 @@ class StarPilotVCruise:
     force_stop_handoff_m = get_force_stop_handoff_distance(
       getattr(starpilot_toggles, "car_model", "")
     )
+    force_stop_distance_bias_m = get_force_stop_distance_bias(
+      getattr(starpilot_toggles, "car_model", "")
+    )
 
     raw_stop_seen = bool(
       self.starpilot_planner.starpilot_cem.stop_light_detected
@@ -615,7 +621,7 @@ class StarPilotVCruise:
 
         # Kinematic profile with user offset. Positive offset shifts the perceived
         # line further down the road -> car rolls further before commanding 0.
-        effective_d = self.tracked_model_length + offset_m
+        effective_d = self.tracked_model_length + offset_m + force_stop_distance_bias_m
         if effective_d <= force_stop_handoff_m:
           v_target = 0.0
         else:
@@ -676,7 +682,7 @@ class StarPilotVCruise:
         adjacent_stop_d = self._get_adjacent_stop_distance(sm)
         if adjacent_stop_d is not None:
           approach_d = min(approach_d, adjacent_stop_d)
-        approach_d += offset_m
+        approach_d += offset_m + force_stop_distance_bias_m
         if approach_d > force_stop_handoff_m:
           targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m)))