From 230ed14ece8d3eeb000e0e082ead8a9c11fbfb80 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 16 Aug 2026 17:03:51 -0500 Subject: [PATCH] Nuts --- cereal/log.capnp | 14 +++ cereal/services.py | 1 + .../controls/lib/longitudinal_planner.py | 6 +- .../lib/longitudinal_vehicle_tunes.py | 7 ++ .../controls/tests/test_starpilot_vcruise.py | 10 ++- selfdrive/modeld/helpers.py | 14 ++- selfdrive/modeld/modeld.py | 89 ++++++++++++++++++- selfdrive/modeld/tests/test_usbgpu_helpers.py | 56 ++++++++++++ starpilot/controls/lib/starpilot_vcruise.py | 12 ++- 9 files changed, 192 insertions(+), 17 deletions(-) diff --git a/cereal/log.capnp b/cereal/log.capnp index 4f12e4cdf..38b20bb1b 100644 --- a/cereal/log.capnp +++ b/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; diff --git a/cereal/services.py b/cereal/services.py index 797b6cf27..01593ed66 100755 --- a/cereal/services.py +++ b/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), diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b292ffd85..d47946070 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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, diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index 731a10ed4..865a281d6 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/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 diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index cab80192c..a5c9b9b98 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/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) diff --git a/selfdrive/modeld/helpers.py b/selfdrive/modeld/helpers.py index 20d0706c7..d508aa691 100644 --- a/selfdrive/modeld/helpers.py +++ b/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]: diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 3df17ec62..bf84fd7cb 100755 --- a/selfdrive/modeld/modeld.py +++ b/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(" 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 diff --git a/selfdrive/modeld/tests/test_usbgpu_helpers.py b/selfdrive/modeld/tests/test_usbgpu_helpers.py index 04808d610..47f8e20d8 100644 --- a/selfdrive/modeld/tests/test_usbgpu_helpers.py +++ b/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 diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index e92d4b442..00bb79a70 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/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)))