diff --git a/openpilot/selfdrive/modeld/helpers.py b/openpilot/selfdrive/modeld/helpers.py index 0d88e12a6a..9ac772e697 100644 --- a/openpilot/selfdrive/modeld/helpers.py +++ b/openpilot/selfdrive/modeld/helpers.py @@ -1,4 +1,5 @@ import sys +import time from pathlib import Path from openpilot.common.hardware import AGNOS @@ -29,6 +30,14 @@ def chestnut_present() -> bool: pass return False +def wait_for_chestnut(timeout: float = 10.) -> None: + # the cable is detected before chestnut enumerates + st = time.monotonic() + while not chestnut_present(): + if time.monotonic() - st > timeout: + raise TimeoutError("chestnut did not enumerate") + time.sleep(0.1) + def chestnut_compiled() -> bool: path = modeld_pkl_path(chestnut=True) return path.is_file() and all( diff --git a/openpilot/selfdrive/modeld/modeld.py b/openpilot/selfdrive/modeld/modeld.py index f33684dc47..ee76d72f46 100755 --- a/openpilot/selfdrive/modeld/modeld.py +++ b/openpilot/selfdrive/modeld/modeld.py @@ -28,6 +28,7 @@ from msgq.visionipc import VisionIpcClient, VisionBuf from opendbc.car.car_helpers import get_demo_car_params from openpilot.common.swaglog import cloudlog from openpilot.common.params import Params +from openpilot.common.hardware.usb import cable_connected from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import config_realtime_process, DT_MDL from openpilot.common.transformations.camera import DEVICE_CAMERAS @@ -38,7 +39,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, from openpilot.selfdrive.modeld.parse_model_outputs import Parser from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_driving_model_data, fill_pose_msg, PublishState from openpilot.selfdrive.modeld.constants import ModelConstants, Plan -from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob +from openpilot.selfdrive.modeld.helpers import MODELS_DIR, chestnut_present, chestnut_compiled, modeld_pkl_path, load_oob, wait_for_chestnut SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') @@ -228,7 +229,7 @@ class ModelState: def main(demo=False): cloudlog.warning("modeld init") - CHESTNUT = chestnut_present() and chestnut_compiled() + CHESTNUT = chestnut_compiled() and (chestnut_present() or cable_connected()) if CHESTNUT: os.environ['HCQDEV_WAIT_TIMEOUT_MS'] = '3000' params = Params() @@ -268,6 +269,7 @@ def main(demo=False): def load_big(): nonlocal big_model try: + wait_for_chestnut() m = ModelState(vipc_client_main.width, vipc_client_main.height, True) m.warmup() big_model = m diff --git a/openpilot/selfdrive/ui/ui_state.py b/openpilot/selfdrive/ui/ui_state.py index 08f63228b7..fecafc6b46 100644 --- a/openpilot/selfdrive/ui/ui_state.py +++ b/openpilot/selfdrive/ui/ui_state.py @@ -202,7 +202,7 @@ class UIState: self.status = UIStatus.DISENGAGED self.started_frame = self.sm.frame self.started_time = time.monotonic() - self.chestnut_present = self.sm["deviceState"].chestnutPresent + self.chestnut_present = self.sm["deviceState"].chestnutPresent or (self.chestnut_compiled and self.usb_connected) for callback in self._offroad_transition_callbacks: callback() @@ -222,10 +222,10 @@ class UIState: self.chestnut_state = ChestnutState.DISCONNECTED elif not self.chestnut_compiled: self.chestnut_state = ChestnutState.UNCOMPILED - elif self.chestnut_state == ChestnutState.FAILED or not detected or (model_seen and (not self.sm.alive["modelV2"] or not self.sm["modelV2"].big)): - self.chestnut_state = ChestnutState.FAILED elif self.chestnut_loading or not model_seen: self.chestnut_state = ChestnutState.LOADING + elif self.chestnut_state == ChestnutState.FAILED or not detected or (model_seen and (not self.sm.alive["modelV2"] or not self.sm["modelV2"].big)): + self.chestnut_state = ChestnutState.FAILED elif self.chestnut_active is False: self.chestnut_state = ChestnutState.FAILED else: