From 7d5596d5c3527f274a3615181f52095909ffb8e9 Mon Sep 17 00:00:00 2001 From: Daniel Koepping Date: Tue, 1 Sep 2026 05:59:02 -0700 Subject: [PATCH] monitor chestnut USB in hardwared (#38741) hardwared: monitor chestnut USB independently --- openpilot/cereal/log.capnp | 1 + openpilot/cereal/services.py | 1 + openpilot/selfdrive/modeld/modeld.py | 67 ++-------- .../system/hardware/chestnut/monitoring.py | 124 ++++++++++++++++++ openpilot/system/hardware/chestnut/status.py | 37 ++++-- openpilot/system/hardware/hardwared.py | 25 +++- 6 files changed, 180 insertions(+), 75 deletions(-) create mode 100644 openpilot/system/hardware/chestnut/monitoring.py diff --git a/openpilot/cereal/log.capnp b/openpilot/cereal/log.capnp index fb4867b2a..9ca1092e8 100644 --- a/openpilot/cereal/log.capnp +++ b/openpilot/cereal/log.capnp @@ -2593,6 +2593,7 @@ struct Event { clocks @35 :Clocks; deviceState @6 :DeviceState; chestnutState @152 :ChestnutState; + chestnutGpuState @153 :ChestnutState; logMessage @18 :Text; errorLogMessage @85 :Text; diff --git a/openpilot/cereal/services.py b/openpilot/cereal/services.py index 08633d697..f906e4610 100755 --- a/openpilot/cereal/services.py +++ b/openpilot/cereal/services.py @@ -26,6 +26,7 @@ _services: dict[str, tuple] = { "temperatureSensor": (True, 2., 200), "deviceState": (True, 2., 1), "chestnutState": (True, 10., 10), + "chestnutGpuState": (False, 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/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index a795f366c..1178349c4 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -6,8 +6,6 @@ import os os.environ['GMMU'] = '0' # for chestnut fast loading, noop for qcom from tinygrad.tensor import Tensor from tinygrad.device import Device -import usb1 -import struct import threading import time import numpy as np @@ -32,7 +30,6 @@ from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.compile_modeld import make_input_queues, WARP_INPUTS, POLICY_INPUTS from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState from openpilot.common.file_chunker import open_file_chunked -from openpilot.common.hardware.usb import CHESTNUT_USB_IDS from openpilot.selfdrive.modeld.constants import ModelConstants, Plan from openpilot.selfdrive.modeld.helpers import chestnut_present, chestnut_compiled, modeld_pkl_path, get_tg_input_devices, load_oob @@ -73,45 +70,14 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log. shouldStop=bool(stop)) -class ChestnutState: - # only modeld can access chestnut +class ChestnutGpuState: + # SMU metrics are only accessible from modeld. def __init__(self, pm: PubMaster, big: bool): self.pm = pm self.big = big self.valid = True self.sends = 0 self.metrics = {} - self._asm_usb = None - - def _close_asm_usb(self) -> None: - if self._asm_usb is not None: - self._asm_usb.close() - self._asm_usb = None - - def _open_asm_usb(self): - context = usb1.USBContext() - for vendor_id, product_id in CHESTNUT_USB_IDS: - if (handle := context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True)) is not None: - return handle - context.close() - - def _read_ina(self) -> tuple[int, int, bool]: - if "AMD" in Device._opened_devices and self._asm_usb is None: - try: - raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5) - return struct.unpack(' int: @@ -119,8 +85,6 @@ class ChestnutState: 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 if self.big and "AMD" in Device._opened_devices and self.sends % 100 == 1: try: @@ -142,25 +106,14 @@ class ChestnutState: cloudlog.exception("chestnut state read failed") self.valid = False self.metrics.clear() + + msg = messaging.new_message('chestnutGpuState') + state = msg.chestnutGpuState if self.big: for k, v in self.metrics.items(): setattr(state, k, v) - - asm_valid = False - try: - # ASM runs on USB-C power, these still read without a gpu - state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina() - asm_valid = True - except Exception: - pass - if "AMD" in Device._opened_devices: - try: - state.pcieLtssm = Device["AMD"].iface.pci_dev.usb.read(0xB450, 1)[0] - except Exception: - pass - - msg.valid = asm_valid and (not self.big or self.valid) - self.pm.send('chestnutState', msg) + msg.valid = self.big and self.valid + self.pm.send('chestnutGpuState', msg) class FrameMeta: @@ -310,13 +263,13 @@ def main(demo=False): cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting") # messaging - pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutState"] if CHESTNUT else []) + pub_socks = ["modelV2", "drivingModelData", "cameraOdometry"] + (["chestnutGpuState"] if CHESTNUT else []) pm = PubMaster(pub_socks) sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState", "carControl", "lateralDelay"]) publish_state = PublishState() params = Params() - chestnut_state = ChestnutState(pm, model.chestnut) if CHESTNUT else None + chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None # setup filter to track dropped frames frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ) @@ -425,7 +378,7 @@ def main(demo=False): mt1 = time.perf_counter() try: send_chestnut = (chestnut_state is not None and - run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0) + run_count % round(ModelConstants.MODEL_RUN_FREQ / SERVICE_LIST['chestnutGpuState'].frequency) == 0) model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None) except Exception: if not params.get_bool("ChestnutActive"): diff --git a/openpilot/system/hardware/chestnut/monitoring.py b/openpilot/system/hardware/chestnut/monitoring.py new file mode 100644 index 000000000..829db9961 --- /dev/null +++ b/openpilot/system/hardware/chestnut/monitoring.py @@ -0,0 +1,124 @@ +import struct +from contextlib import suppress + +import usb1 + +import openpilot.cereal.messaging as messaging +from openpilot.cereal.services import SERVICE_LIST +from openpilot.common.hardware.usb import CHESTNUT_USB_IDS + + +USB_TIMEOUT_MS = 100 +PCIE_LTSSM_ADDRESS = 0xB450 + + +class ChestnutUsb: + def __init__(self): + self.context: usb1.USBContext | None = None + self.handle = None + + def close(self) -> None: + handle, context = self.handle, self.context + self.handle = None + self.context = None + with suppress(Exception): + if handle is not None: + handle.close() + with suppress(Exception): + if context is not None: + context.close() + + def connect(self) -> bool: + if self.handle is not None: + return True + + context = usb1.USBContext() + for vendor_id, product_id in CHESTNUT_USB_IDS: + handle = context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True) + if handle is not None: + self.context = context + self.handle = handle + return True + context.close() + return False + + def _read(self, request: int, value: int, length: int) -> bytes: + if self.handle is None: + raise usb1.USBErrorNoDevice + raw = bytes(self.handle.controlRead(0xC0, request, value, 0, length, timeout=USB_TIMEOUT_MS)) + if len(raw) != length: + raise ValueError(f"short chestnut USB response: {len(raw)}/{length}") + return raw + + def read_ina(self) -> tuple[int, int, bool]: + return struct.unpack(' int: + return self._read(0xE4, PCIE_LTSSM_ADDRESS, 1)[0] + + +class ChestnutMonitoring: + def __init__(self, usb: ChestnutUsb | None = None): + self.usb = usb or ChestnutUsb() + self.gpu_state = None + self.seen = False + self.enabled = False + self.usb_failed = False + + def set_enabled(self, enabled: bool) -> None: + if self.enabled == enabled: + return + self.enabled = enabled + self.usb.close() + self.usb_failed = False + + def retry(self) -> None: + self.usb_failed = False + + def model_alive(self, sm: messaging.SubMaster, now: float) -> bool: + modeld = next((p for p in sm['managerState'].processes if p.name == 'modeld'), None) + if modeld is not None and modeld.shouldBeRunning and not modeld.running: + return False + recv_time = sm.recv_time['chestnutGpuState'] + return recv_time > 0. and now - recv_time < 10. / SERVICE_LIST['chestnutGpuState'].frequency + + def update_gpu_state(self, sm: messaging.SubMaster, now: float) -> None: + if sm.updated['chestnutGpuState']: + self.gpu_state = sm['chestnutGpuState'] if sm.valid['chestnutGpuState'] else None + elif not self.model_alive(sm, now): + self.gpu_state = None + + def update(self, sm: messaging.SubMaster, now: float, model_loading: bool = False): + self.update_gpu_state(sm, now) + return self.build_message(model_loading) + + def build_message(self, model_loading: bool = False): + if not self.enabled: + return None + + msg = messaging.new_message('chestnutState') + if self.gpu_state is not None: + msg.chestnutState = self.gpu_state + state = msg.chestnutState + + if self.usb_failed: + return msg if self.seen else None + + try: + if not self.usb.connect(): + self.usb_failed = True + return msg if self.seen else None + + self.seen = True + voltage, current, fault = self.usb.read_ina() + pcie_ltssm = self.usb.read_pcie_ltssm() + state.supplyVoltage = voltage + state.supplyCurrent = current + state.supplyFault = fault + state.pcieLtssm = pcie_ltssm + msg.valid = True + except Exception as e: + if not model_loading or not isinstance(e, usb1.USBErrorTimeout): + self.usb.close() + self.usb_failed = True + return msg diff --git a/openpilot/system/hardware/chestnut/status.py b/openpilot/system/hardware/chestnut/status.py index c3321a397..34456b2c1 100644 --- a/openpilot/system/hardware/chestnut/status.py +++ b/openpilot/system/hardware/chestnut/status.py @@ -21,14 +21,13 @@ class ChestnutStatus: self.power_lost = False self.power_restored = False self.link_failures = 0 - self.model_loading_seen = False self.model_attempted = False self.overheated = False self.usb_seen = False self.usb_failed = False def update(self, offroad: bool, branch: str, usb_state: list[dict], firmware_failed: bool, - model_loading: bool, model_active: bool | None, state, set_alert) -> None: + model_active: bool | None, state, usb_failed: bool, set_alert) -> None: detected = [d for d in usb_state if is_chestnut_usb_id(d["vendorId"], d["productId"], include_bootloader=True)] devices = [d for d in detected if is_chestnut_usb_id(d["vendorId"], d["productId"])] firmware_ok = len(devices) == 1 and devices[0]["product"] == CHESTNUT_USB_PRODUCT @@ -40,16 +39,15 @@ class ChestnutStatus: self.power_lost = False self.power_restored = False self.link_failures = 0 - self.model_loading_seen = False self.model_attempted = False self.usb_seen = firmware_ok self.usb_failed = False - self.model_loading_seen |= model_loading - self.model_attempted |= self.model_loading_seen and not model_loading and model_active is not None + self.model_attempted |= model_active is not None - if not offroad and self.usb_seen and not firmware_ok: - self.usb_failed = True + if not offroad: + self.usb_seen |= firmware_ok + self.usb_failed = not offroad and self.usb_seen and (not firmware_ok or usb_failed) if not offroad and state is not None: powered = state.supplyVoltage >= CHESTNUT_POWERED_VOLTAGE @@ -80,17 +78,28 @@ class ChestnutStatus: release = branch in CHESTNUT_RELEASE_BRANCHES missing = self.usb_failed or (offroad and release and time.monotonic() - self.started > 10. and len(detected) != 1) slow_usb = offroad and len(devices) == 1 and devices[0]["speedMbps"] < 5000 - set_alert("Offroad_ChestnutBranch", not release and len(devices) == 1) - set_alert("Offroad_ChestnutNotDetected", missing) - set_alert("Offroad_ChestnutOverheated", self.overheated, f"{state.tempC:.0f} °C" if state is not None else None) - set_alert("Offroad_ChestnutUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None) + update_failed = offroad and firmware_failed + compiled = firmware_ok and chestnut_compiled() + uncompiled = offroad and firmware_ok and not compiled + if self.power_lost: pcie_alert = ("Chestnut power restored. 12V is stable again, cycle ignition." if self.power_restored else "Chestnut power disconnected. Check 12V connection, then cycle ignition." if self.power_unavailable else "Chestnut power lost. Possibly caused by an engine-crank voltage drop. Check 12V connection, then cycle ignition.") else: pcie_alert = "Chestnut GPU unavailable. PCIe link is not up. Check the GPU is securely seated." - set_alert("Offroad_ChestnutPcieUnavailable", self.pcie_failed, pcie_alert) - set_alert("Offroad_ChestnutUncompiled", offroad and firmware_ok and not chestnut_compiled()) - set_alert("Offroad_ChestnutUpdateFailed", offroad and firmware_failed) + + alerts = ( + ("Offroad_ChestnutNotDetected", missing, None), + ("Offroad_ChestnutUpdateFailed", update_failed, None), + ("Offroad_ChestnutUncompiled", uncompiled, None), + ("Offroad_ChestnutPcieUnavailable", self.pcie_failed, pcie_alert), + ("Offroad_ChestnutOverheated", self.overheated, f"{state.tempC:.0f} °C" if state is not None else None), + ("Offroad_ChestnutUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None), + ) + active_alert = next((name for name, active, _ in alerts if active), None) + + set_alert("Offroad_ChestnutBranch", not release and len(devices) == 1 and not missing) + for name, _, extra_text in alerts: + set_alert(name, name == active_alert, extra_text) self.offroad = offroad diff --git a/openpilot/system/hardware/hardwared.py b/openpilot/system/hardware/hardwared.py index 11fe41400..c7d11fa5b 100755 --- a/openpilot/system/hardware/hardwared.py +++ b/openpilot/system/hardware/hardwared.py @@ -27,6 +27,7 @@ from openpilot.common.swaglog import cloudlog from openpilot.system.hardware.power_monitoring import PowerMonitoring from openpilot.system.hardware.fan_controller import FanController from openpilot.system.hardware.chestnut.status import ChestnutStatus +from openpilot.system.hardware.chestnut.monitoring import ChestnutMonitoring from openpilot.common.version import terms_version, training_version from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID @@ -195,8 +196,9 @@ def hw_state_thread(end_event, hw_queue): def hardware_thread(end_event, hw_queue) -> None: system_stats = LinuxSystemStats() - pm = messaging.PubMaster(['deviceState']) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "chestnutState"], poll="pandaStates") + pm = messaging.PubMaster(['deviceState', 'chestnutState']) + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", + "chestnutState", "chestnutGpuState", "managerState"], poll="pandaStates") count = 0 @@ -244,7 +246,9 @@ def hardware_thread(end_event, hw_queue) -> None: fan_controller = FanController(int(1./DT_HW)) chestnut = Chestnut() + chestnut_monitoring = ChestnutMonitoring() chestnut_status = ChestnutStatus() + model_loading = params.get_bool("ChestnutLoading") branch = get_short_branch() while not end_event.is_set(): @@ -276,6 +280,8 @@ def hardware_thread(end_event, hw_queue) -> None: # Run at 2Hz, plus either edge of ignition ign_edge = (started_ts is not None) != all(onroad_conditions.values()) if (sm.frame % round(SERVICE_LIST['pandaStates'].frequency * DT_HW) != 0) and not ign_edge: + if (chestnut_msg := chestnut_monitoring.update(sm, time.monotonic(), model_loading)) is not None: + pm.send('chestnutState', chestnut_msg) continue msg = messaging.new_message('deviceState', valid=True) @@ -309,9 +315,11 @@ def hardware_thread(end_event, hw_queue) -> None: chestnut.update(started_ts is None, last_hw_state.usb_state) chestnut_state = sm["chestnutState"] chestnut_valid = sm.alive["chestnutState"] and sm.valid["chestnutState"] + model_loading = params.get_bool("ChestnutLoading") + model_active = params.get("ChestnutActive") chestnut_status.update(started_ts is None, branch, last_hw_state.usb_state, chestnut.failed, - params.get_bool("ChestnutLoading"), params.get("ChestnutActive"), - chestnut_state if chestnut_valid else None, set_offroad_alert_if_changed) + model_active, chestnut_state if chestnut_valid else None, chestnut_monitoring.usb_failed, + set_offroad_alert_if_changed) # this subset is only used for offroad temp_sources = [ msg.deviceState.memoryTempC, @@ -419,6 +427,15 @@ def hardware_thread(end_event, hw_queue) -> None: if off_ts is None: off_ts = time.monotonic() + chestnut_usb_ready = any(is_chestnut_usb_id(d["vendorId"], d["productId"]) and d["product"] == CHESTNUT_USB_PRODUCT + for d in last_hw_state.usb_state) + flash_active = chestnut.thread is not None and chestnut.thread.is_alive() + chestnut_monitoring.set_enabled(started_ts is not None and (chestnut_usb_ready or chestnut_monitoring.seen) and not flash_active) + if chestnut_usb_ready and chestnut_monitoring.usb_failed: + chestnut_monitoring.retry() + if (chestnut_msg := chestnut_monitoring.update(sm, time.monotonic(), model_loading)) is not None: + pm.send('chestnutState', chestnut_msg) + # Offroad power monitoring voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage power_monitor.calculate(voltage, onroad_conditions["ignition"])