diff --git a/common/params_keys.h b/common/params_keys.h index c65c99328..debfc0938 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -197,7 +197,7 @@ inline static std::unordered_map keys = { {"CalibratedLateralAcceleration", {PERSISTENT, FLOAT, "2.0", "2.0", 2}}, {"CalibrationProgress", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"CameraOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, - {"CameraView", {PERSISTENT, INT, "3", "0", 2, SETTINGS_SIMPLE}}, + {"CameraView", {PERSISTENT, INT, "2", "0", 2, SETTINGS_SIMPLE}}, {"CancelDownloadMaps", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"DisableWideRoad", {PERSISTENT, BOOL, "0", "0", 3}}, {"CancelModelDownload", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, diff --git a/scripts/model_compiler.py b/scripts/model_compiler.py index c5d11fe52..bf67056ec 100644 --- a/scripts/model_compiler.py +++ b/scripts/model_compiler.py @@ -50,6 +50,7 @@ USBGPU_PROBE_ATTEMPTS = 10 USBGPU_PROBE_TIMEOUT = 2 USBDEVFS_CONTROL = 0xC0185500 USBGPU_VID_PIDS = (("add1", "0001"), ("3801", "0001")) +USBGPU_FIRMWARE_PRODUCT = "custom ed4e39b7-CLEAN" class _UsbdevfsControl(ctypes.Structure): @@ -91,6 +92,8 @@ def _probe_external_gpu_link_once() -> tuple[bool, str]: diagnostics: list[str] = [] for path in glob.glob("/sys/bus/usb/devices/*"): try: + if not Path(path, "idVendor").is_file(): + continue vendor = Path(path, "idVendor").read_text().strip().lower() product = Path(path, "idProduct").read_text().strip().lower() if (vendor, product) not in USBGPU_VID_PIDS: @@ -98,6 +101,9 @@ def _probe_external_gpu_link_once() -> tuple[bool, str]: bus = int(Path(path, "busnum").read_text()) device = int(Path(path, "devnum").read_text()) location = f"usb:{bus}-{device}" + firmware = Path(path, "product").read_text().strip() + if firmware and firmware != USBGPU_FIRMWARE_PRODUCT: + return False, f"{location}: firmware {firmware!r}, expected {USBGPU_FIRMWARE_PRODUCT!r}" fd = os.open(f"/dev/bus/usb/{bus:03d}/{device:03d}", os.O_RDWR) except (OSError, ValueError) as exc: diagnostics.append(f"{path}: open failed ({exc})") @@ -138,6 +144,12 @@ def wait_for_external_gpu(compile_env: dict[str, str]) -> bool: ready, detail = False, str(exc) if ready: return True + if "firmware" in detail and "expected" in detail: + raise RuntimeError( + f"External GPU firmware is out of date: {detail}. " + "Wait for hardwared to flash the dock, or run " + "sudo python3 system/hardware/chestnut/flash.py ed4e39b7." + ) diagnostics.append(detail) detail = diagnostics[-1] if diagnostics else "unknown error" diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d5365f3cb..b0c86b4a2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -462,6 +462,10 @@ class LatControlTorque(LatControl): friction_scale *= get_ioniq_6_2025_low_speed_center_friction_scale( setpoint, desired_lateral_jerk, CS.vEgo, ) + if self.is_genesis_gv70: + ff *= get_genesis_gv70_unwind_ff_scale( + setpoint, measurement, desired_lateral_jerk, CS.vEgo, + ) if ioniq_6_active: vehicle_friction_jerk_deadzone = ( IONIQ_6_2025_FRICTION_JERK_DEADZONE if self.is_ioniq_6_2025 else IONIQ_6_FRICTION_JERK_DEADZONE diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index d8827d238..b32916c40 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -200,6 +200,13 @@ GENESIS_GV70_FRICTION_CENTER_LAT = 0.28 GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12 GENESIS_GV70_FRICTION_CALM_JERK = 0.35 GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10 +GENESIS_GV70_UNWIND_FF_REDUCTION_MAX = 0.35 +GENESIS_GV70_UNWIND_FF_OVERSHOOT = 0.15 +GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH = 0.18 +GENESIS_GV70_UNWIND_FF_JERK = 0.10 +GENESIS_GV70_UNWIND_FF_JERK_WIDTH = 0.10 +GENESIS_GV70_UNWIND_FF_SPEED = 10.0 * CV.MPH_TO_MS +GENESIS_GV70_UNWIND_FF_SPEED_WIDTH = 4.0 * CV.MPH_TO_MS GENESIS_G70_FRICTION_THRESHOLD_GAIN = 0.10 GENESIS_G70_FRICTION_SPEED_ONSET = 10.0 @@ -2583,6 +2590,24 @@ def get_genesis_gv70_friction_threshold(v_ego: float, desired_lateral_accel: flo return base_threshold * (1.0 + gain) +def get_genesis_gv70_unwind_ff_scale(setpoint: float, measured_lateral_accel: float, + desired_lateral_jerk: float, v_ego: float) -> float: + """Remove old-turn feedforward when the GV70 has already over-rotated.""" + if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0: + return 1.0 + + overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0) + if overshoot <= 0.0: + return 1.0 + overshoot_weight = _sigmoid((overshoot - GENESIS_GV70_UNWIND_FF_OVERSHOOT) / + GENESIS_GV70_UNWIND_FF_OVERSHOOT_WIDTH) + jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_GV70_UNWIND_FF_JERK) / + GENESIS_GV70_UNWIND_FF_JERK_WIDTH) + speed_weight = _sigmoid((v_ego - GENESIS_GV70_UNWIND_FF_SPEED) / + GENESIS_GV70_UNWIND_FF_SPEED_WIDTH) + return 1.0 - GENESIS_GV70_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight * speed_weight + + def get_genesis_g70_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: base_threshold = get_standard_friction_threshold(v_ego) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 96f2b1ec4..bbd3a1924 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -77,6 +77,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g70_low_speed_angle_damping, get_genesis_g70_low_speed_output_limit, get_genesis_gv70_friction_threshold, + get_genesis_gv70_unwind_ff_scale, get_elantra_non_scc_ff_scale, get_palisade_ff_scale, get_palisade_center_output_scale, @@ -661,6 +662,14 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_genesis_gv70_unwind_ff_scale(self): + assert get_genesis_gv70_unwind_ff_scale(-0.3, -0.3, 0.8, 15.0) == 1.0 + assert get_genesis_gv70_unwind_ff_scale(-0.3, 0.1, 0.8, 15.0) == 1.0 + + reduced = get_genesis_gv70_unwind_ff_scale(-0.2, -1.0, 1.0, 20.0) + assert 0.6 < reduced < 1.0 + assert get_genesis_gv70_unwind_ff_scale(-0.2, -1.0, -1.0, 20.0) == 1.0 + def test_palisade_ff_scale_curve(self): assert get_palisade_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_palisade_ff_scale(0.6, 0.0, 8.0) diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 5b87fee58..1e600ac8f 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -4,6 +4,7 @@ import pytest from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController from openpilot.starpilot.controls.lib.starpilot_vcruise import ( FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, @@ -17,6 +18,7 @@ from types import SimpleNamespace class FakeParams: def __init__(self, values=None): self.values = dict(values or {}) + self.writes = [] def get(self, *args, **kwargs): key = args[0] if args else None @@ -25,8 +27,9 @@ class FakeParams: def get_float(self, *args, **kwargs): return 0.0 - def put_nonblocking(self, *args, **kwargs): - pass + def put_nonblocking(self, key, value): + self.values[key] = value + self.writes.append((key, value)) def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None, road_curvature=0.0): @@ -56,6 +59,7 @@ def make_sm(*, standstill=True, min_steer_speed=0.0): "carState": SimpleNamespace( standstill=standstill, gasPressed=False, + brakePressed=False, vCruiseCluster=0.0, vEgoCluster=0.0, leftBlinker=False, @@ -64,6 +68,7 @@ def make_sm(*, standstill=True, min_steer_speed=0.0): ), "carParams": SimpleNamespace(minSteerSpeed=min_steer_speed), "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), + "onroadEvents": [], } @@ -196,6 +201,46 @@ def test_curve_speed_controller_stays_enabled_with_a_lead_by_default(): assert vcruise.csc_controlling_speed +@pytest.mark.parametrize( + ("long_active", "gas_pressed"), + [(False, False), (True, True)], +) +def test_curve_speed_controller_learns_when_speed_is_manually_controlled(long_active, gas_pressed): + planner, vcruise = make_vcruise(road_curvature=0.02) + sm = make_sm(standstill=False) + sm["carControl"].longActive = long_active + sm["carState"].gasPressed = gas_pressed + toggles = make_toggles() + toggles.curve_speed_controller = True + planner.driving_in_curve = True + planner.road_curvature_detected = True + planner.lateral_acceleration = 2.4 + vcruise.csc.training_timer = PLANNER_TIME + + update_vcruise(vcruise, sm, toggles, now=50.0, v_ego=20.0) + + assert vcruise.csc.enable_training + assert vcruise.csc.curvature_data["0.02"]["count"] == 1 + assert not vcruise.csc_controlling_speed + + +def test_curve_speed_controller_persists_data_after_leaving_curve(): + planner, vcruise = make_vcruise(road_curvature=0.02) + sm = make_sm(standstill=False) + sm["carControl"].longActive = False + planner.driving_in_curve = True + planner.lateral_acceleration = 2.4 + vcruise.csc.training_timer = PLANNER_TIME + + vcruise.csc.log_data(20.0, sm) + assert not any(key == "CurvatureData" for key, _ in planner.params.writes) + + planner.driving_in_curve = False + vcruise.csc.log_data(20.0, sm) + + assert any(key == "CurvatureData" for key, _ in planner.params.writes) + + def test_curve_speed_controller_ramps_toward_curve_speed_at_bounded_rate(): planner = SimpleNamespace( params=FakeParams(), diff --git a/selfdrive/modeld/helpers.py b/selfdrive/modeld/helpers.py index d2ae5a11b..20d0706c7 100644 --- a/selfdrive/modeld/helpers.py +++ b/selfdrive/modeld/helpers.py @@ -8,7 +8,7 @@ import tempfile from pathlib import Path from tinygrad.device import Device -from openpilot.system.hardware.usb import chestnut_present +from openpilot.system.hardware.usb import chestnut_firmware_ready MODELS_DIR = Path(__file__).resolve().parent / "models" TG_INPUT_DEVICES_PATH = MODELS_DIR / "tg_input_devices.json" @@ -70,7 +70,7 @@ def modeld_pkl_path(usbgpu: bool) -> Path: def usbgpu_present() -> bool: - return chestnut_present() + return chestnut_firmware_ready() def tinygrad_dev_config(usbgpu: bool, tici: bool) -> str: diff --git a/selfdrive/ui/layouts/settings/starpilot/appearance.py b/selfdrive/ui/layouts/settings/starpilot/appearance.py index 3d918b40d..470a344a2 100644 --- a/selfdrive/ui/layouts/settings/starpilot/appearance.py +++ b/selfdrive/ui/layouts/settings/starpilot/appearance.py @@ -398,7 +398,7 @@ class StarPilotAppearanceLayout(_SettingsPage): self._system_rows = [ SettingRow("CameraView", "value", tr_noop("Camera View"), subtitle="", - get_value=lambda: tr(CAMERA_VIEWS[self._params.get_int("CameraView")]), + get_value=lambda: tr(CAMERA_VIEWS[self._params.get_int("CameraView", return_default=True, default=2)]), on_click=self._show_camera_view_selector), SettingRow("DriverCamera", "toggle", tr_noop("Driver Camera"), subtitle="", @@ -605,7 +605,7 @@ class StarPilotAppearanceLayout(_SettingsPage): # ── Camera view ── def _show_camera_view_selector(self): - current = self._params.get_int("CameraView") + current = self._params.get_int("CameraView", return_default=True, default=2) def on_select(res): if res == DialogResult.CONFIRM and dialog.selection: diff --git a/selfdrive/ui/mici/layouts/settings/visuals.py b/selfdrive/ui/mici/layouts/settings/visuals.py index d89d2a4d7..c5fb036c1 100644 --- a/selfdrive/ui/mici/layouts/settings/visuals.py +++ b/selfdrive/ui/mici/layouts/settings/visuals.py @@ -16,12 +16,12 @@ class CameraViewBigButton(BigButton): self.refresh() def refresh(self): - current_idx = self._params.get_int("CameraView", return_default=True, default=3) + current_idx = self._params.get_int("CameraView", return_default=True, default=2) current_idx = max(0, min(current_idx, len(CAMERA_VIEW_LABELS) - 1)) self.set_value(CAMERA_VIEW_LABELS[current_idx].lower()) def _show_selector(self): - current_idx = self._params.get_int("CameraView", return_default=True, default=3) + current_idx = self._params.get_int("CameraView", return_default=True, default=2) current_idx = max(0, min(current_idx, len(CAMERA_VIEW_LABELS) - 1)) dialog_holder: dict[str, BigMultiOptionDialog] = {} diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index 799062325..bc9f78719 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -703,7 +703,9 @@ class AugmentedRoadView(CameraView): if camera_view_none: rl.draw_rectangle_rec(self._content_rect, rl.BLACK) else: + gui_app.mark_progress("mici.onroad.before_camera") super()._render(self._content_rect) + gui_app.mark_progress("mici.onroad.after_camera") waiting_for_controls = ui_state.started and not self._controls_ready() if waiting_for_controls: @@ -728,7 +730,9 @@ class AugmentedRoadView(CameraView): # Draw all UI overlays if draw_road_overlays: + gui_app.mark_progress("mici.onroad.before_model") self._model_renderer.render(self._content_rect) + gui_app.mark_progress("mici.onroad.after_model") # Fade out bottom of overlays for looks rl.draw_texture_ex(self._fade_texture, rl.Vector2(self._content_rect.x, self._content_rect.y), 0.0, 1.0, rl.WHITE) @@ -752,7 +756,9 @@ class AugmentedRoadView(CameraView): if ui_state.started: self._alert_renderer.render(self._content_rect) if draw_hud_controls: + gui_app.mark_progress("mici.onroad.before_hud") self._hud_renderer.render_foreground() + gui_app.mark_progress("mici.onroad.after_hud") rendered_standstill_timer = False if draw_hud_controls: rendered_standstill_timer = self._standstill_timer.render(self._content_rect, in_reverse) @@ -765,10 +771,12 @@ class AugmentedRoadView(CameraView): # Custom UI extension point - add custom overlays here # Use self._content_rect for positioning within camera bounds if draw_road_overlays: + gui_app.mark_progress("mici.onroad.before_sidebar") if ui_state.ui_params.get_bool("StockConfidenceBallWidget") and not self._sidebar_widgets.demo_active: self._confidence_ball.render(self.rect) else: self._sidebar_widgets.render(self.rect) + gui_app.mark_progress("mici.onroad.after_sidebar") if draw_hud_controls and (camera_view_none or is_driver_stream or not in_reverse): self._favorite_slots.render(self._content_rect) if camera_view_none or is_driver_stream or not in_reverse: @@ -832,9 +840,9 @@ class AugmentedRoadView(CameraView): @staticmethod def _camera_view() -> int: - camera_view = ui_state.ui_params.get_int("CameraView", return_default=True, default=CAMERA_VIEW_WIDE) + camera_view = ui_state.ui_params.get_int("CameraView", return_default=True, default=CAMERA_VIEW_STANDARD) if camera_view not in (CAMERA_VIEW_AUTO, CAMERA_VIEW_DRIVER, CAMERA_VIEW_STANDARD, CAMERA_VIEW_WIDE, CAMERA_VIEW_NONE): - return CAMERA_VIEW_WIDE + return CAMERA_VIEW_STANDARD return camera_view def _switch_stream_if_needed(self, sm, camera_view: int): @@ -846,13 +854,13 @@ class AugmentedRoadView(CameraView): reentry_selection_pending = (getattr(self, "_onroad_reentry_pending", False) and not getattr(self, "_reentry_stream_selected", False)) - if reentry_selection_pending: - self._refresh_available_streams() - if self._update_reverse_driver_camera_state(): self.switch_stream(DRIVER_CAM) return + if reentry_selection_pending or not self.available_streams: + self._refresh_available_streams() + wide_available = WIDE_CAM in self.available_streams if camera_view == CAMERA_VIEW_DRIVER: target = DRIVER_CAM @@ -867,8 +875,10 @@ class AugmentedRoadView(CameraView): elif v_ego > ROAD_CAM_MIN_SPEED: target = ROAD_CAM else: - # Hysteresis zone - keep the current road camera selection. - target = WIDE_CAM if self.stream_type == WIDE_CAM and wide_available else ROAD_CAM + # Hysteresis zone - keep the current or pending road camera selection. + current_road_stream = (self._target_stream_type if self._switching and + self._target_stream_type in (ROAD_CAM, WIDE_CAM) else self.stream_type) + target = WIDE_CAM if current_road_stream == WIDE_CAM and wide_available else ROAD_CAM else: target = ROAD_CAM diff --git a/selfdrive/ui/mici/tests/test_camera_cleanup.py b/selfdrive/ui/mici/tests/test_camera_cleanup.py index 8b75d8809..324a9fceb 100644 --- a/selfdrive/ui/mici/tests/test_camera_cleanup.py +++ b/selfdrive/ui/mici/tests/test_camera_cleanup.py @@ -174,6 +174,7 @@ def test_reverse_activation_cancels_mismatched_pending_switch(): view._target_stream_type = mici_augmented_road_view.WIDE_CAM view._target_client = object() view._switching = True + view.available_streams = [] view._closed = True view._update_reverse_driver_camera_state = lambda: True @@ -206,6 +207,31 @@ def test_onroad_reentry_keeps_matching_candidate_alive(): assert view._switching +@pytest.mark.parametrize("module", (big_augmented_road_view, mici_augmented_road_view)) +def test_initial_camera_selection_discovers_wide_before_connecting(module): + view = module.AugmentedRoadView.__new__(module.AugmentedRoadView) + selected = [] + view._stream_type = module.ROAD_CAM + view._target_stream_type = None + view._target_client = None + view._switching = False + view.available_streams = [] + view._onroad_reentry_pending = False + view._reentry_stream_selected = False + view._closed = True + view._update_reverse_driver_camera_state = lambda: False + view._refresh_available_streams = lambda: view.available_streams.append(module.WIDE_CAM) + view.switch_stream = selected.append + + sm = { + "selfdriveState": SimpleNamespace(experimentalMode=True), + "carState": SimpleNamespace(vEgo=0.0), + } + view._switch_stream_if_needed(sm, module.CAMERA_VIEW_AUTO) + + assert selected == [module.WIDE_CAM] + + def test_onroad_transition_marks_camera_reentry(monkeypatch): module = big_cameraview diff --git a/selfdrive/ui/onroad/augmented_road_view.py b/selfdrive/ui/onroad/augmented_road_view.py index 38869a381..62d59f6a4 100644 --- a/selfdrive/ui/onroad/augmented_road_view.py +++ b/selfdrive/ui/onroad/augmented_road_view.py @@ -197,9 +197,9 @@ class AugmentedRoadView(CameraView): @staticmethod def _camera_view() -> int: params = ui_state.ui_params - camera_view = params.get_int("CameraView", return_default=True, default=CAMERA_VIEW_WIDE) + camera_view = params.get_int("CameraView", return_default=True, default=CAMERA_VIEW_STANDARD) if camera_view not in (CAMERA_VIEW_AUTO, CAMERA_VIEW_DRIVER, CAMERA_VIEW_STANDARD, CAMERA_VIEW_WIDE, CAMERA_VIEW_NONE): - return CAMERA_VIEW_WIDE + return CAMERA_VIEW_STANDARD return camera_view def _switch_stream_if_needed(self, sm, camera_view: int): @@ -211,28 +211,31 @@ class AugmentedRoadView(CameraView): reentry_selection_pending = (getattr(self, "_onroad_reentry_pending", False) and not getattr(self, "_reentry_stream_selected", False)) - if reentry_selection_pending: - self._refresh_available_streams() - if self._update_reverse_driver_camera_state(): target = DRIVER_CAM - elif camera_view == CAMERA_VIEW_DRIVER: - target = DRIVER_CAM - elif camera_view == CAMERA_VIEW_STANDARD: - target = ROAD_CAM - elif camera_view == CAMERA_VIEW_WIDE: - target = WIDE_CAM if WIDE_CAM in self.available_streams else ROAD_CAM - elif sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: - v_ego = sm['carState'].vEgo - if v_ego < WIDE_CAM_MAX_SPEED: - target = WIDE_CAM - elif v_ego > ROAD_CAM_MIN_SPEED: - target = ROAD_CAM - else: - # Hysteresis zone - keep current road camera selection. - target = WIDE_CAM if self.stream_type == WIDE_CAM else ROAD_CAM else: - target = ROAD_CAM + if reentry_selection_pending or not self.available_streams: + self._refresh_available_streams() + + if camera_view == CAMERA_VIEW_DRIVER: + target = DRIVER_CAM + elif camera_view == CAMERA_VIEW_STANDARD: + target = ROAD_CAM + elif camera_view == CAMERA_VIEW_WIDE: + target = WIDE_CAM if WIDE_CAM in self.available_streams else ROAD_CAM + elif sm['selfdriveState'].experimentalMode and WIDE_CAM in self.available_streams: + v_ego = sm['carState'].vEgo + if v_ego < WIDE_CAM_MAX_SPEED: + target = WIDE_CAM + elif v_ego > ROAD_CAM_MIN_SPEED: + target = ROAD_CAM + else: + # Hysteresis zone - keep the current or pending road camera selection. + current_road_stream = (self._target_stream_type if self._switching and + self._target_stream_type in (ROAD_CAM, WIDE_CAM) else self.stream_type) + target = WIDE_CAM if current_road_stream == WIDE_CAM else ROAD_CAM + else: + target = ROAD_CAM if (reentry_selection_pending or self.stream_type != target or (self._switching and self._target_stream_type != target)): diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index 41f524cef..b1e7a871d 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -16,6 +16,7 @@ from openpilot.system.ui.widgets import Widget from openpilot.selfdrive.ui.ui_state import ui_state CONNECTION_RETRY_INTERVAL = 0.2 # seconds between connection attempts +STREAM_DISCOVERY_REFRESH_INTERVAL = 0.5 # seconds between nonblocking stream advertisements MICI_FORCE_TEXTURE_CAMERA = os.getenv("MICI_FORCE_TEXTURE_CAMERA", "0") == "1" # One stale frame can be normal ring-buffer reuse; repeated consecutive regressions demote EGL. EGL_REGRESSIVE_FRAME_FALLBACK_THRESHOLD = 3 @@ -152,6 +153,8 @@ class CameraView(Widget): self._texture_needs_update = True self.last_connection_attempt: float = 0.0 + self._last_stream_discovery: float = -float("inf") + self._last_switch_request: float = -float("inf") self._use_egl = TICI and not MICI_FORCE_TEXTURE_CAMERA and init_egl() if TICI and MICI_FORCE_TEXTURE_CAMERA: cloudlog.warning("CameraView EGL disabled by MICI_FORCE_TEXTURE_CAMERA, using texture rendering") @@ -216,6 +219,8 @@ class CameraView(Widget): self.available_streams.clear() self._texture_needs_update = True self.last_connection_attempt = 0.0 + self._last_stream_discovery = -float("inf") + self._last_switch_request = -float("inf") self._onroad_reentry_pending = ui_state.is_onroad() self._reentry_stream_selected = False @@ -224,6 +229,11 @@ class CameraView(Widget): self._placeholder_color = color def _refresh_available_streams(self) -> None: + current_time = rl.get_time() + if current_time - getattr(self, "_last_stream_discovery", -float("inf")) < STREAM_DISCOVERY_REFRESH_INTERVAL: + return + self._last_stream_discovery = current_time + streams = VisionIpcClient.available_streams(self._name, block=False) if streams: self.available_streams = list(streams) @@ -236,14 +246,23 @@ class CameraView(Widget): self._select_reentry_stream(stream_type) return + current_time = rl.get_time() if self._switching: if self._target_stream_type == stream_type: return + if self._stream_type == stream_type: + self._cancel_pending_switch() + return + if current_time - getattr(self, "_last_switch_request", -float("inf")) < CONNECTION_RETRY_INTERVAL: + return self._cancel_pending_switch() if self._stream_type == stream_type: return + if current_time - getattr(self, "_last_switch_request", -float("inf")) < CONNECTION_RETRY_INTERVAL: + return + cloudlog.debug(f'Preparing switch from {self._stream_type} to {stream_type}') if self._target_client: @@ -252,6 +271,7 @@ class CameraView(Widget): self._target_stream_type = stream_type self._target_client = VisionIpcClient(self._name, stream_type, conflate=True) self._switching = True + self._last_switch_request = current_time def _cancel_pending_switch(self) -> None: if self._target_client is not None: @@ -603,7 +623,7 @@ class CameraView(Widget): self._initialize_textures() available_streams = getattr(self.client, "available_streams", None) if available_streams is not None: - self.available_streams = available_streams(self._name, block=False) + self.available_streams = list(available_streams(self._name, block=False)) def _initialize_textures(self): self._clear_textures() diff --git a/starpilot/common/favorite_slots.py b/starpilot/common/favorite_slots.py index 9cf0a3cc5..e5e4b2e86 100644 --- a/starpilot/common/favorite_slots.py +++ b/starpilot/common/favorite_slots.py @@ -196,6 +196,9 @@ def toggle_favorite_slot(slot_index: int, params: Params | None = None, params_m if not is_bool_param(params, key): return False + if key == "AlphaLongitudinalEnabled" and params.get_bool("IsOnroad"): + return False + next_value = not params.get_bool(key) put_bool = getattr(params, "put_bool_nonblocking", None) or getattr(params, "put_bool", None) if put_bool is None: diff --git a/starpilot/common/tests/test_favorite_slots.py b/starpilot/common/tests/test_favorite_slots.py index 85da1c324..c77bcbe16 100644 --- a/starpilot/common/tests/test_favorite_slots.py +++ b/starpilot/common/tests/test_favorite_slots.py @@ -16,6 +16,8 @@ class FakeParams: self.store = {} self.types = { FAVORITE_SLOTS_PARAM: ParamKeyType.JSON, + "AlphaLongitudinalEnabled": ParamKeyType.BOOL, + "ForceOffroad": ParamKeyType.BOOL, "RedneckCruise": ParamKeyType.BOOL, "NotBool": ParamKeyType.INT, } @@ -90,6 +92,30 @@ def test_toggle_favorite_slot_flips_bool_and_requests_refresh(): assert memory.get_bool("StarPilotTogglesUpdated") is True +def test_toggle_favorite_slot_blocks_alpha_longitudinal_onroad(): + params = FakeParams() + params.put("IsOnroad", True) + params.put("AlphaLongitudinalEnabled", False) + params.put(FAVORITE_SLOTS_PARAM, [ + {"enabled": True, "show_onroad": True, "key": "AlphaLongitudinalEnabled", "label": "Alpha Longitudinal"}, + ]) + + assert toggle_favorite_slot(0, params, FakeParams()) is False + assert params.get_bool("AlphaLongitudinalEnabled") is False + + +def test_toggle_favorite_slot_leaves_force_offroad_unrestricted(): + params = FakeParams() + params.put("IsOnroad", True) + params.put("ForceOffroad", False) + params.put(FAVORITE_SLOTS_PARAM, [ + {"enabled": True, "show_onroad": True, "key": "ForceOffroad", "label": "Force Offroad"}, + ]) + + assert toggle_favorite_slot(0, params, FakeParams()) is True + assert params.get_bool("ForceOffroad") is True + + def test_toggle_favorite_slot_action_increments_virtual_button_counter(): params = FakeParams() memory = FakeParams() diff --git a/starpilot/controls/lib/curve_speed_controller.py b/starpilot/controls/lib/curve_speed_controller.py index e4f96ec0a..7d21780be 100644 --- a/starpilot/controls/lib/curve_speed_controller.py +++ b/starpilot/controls/lib/curve_speed_controller.py @@ -16,6 +16,27 @@ ROUNDING_PRECISION = 5 STEP = 0.001 +def is_user_overriding_longitudinal(sm): + try: + if any(getattr(event, "overrideLongitudinal", False) for event in sm["onroadEvents"]): + return True + except (KeyError, TypeError): + pass + + car_state = sm["carState"] + starpilot_car_state = sm["starpilotCarState"] + return bool( + getattr(car_state, "gasPressed", False) or + getattr(car_state, "brakePressed", False) or + getattr(starpilot_car_state, "accelPressed", False) + ) + + +def is_manual_speed_control(sm): + """Return whether the driver, rather than longitudinal control, owns speed.""" + return not bool(sm["carControl"].longActive) or is_user_overriding_longitudinal(sm) + + class CurveSpeedController: def __init__(self, StarPilotVCruise): self.starpilot_planner = StarPilotVCruise.starpilot_planner @@ -23,7 +44,9 @@ class CurveSpeedController: self.enable_training = False self.target_set = False - self.training_timer = 0 + self.training_timer = 0.0 + self.persistence_timer = 0.0 + self.data_dirty = False curvature_data = self.starpilot_planner.params.get("CurvatureData") self.curvature_data = self._normalize_curvature_data(curvature_data) @@ -75,54 +98,72 @@ class CurveSpeedController: return normalized + def _persist_data(self): + if not self.data_dirty: + return + + progress = 0.0 + for key in self.required_curvatures: + if key in self.curvature_data: + progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) + + self.starpilot_planner.params.put_nonblocking("CalibrationProgress", (progress / len(self.required_curvatures)) * 100) + self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) + self.data_dirty = False + self.persistence_timer = 0.0 + + def flush_data(self): + self._persist_data() + def log_data(self, v_ego, sm): - self.enable_training = v_ego > CRUISING_SPEED - self.enable_training &= not self.starpilot_planner.tracking_lead - self.enable_training &= not sm["carControl"].longActive + eligible = ( + v_ego > CRUISING_SPEED and + not self.starpilot_planner.tracking_lead and + is_manual_speed_control(sm) + ) + self.enable_training = False - if self.enable_training: - self.training_timer += DT_MDL + if not eligible: + self.flush_data() + self.training_timer = 0.0 + self.persistence_timer = 0.0 + return - if self.training_timer >= PLANNER_TIME and self.starpilot_planner.driving_in_curve and not (sm["carState"].leftBlinker or sm["carState"].rightBlinker): - lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) - road_curvature = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) + self.training_timer += DT_MDL + if self.data_dirty: + self.persistence_timer += DT_MDL - key = road_curvature - if key in self.curvature_data: - data = self.curvature_data[key] + in_curve = ( + self.training_timer >= PLANNER_TIME and + self.starpilot_planner.driving_in_curve and + not (sm["carState"].leftBlinker or sm["carState"].rightBlinker) + ) + if in_curve: + lateral_acceleration = abs(self.starpilot_planner.lateral_acceleration) + road_curvature = self._bucket_curvature(abs(self.starpilot_planner.road_curvature)) - average = data["average"] - count = data["count"] - - self.curvature_data[key] = { - "average": ((average * count) + lateral_acceleration) / (count + 1), - "count": count + 1 - } - else: - self.curvature_data[key] = { - "average": lateral_acceleration, - "count": 1 - } - - self.update_lateral_acceleration() + if road_curvature in self.curvature_data: + data = self.curvature_data[road_curvature] + average = data["average"] + count = data["count"] + self.curvature_data[road_curvature] = { + "average": ((average * count) + lateral_acceleration) / (count + 1), + "count": count + 1 + } else: - self.enable_training = False + self.curvature_data[road_curvature] = { + "average": lateral_acceleration, + "count": 1 + } - elif self.training_timer >= PLANNER_TIME: - progress = 0.0 - for key in self.required_curvatures: - if key in self.curvature_data: - progress += min(self.curvature_data[key]["count"] / CALIBRATION_PROGRESS_THRESHOLD, 1.0) + self.data_dirty = True + self.update_lateral_acceleration() + self.enable_training = True - self.starpilot_planner.params.put_nonblocking("CalibrationProgress", (progress / len(self.required_curvatures)) * 100) - self.starpilot_planner.params.put_nonblocking("CurvatureData", self.curvature_data) - - self.enable_training = False - self.training_timer = 0 - - else: - self.enable_training = False - self.training_timer = 0 + if self.persistence_timer >= PLANNER_TIME: + self.flush_data() + elif self.data_dirty: + self.flush_data() def update_lateral_acceleration(self): if self.curvature_data: diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index c45ec174e..3e9c8cbbb 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -6,7 +6,7 @@ from openpilot.common.constants import CV 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 +from openpilot.starpilot.controls.lib.curve_speed_controller import CurveSpeedController, is_manual_speed_control from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController CSC_MIN_SPEED = CITY_SPEED_LIMIT * CV.MPH_TO_MS @@ -479,8 +479,10 @@ class StarPilotVCruise: # FrogsGoMoo's Curve Speed Controller following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False)) + manual_speed_control = is_manual_speed_control(sm) csc_available = ( long_control_active and + not manual_speed_control and v_ego > CRUISING_SPEED and starpilot_toggles.curve_speed_controller and (not getattr(starpilot_toggles, "csc_no_lead", False) or not following_lead) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index b742d9b71..f54e0494c 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -108,6 +108,7 @@ class StarPilotPlanner: self.radarless_follow_hold_until = 0.0 def shutdown(self): + self.starpilot_vcruise.csc.flush_data() self.starpilot_vcruise.slc.shutdown() self.starpilot_weather.executor.shutdown(wait=False, cancel_futures=True) diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index cc319a1e2..282f15f74 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -38,6 +38,10 @@ TRACK_CROP_PADDING_RATIO = 0.06 TRACK_REPEAT_CONFIDENCE_BONUS = 0.12 # Keep optional vision work responsive while giving realtime processes more headroom. BUSY_INFERENCE_INTERVAL = 1.5 +# Do not schedule another expensive inference before the previous one has given +# the rest of the device enough time to run. This matters on Mici, where a +# detector/classifier pass can take several hundred milliseconds. +PROCESSING_THROTTLE_RATIO = 2.5 MEMORY_PRESSURE_INFERENCE_INTERVAL = 2.0 MEMORY_PRESSURE_CLASSIFICATION_INTERVAL = 0.75 MEMORY_PRESSURE_AVAILABLE_KB = 512 * 1024 @@ -982,17 +986,26 @@ class SpeedLimitVisionDaemon: elif memory_pressure == "pressure": interval = max(interval, MEMORY_PRESSURE_INFERENCE_INTERVAL) reason = "memory_pressure" - elif self.coexistence_mode: - cpu_usage = list(self.sm["deviceState"].cpuUsagePercent) if self.sm is not None and self.sm.valid.get("deviceState", False) else [] - factor = device_cpu_throttle_factor(cpu_usage, name="SpeedLimit") - if factor > 1.05: + else: + processing_interval = min( + BUSY_INFERENCE_INTERVAL, + self.last_frame_process_duration_s * PROCESSING_THROTTLE_RATIO, + ) + if processing_interval > interval: + interval = processing_interval + reason = "processing_cost" + + if self.coexistence_mode: + cpu_usage = list(self.sm["deviceState"].cpuUsagePercent) if self.sm is not None and self.sm.valid.get("deviceState", False) else [] + factor = device_cpu_throttle_factor(cpu_usage, name="SpeedLimit") + if factor > 1.05: + self.last_cpu_busy = True + interval *= factor + reason = f"cpu_{factor:.1f}x" + elif self._device_cpu_busy(): self.last_cpu_busy = True - interval *= factor - reason = f"cpu_{factor:.1f}x" - elif self._device_cpu_busy(): - self.last_cpu_busy = True - interval = max(interval, BUSY_INFERENCE_INTERVAL) - reason = "cpu_busy" + interval = max(interval, BUSY_INFERENCE_INTERVAL) + reason = "cpu_busy" self.last_inference_interval = interval self.last_inference_interval_reason = reason return interval diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index cb226fd4f..f37980c58 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -112,6 +112,23 @@ def test_memory_pressure_level(available_kb, usage_percent, expected): assert slv.memory_pressure_level(available_kb, usage_percent) == expected +def test_inference_interval_backs_off_after_expensive_inference(): + daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.followup_until = 0.0 + daemon.last_live_pose_inputs_not_ok_at = -float("inf") + daemon.last_frame_process_duration_s = 0.4 + daemon.memory_pressure_state = "normal" + daemon.coexistence_mode = False + daemon.last_cpu_busy = False + daemon._update_memory_pressure = lambda: "normal" + daemon._device_cpu_busy = lambda: False + + interval = daemon._inference_interval(10.0) + + assert interval == pytest.approx(1.0) + assert daemon.last_inference_interval_reason == "processing_cost" + + def test_disconnect_camera_releases_client_state(): daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) daemon.client = object() diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js index f70d0a58f..555dc7eb1 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings.js +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings.js @@ -106,6 +106,7 @@ function isSettingVisible(section, param) { // This policy controls Galaxy rendering only; hidden params retain their stored values. if (HIDDEN_SETTING_KEYS.has(param.key) || !isVehicleSettingVisible(section, param)) return false if (RADAR_REQUIRED_KEYS.has(param.key) && !state.values.HasRadar) return false + if (param.key === "AlphaLongitudinalEnabled" && !state.values.AlphaLongitudinalAvailable) return false if (state.values[GALAXY_DEVELOPER_MODE_KEY]) return true return section.name === "Favorites" || param.settings_tier === "simple" } @@ -1101,6 +1102,11 @@ async function updateParam(key, elType) { return } + if (elType === "checkbox" && formattedVal && param.confirm_message && !window.confirm(param.confirm_message)) { + revertInput(key, current, elType) + return + } + try { const res = await fetch("/api/params", { method: "PUT", @@ -1231,6 +1237,12 @@ function clearSearchFilter() { const cancelButtonKeys = new Set(["CancelButtonControl", "LongCancelButtonControl", "VeryLongCancelButtonControl"]) function getSettingLockReason(param) { + if (param?.requires_offroad && state.values.IsOnroad) { + return "This setting can only be changed while parked." + } + if (param?.requires_parked && !state.values.VehicleParked) { + return "This setting can only be changed while the vehicle is in Park." + } if (param?.disabled_when_key_true && state.values[param.disabled_when_key_true]) { return param.disabled_reason || "Disabled by another setting." } diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index b7017fdc5..4dadd728b 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -4069,6 +4069,28 @@ "ui_type": "toggle", "settings_tier": "simple" }, + { + "key": "AlphaLongitudinalEnabled", + "label": "openpilot Longitudinal Control (Alpha)", + "description": "Enable openpilot longitudinal control for vehicles that support it. This replaces the vehicle's stock ACC and may disable automatic emergency braking. Changing this setting restarts the driving stack.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "GalaxyDeveloperMode", + "requires_offroad": true, + "confirm_message": "Enable openpilot Longitudinal Control? This is an alpha feature, replaces the vehicle's stock ACC, and may disable automatic emergency braking.", + "settings_tier": "advanced" + }, + { + "key": "ForceOffroad", + "label": "Force Offroad", + "description": "Temporarily switch the device to the offroad state for remote updates when the vehicle is parked. This control is only available while the vehicle reports Park.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "GalaxyDeveloperMode", + "requires_parked": true, + "confirm_message": "Force the device offroad? Only use this while the vehicle is in Park.", + "settings_tier": "advanced" + }, { "key": "RedneckCruise", "label": "Redneck Cruise", diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index f725800d5..3ea99252e 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -68,7 +68,7 @@ def test_galaxy_layout_contains_basic_mode_controls(): } <= sections["Longitudinal (Speed & Following)"].keys() assert "RedneckCruise" not in sections["Longitudinal (Speed & Following)"].keys() assert sections["Developer"]["RedneckCruise"]["parent_key"] == "GalaxyDeveloperMode" - assert {"GalaxyDeveloperMode", "UseOldUI"} <= sections["Developer"].keys() + assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode", "UseOldUI"} <= sections["Developer"].keys() def test_device_shutdown_uses_literal_hours(): @@ -155,6 +155,12 @@ def test_requested_simple_and_advanced_settings_tiers(): assert longitudinal[key]["settings_tier"] == "advanced" assert developer["GalaxyDeveloperMode"]["settings_tier"] == "simple" + assert developer["AlphaLongitudinalEnabled"]["parent_key"] == "GalaxyDeveloperMode" + assert developer["AlphaLongitudinalEnabled"]["requires_offroad"] is True + assert developer["AlphaLongitudinalEnabled"]["settings_tier"] == "advanced" + assert developer["ForceOffroad"]["parent_key"] == "GalaxyDeveloperMode" + assert developer["ForceOffroad"]["requires_parked"] is True + assert developer["ForceOffroad"]["settings_tier"] == "advanced" assert developer["UseOldUI"]["settings_tier"] == "simple" assert developer["DeveloperUI"]["settings_tier"] == "advanced" assert developer["RedneckCruise"]["settings_tier"] == "advanced" diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index 4244b8ee7..ef5148741 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -85,7 +85,15 @@ def _params_client(monkeypatch, values, device_type): monkeypatch.setattr( the_galaxy, "_get_param_type_info", - lambda: ({"UseOldUI", "TryRaylibUI"}, {"UseOldUI": bool, "TryRaylibUI": bool}), + lambda: ( + {"AlphaLongitudinalEnabled", "ForceOffroad", "UseOldUI", "TryRaylibUI"}, + { + "AlphaLongitudinalEnabled": bool, + "ForceOffroad": bool, + "UseOldUI": bool, + "TryRaylibUI": bool, + }, + ), ) monkeypatch.setattr(the_galaxy.HARDWARE, "get_device_type", lambda: device_type) monkeypatch.setattr(the_galaxy.Paths, "comma_home", lambda: "/tmp/dashboard-test-home", raising=False) @@ -304,6 +312,82 @@ def test_legacy_try_raylib_ui_payload_updates_use_old_ui(monkeypatch): assert fake_params.writes == [("UseOldUI", False), ("TryRaylibUI", True)] +def test_alpha_longitudinal_toggle_writes_and_requests_offroad_cycle(monkeypatch): + client, fake_params = _params_client(monkeypatch, { + "AlphaLongitudinalEnabled": False, + "IsOnroad": False, + }, "tici") + monkeypatch.setattr(the_galaxy, "_get_alpha_longitudinal_available", lambda: True) + + response = client.put("/api/params", json={"key": "AlphaLongitudinalEnabled", "value": True}) + + assert response.status_code == 200 + assert fake_params.values["AlphaLongitudinalEnabled"] is True + assert fake_params.values["OnroadCycleRequested"] is True + assert fake_params.writes == [ + ("AlphaLongitudinalEnabled", True), + ("OnroadCycleRequested", True), + ] + + +def test_alpha_longitudinal_toggle_rejects_onroad(monkeypatch): + client, fake_params = _params_client(monkeypatch, { + "AlphaLongitudinalEnabled": False, + "IsOnroad": True, + }, "tici") + monkeypatch.setattr(the_galaxy, "_get_alpha_longitudinal_available", lambda: True) + + response = client.put("/api/params", json={"key": "AlphaLongitudinalEnabled", "value": True}) + + assert response.status_code == 403 + assert response.get_json()["error"] == "Cannot change Alpha Longitudinal while driving." + assert fake_params.writes == [] + + +def test_alpha_longitudinal_toggle_rejects_unsupported_vehicle(monkeypatch): + client, fake_params = _params_client(monkeypatch, { + "AlphaLongitudinalEnabled": False, + "IsOnroad": False, + }, "tici") + monkeypatch.setattr(the_galaxy, "_get_alpha_longitudinal_available", lambda: False) + + response = client.put("/api/params", json={"key": "AlphaLongitudinalEnabled", "value": True}) + + assert response.status_code == 403 + assert response.get_json()["error"] == "Alpha Longitudinal is not available for the detected vehicle." + assert fake_params.writes == [] + + +def test_force_offroad_toggle_requires_live_park(monkeypatch): + client, fake_params = _params_client(monkeypatch, { + "ForceOffroad": False, + "ForceOnroad": False, + "IsOnroad": True, + }, "tici") + monkeypatch.setattr(the_galaxy, "_get_vehicle_parked", lambda: True) + + response = client.put("/api/params", json={"key": "ForceOffroad", "value": True}) + + assert response.status_code == 200 + assert response.get_json()["updated"] == {"ForceOffroad": True, "ForceOnroad": False} + assert fake_params.values["ForceOffroad"] is True + assert fake_params.values["ForceOnroad"] is False + + +def test_force_offroad_toggle_rejects_when_not_parked(monkeypatch): + client, fake_params = _params_client(monkeypatch, { + "ForceOffroad": False, + "IsOnroad": True, + }, "tici") + monkeypatch.setattr(the_galaxy, "_get_vehicle_parked", lambda: False) + + response = client.put("/api/params", json={"key": "ForceOffroad", "value": True}) + + assert response.status_code == 403 + assert response.get_json()["error"] == "Force Offroad is only available while the vehicle is in Park." + assert fake_params.writes == [] + + def test_curve_speed_controller_reset_clears_learned_data_offroad(monkeypatch): client, fake_params = _params_client(monkeypatch, { "IsOnroad": False, diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index e9f97d706..7fc7218a3 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -2418,6 +2418,8 @@ def _get_favorite_slot_options(): continue if param_data.get("ui_type") != "toggle" or param_data.get("data_type") != "bool": continue + if key == "AlphaLongitudinalEnabled" and not _get_alpha_longitudinal_available(): + continue seen.add(key) options.append({ @@ -3214,6 +3216,30 @@ def _get_has_radar(): except Exception: return False +def _get_vehicle_parked(): + try: + sm = messaging.SubMaster(["carState"], poll="carState") + sm.update(100) + if not sm.seen["carState"] or not sm.alive["carState"] or not sm.valid["carState"]: + return False + + gear_shifter = getattr(getattr(car, "CarState", None), "GearShifter", None) + park_value = getattr(gear_shifter, "park", None) + return park_value is not None and getattr(sm["carState"], "gearShifter", None) == park_value + except Exception: + return False + +def _get_alpha_longitudinal_available(): + cp_bytes = _safe_params_get_live_raw("CarParamsPersistent") + if not cp_bytes: + return False + + try: + with car.CarParams.from_bytes(cp_bytes) as cp: + return bool(getattr(cp, "alphaLongitudinalAvailable", False)) + except Exception: + return False + def _get_hardware_snapshot_items(): starpilot_toggles = _get_starpilot_toggles_snapshot() @@ -4492,6 +4518,34 @@ def setup(app): "updated": updated, }), 200 + if key == "AlphaLongitudinalEnabled": + if not _get_alpha_longitudinal_available(): + return jsonify({"error": "Alpha Longitudinal is not available for the detected vehicle."}), 403 + if params.get_bool("IsOnroad"): + return jsonify({"error": "Cannot change Alpha Longitudinal while driving."}), 403 + + enabled = str_val.strip() in ("1", "true", "True") + params.put_bool(key, enabled) + params.put_bool("OnroadCycleRequested", True) + update_starpilot_toggles() + return jsonify({ + "message": f"Parameter '{key}' updated successfully. The driving stack will restart shortly.", + "updated": {key: enabled}, + }), 200 + + if key == "ForceOffroad": + if not _get_vehicle_parked(): + return jsonify({"error": "Force Offroad is only available while the vehicle is in Park."}), 403 + + enabled = str_val.strip() in ("1", "true", "True") + params.put_bool("ForceOffroad", enabled) + params.put_bool("ForceOnroad", False) + update_starpilot_toggles() + return jsonify({ + "message": f"Force Offroad {'enabled' if enabled else 'disabled'}.", + "updated": {"ForceOffroad": enabled, "ForceOnroad": False}, + }), 200 + # 1. Prevent changing the model or reboot-required toggles while the car is actively driving reboot_keys = {"Model", "DrivingModel", "AlwaysOnLateral", "DisableOpenpilotLongitudinal", "ForceTorqueController", "NNFF", "NNFFLite"} if key in reboot_keys and params.get_bool("IsOnroad"): @@ -4861,6 +4915,8 @@ def setup(app): result[key] = None result["HasRadar"] = _get_has_radar() + result["VehicleParked"] = _get_vehicle_parked() + result["AlphaLongitudinalAvailable"] = _get_alpha_longitudinal_available() return jsonify(_sanitize_json_value(result)), 200 diff --git a/system/hardware/chestnut/firmware_wrapped.bin b/system/hardware/chestnut/firmware_wrapped.bin new file mode 100644 index 000000000..1d738e4a7 Binary files /dev/null and b/system/hardware/chestnut/firmware_wrapped.bin differ diff --git a/system/hardware/chestnut/flash.py b/system/hardware/chestnut/flash.py new file mode 100755 index 000000000..a603a9948 --- /dev/null +++ b/system/hardware/chestnut/flash.py @@ -0,0 +1,577 @@ +#!/usr/bin/env python3 +"""chestnut (ASM2464) SPI flasher using data-USB EP0 control transfers.""" +import argparse +import ctypes +import errno +import fcntl +import glob +import hashlib +import os +import re +import signal +import struct +import sys +import time +import zlib +from pathlib import Path + +VID_PIDS = (("add1", "0001"), ("3801", "0001")) +ROM_VID_PIDS = (("174c", "2464"), ("174c", "2463")) +ROM_PRODUCT = "USB 3.2 PCIe TinyEnclosure" +FIRMWARE_PATH = Path(__file__).with_name("firmware_wrapped.bin") +CONFIG_DIR = "/data/chestnut_config" +PM_PATHS = ("/sys/bus/platform/devices/a600000.ssusb", "/sys/bus/usb/devices/usb4") +VBUS_PATH = "/sys/kernel/debug/regulator/smb2-vbus/enable" +IMAGE_OFFSET = 0x100 +SECTOR, PAGE = 4096, 128 +MAX_CODE_SIZE = 0x10000 +FLASH_BUDGET = 600.0 +USBDEVFS_CONTROL = 0xC0185500 +USBDEVFS_BULK = 0xC0185502 +USBDEVFS_SETINTERFACE = 0x80085504 +USBDEVFS_SETCONFIGURATION = 0x80045505 +USBDEVFS_CLAIMINTERFACE = 0x8004550F +USBDEVFS_RESET = 0x5514 +USBDEVFS_CLEAR_HALT = 0x80045515 + +_deadline = float("inf") + + +def check_budget(): + if time.monotonic() > _deadline: + raise TimeoutError(f"flash did not converge within {FLASH_BUDGET:g}s") + + +class Ctrl(ctypes.Structure): + _fields_ = [("request_type", ctypes.c_uint8), ("request", ctypes.c_uint8), + ("value", ctypes.c_uint16), ("index", ctypes.c_uint16), + ("length", ctypes.c_uint16), ("timeout", ctypes.c_uint32), + ("data", ctypes.c_void_p)] + + +class Bulk(ctypes.Structure): + _fields_ = [("ep", ctypes.c_uint), ("len", ctypes.c_uint), + ("timeout", ctypes.c_uint), ("data", ctypes.c_void_p)] + + +class RomFallback(Exception): + pass + + +def find_chestnut(): + found = [] + for d in glob.glob("/sys/bus/usb/devices/*"): + try: + vid_pid = (open(d + "/idVendor").read().strip(), open(d + "/idProduct").read().strip()) + if vid_pid in VID_PIDS + ROM_VID_PIDS: + found.append((d, vid_pid, open(d + "/product").read().strip())) + except OSError: + pass + if len(found) > 1: + raise RuntimeError(f"expected one chestnut, found {len(found)}") + return found[0] if found else (None, None, None) + + +def in_rom_bootloader(vid_pid, product): + # the ROM bootloader reports the config page strings, or its own when the config page is lost + return vid_pid in ROM_VID_PIDS or product == ROM_PRODUCT or (product or "").startswith("AS2462") + + +def disable_runtime_pm(path): + control = os.path.join(path, "power/control") + if not os.path.exists(control): + return + with open(control, "w") as f: + f.write("on\n") + if open(control).read().strip() != "on": + raise RuntimeError(f"could not disable USB runtime PM: {control}") + delay = os.path.join(path, "power/autosuspend_delay_ms") + if os.path.exists(delay): + with open(delay, "w") as f: + f.write("-1\n") + + +def unbind_drivers(path): + for interface in glob.glob(path + ":*"): + driver = interface + "/driver" + if os.path.islink(driver): + with open(os.path.realpath(driver) + "/unbind", "w") as f: + f.write(os.path.basename(interface)) + + +def open_device(path): + bus, dev = int(open(path + "/busnum").read()), int(open(path + "/devnum").read()) + return os.open(f"/dev/bus/usb/{bus:03d}/{dev:03d}", os.O_RDWR) + + +def link_up() -> bool: + # asm enumerates on USB-C alone, gpu is only usable once pcie link is up + try: + path, _, _ = find_chestnut() + if path is None: + return False + fd = open_device(path) + except (OSError, RuntimeError): + return False + try: + fcntl.ioctl(fd, USBDEVFS_CONTROL, Ctrl(0x40, 0xF3, 1, 0, 0, 2000, None)) + buf = (ctypes.c_ubyte * 1)() + fcntl.ioctl(fd, USBDEVFS_CONTROL, Ctrl(0xC0, 0xE4, 0xB450, 0, 1, 1000, ctypes.cast(buf, ctypes.c_void_p))) + return buf[0] == 0x78 # LTSSM L0 + except OSError: + return False + finally: + os.close(fd) + + +def claim_interface(path, setup=False): + # unbind usb-storage, which binds to the ROM bootloader + disable_runtime_pm(path) + unbind_drivers(path) + fd = open_device(path) + try: + if setup: + fcntl.ioctl(fd, USBDEVFS_SETCONFIGURATION, struct.pack("I", 1)) + fcntl.ioctl(fd, USBDEVFS_CLAIMINTERFACE, struct.pack("I", 0)) + if setup: + fcntl.ioctl(fd, USBDEVFS_SETINTERFACE, struct.pack("II", 0, 0)) + except OSError as e: + os.close(fd) + if e.errno == errno.EBUSY: + raise RuntimeError("chestnut is in use, stop modeld/GPU processes before flashing") from e + raise + return fd + + +class Flash: + def __init__(self): + self.fd = -1 + + def close(self): + if self.fd >= 0: + os.close(self.fd) + self.fd = -1 + + def connect(self, timeout=5.0): + self.close() + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + path, vid_pid, product = find_chestnut() + if in_rom_bootloader(vid_pid, product): + raise RomFallback("chestnut fell back to the ROM bootloader") + if path is not None: + self.fd = claim_interface(path) + return + time.sleep(0.1) + raise RuntimeError(f"chestnut did not enumerate within {timeout:g}s") + + def reg_write(self, addr, value): + fcntl.ioctl(self.fd, USBDEVFS_CONTROL, + Ctrl(0x40, 0xE5, addr & 0xFFFF, value & 0xFFFF, 0, 2000, None)) + + def reg_read(self, addr, length=1): + buf = (ctypes.c_ubyte * length)() + fcntl.ioctl(self.fd, USBDEVFS_CONTROL, + Ctrl(0xC0, 0xE4, addr & 0xFFFF, 0, length, 2000, ctypes.cast(buf, ctypes.c_void_p))) + return bytes(buf) + + def write_buffer(self, data): + for i, value in enumerate(data): + self.reg_write(0x7000 + i, value) + + def wait_controller(self, timeout=2.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if not self.reg_read(0xC8A9)[0] & 1: + return + raise TimeoutError("flash controller timeout") + + def transaction(self, command, addr=0, length=0, addr_len=0x07, mode=0): + for reg, value in ((0xC8AD, mode), (0xC8AE, 0), (0xC8AF, 0), (0xC8AA, command), (0xC8AC, addr_len), + (0xC8A1, addr), (0xC8A2, addr >> 8), (0xC8AB, addr >> 16), (0xC8A3, length >> 8), (0xC8A4, length)): + self.reg_write(reg, value & 0xFF) + self.reg_write(0xC8A9, 1) + self.wait_controller() + for _ in range(4): + self.reg_write(0xC8AD, 0) + + def write_enable(self): + for reg, value in ((0xC8AD, 0), (0xC8AA, 0x06), (0xC8AC, 0x04), (0xC8A3, 0), (0xC8A4, 0), (0xC8A9, 1)): + self.reg_write(reg, value) + self.wait_controller() + + def status(self): + self.transaction(0x05, length=1, addr_len=0x04) + return self.reg_read(0x7000)[0] + + def wait_write_done(self, timeout=10.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if not self.status() & 1: + return + time.sleep(0.005) + raise TimeoutError("SPI flash WIP timeout") + + def init(self): + self.reg_write(0xCC33, 0x04) + self.reg_write(0xCA81, self.reg_read(0xCA81)[0] | 1) + self.reg_write(0xC805, 0x02) + self.reg_write(0xC8A6, 0x04) + for _ in range(5): + self.write_enable() + self.write_buffer(bytes(4)) + self.transaction(0x01, length=1, addr_len=0x04, mode=1) + time.sleep(0.01) + if not self.status() & 0x1C: + return + raise RuntimeError("could not clear SPI block protection") + + def read(self, addr, length): + out = bytearray() + while len(out) < length: + n = min(4096, length - len(out)) + self.transaction(0x03, addr + len(out), max(4096, n)) + for off in range(0, n, 255): + out += self.reg_read(0x7000 + off, min(255, n - off)) + return bytes(out) + + def erase_sector(self, addr): + self.write_enable() + self.transaction(0x20, addr) + self.wait_write_done() + + def program(self, addr, data): + self.write_buffer(data + bytes((-len(data)) % 4)) + self.write_enable() + self.transaction(0x02, addr, len(data), mode=1) + self.wait_write_done() + + +def validate_image(data): + if len(data) < 10: + raise ValueError("wrapped firmware is too short") + body_len = int.from_bytes(data[:4], "little") + if body_len > MAX_CODE_SIZE: + raise ValueError(f"wrapped firmware body exceeds {MAX_CODE_SIZE} bytes") + if len(data) != body_len + 10 or data[4 + body_len] != 0xA5: + raise ValueError("invalid wrapped firmware length or magic") + body = data[4:4 + body_len] + if data[5 + body_len] != sum(body) & 0xFF: + raise ValueError("invalid wrapped firmware checksum") + if data[6 + body_len:] != zlib.crc32(body).to_bytes(4, "little"): + raise ValueError("invalid wrapped firmware CRC") + + +def image_product(image): + match = re.search(rb"custom [0-9a-f]{8}-CLEAN", image) + if match is None: + raise ValueError("no product string in wrapped firmware") + return match.group().decode() + + +def reconnect(flash): + attempt = 0 + while True: + attempt += 1 + check_budget() + try: + flash.connect() + flash.init() + return + except (OSError, TimeoutError, RuntimeError) as e: + print(f"waiting for chestnut (attempt {attempt}): {e}", flush=True) + time.sleep(1) + + +def with_retries(flash, label, operation): + # on any transfer error, reconnect and restart the operation + attempt = 0 + while True: + attempt += 1 + try: + return operation() + except (OSError, TimeoutError, RuntimeError) as e: + check_budget() + print(f"{label} attempt {attempt}: {e}", flush=True) + reconnect(flash) + + +def stable_read(flash, addr, length, count=2): + def read(): + reads = [flash.read(addr, length) for _ in range(count)] + if any(x != reads[0] for x in reads[1:]): + raise RuntimeError(f"unstable flash read at 0x{addr:05x}") + return reads[0] + return with_retries(flash, f"read 0x{addr:05x}", read) + + +def program_sector(flash, addr, target): + def program(): + flash.erase_sector(addr) + if flash.read(addr, SECTOR) != bytes([0xFF]) * SECTOR: + raise RuntimeError("sector erase verification failed") + for off in range(0, SECTOR, PAGE): + chunk = target[off:off + PAGE] + if chunk != bytes([0xFF]) * len(chunk): + flash.program(addr + off, chunk) + if flash.read(addr + off, len(chunk)) != chunk: + raise RuntimeError(f"page verify failed at 0x{addr + off:05x}") + if flash.read(addr, SECTOR) != target: + raise RuntimeError("sector verification failed") + with_retries(flash, f"sector 0x{addr:05x}", program) + + +def config_path(): + return os.path.join(CONFIG_DIR, f"{os.uname().nodename}.bin") + + +def saved_config(path, data): + os.makedirs(os.path.dirname(path), exist_ok=True) + try: + fd = os.open(path, os.O_WRONLY | os.O_CREAT | os.O_EXCL, 0o600) + except FileExistsError as e: + backup = open(path, "rb").read() + if len(backup) != 0x100: + raise RuntimeError(f"invalid config backup: {path}") from e + if backup != data: + print(f"restoring config from {path}", flush=True) + return backup + with os.fdopen(fd, "wb") as f: + f.write(data) + f.flush() + os.fsync(f.fileno()) + return data + + +def rom_write(image, config): + # the ROM bootloader implements only the BOT protocol, and requires a port reset before bulk transfers + path, _, _ = find_chestnut() + if path is None: + raise RuntimeError("chestnut disappeared before recovery") + unbind_drivers(path) + fd = open_device(path) + try: + fcntl.ioctl(fd, USBDEVFS_RESET) + finally: + os.close(fd) + time.sleep(3) + path, _, _ = find_chestnut() + if path is None: + raise RuntimeError("chestnut did not re-enumerate after reset") + fd = claim_interface(path, setup=True) + for ep in (0x02, 0x81): + fcntl.ioctl(fd, USBDEVFS_CLEAR_HALT, struct.pack("I", ep)) + tag = 0 + + def bulk(ep, payload, timeout): + buf = ctypes.create_string_buffer(bytes(payload), len(payload)) + fcntl.ioctl(fd, USBDEVFS_BULK, Bulk(ep, len(payload), timeout, ctypes.cast(buf, ctypes.c_void_p))) + return buf.raw + + def cmd(cdb, data=b"", timeout=30000): + nonlocal tag + tag += 1 + bulk(0x02, struct.pack("BBB12x", 0xE1, 0x50, 0), config[:0x80]) + cmd(struct.pack(">BBB12x", 0xE1, 0x50, 1), config[0x80:]) + cmd(struct.pack(">BBI", 0xE3, 0x50, min(len(image), 0xFF00)), image[:0xFF00]) + if len(image) > 0xFF00: + cmd(struct.pack(">BBI", 0xE3, 0xD0, len(image) - 0xFF00), image[0xFF00:]) + cmd(struct.pack(">BB13x", 0xE8, 0x51)) + finally: + os.close(fd) + print("recovery flash done", flush=True) + + +def vbus_write(value): + try: + with open(VBUS_PATH, "w") as f: + f.write(value + "\n") + except OSError: + pass + + +def vbus_cycle(): + if os.path.exists(VBUS_PATH): + vbus_write("0") + time.sleep(2) + vbus_write("1") + time.sleep(5) + + +def activate(expected_product): + if not os.path.exists(VBUS_PATH): + print("no VBUS control, firmware activates on the next chestnut power cycle", flush=True) + return + print("power-cycling chestnut VBUS", flush=True) + vbus_write("0") + disconnected = False + deadline = time.monotonic() + 5.0 + while time.monotonic() < deadline: + path, _, _ = find_chestnut() + if path is None: + disconnected = True + break + time.sleep(0.2) + time.sleep(1) + vbus_write("1") + if not disconnected: + print("chestnut stayed powered, firmware activates on its next power cycle", flush=True) + return + deadline = time.monotonic() + 15.0 + while time.monotonic() < deadline: + _, _, product = find_chestnut() + if product is not None: + if product == expected_product: + print(f"activated {expected_product}", flush=True) + else: + print(f"chestnut re-enumerated with {product!r}, firmware activates on its next power cycle", flush=True) + return + time.sleep(0.2) + print("chestnut did not re-enumerate, firmware activates on its next power cycle", flush=True) + + +def defer_signal(signum, _frame): + # writing from a handler must not reenter a print already in progress + os.write(1, f"signal {signum} deferred until the chestnut is powered back up\n".encode()) + + +def flash_chestnut(expected_version=None, force=False): + global _deadline + + image = FIRMWARE_PATH.read_bytes() + validate_image(image) + expected_product = image_product(image) + if expected_version is not None and expected_product != f"custom {expected_version}-CLEAN": + raise RuntimeError(f"bundled firmware is {expected_product!r}, expected version {expected_version}") + + path, vid_pid, product = find_chestnut() + if path is None: + print("no chestnut connected", flush=True) + return + if product == expected_product and not force: + print(f"chestnut firmware is up to date ({expected_product})", flush=True) + return + + _deadline = time.monotonic() + FLASH_BUDGET + for pm_path in PM_PATHS: + disable_runtime_pm(pm_path) + + previous = {sig: signal.signal(sig, defer_signal) for sig in (signal.SIGINT, signal.SIGTERM, signal.SIGHUP)} + try: + if in_rom_bootloader(vid_pid, product): + if not recover_from_rom(image, expected_product): + return + # firmware is back, verify it against the bundled image + force, product = True, None + write_image(image, expected_product, product, force) + finally: + for sig, handler in previous.items(): + signal.signal(sig, handler) + + +def recover_from_rom(image, expected_product): + # returns whether the chestnut came back on custom firmware + backup = config_path() + if not os.path.isfile(backup): + raise RuntimeError(f"cannot recover from the ROM bootloader without a config backup at {backup}") + config = open(backup, "rb").read() + if len(config) != 0x100: + raise RuntimeError(f"invalid config backup: {backup}") + + committed = False + while True: + check_budget() + path, vid_pid, product = find_chestnut() + if path is None: + if committed: + print("chestnut is offline, recovered firmware boots on its next power cycle", flush=True) + return False + vbus_cycle() + continue + if not in_rom_bootloader(vid_pid, product): + return True + if committed: + print("chestnut stayed powered, recovered firmware boots on its next power cycle", flush=True) + return False + try: + rom_write(image, config) + committed = True + except (OSError, TimeoutError, RuntimeError) as e: + print(f"ROM recovery failed, retrying: {e}", flush=True) + vbus_cycle() + continue + activate(expected_product) + + +def write_image(image, expected_product, product, force): + if force: + print(f"forced reflash of {expected_product}", flush=True) + else: + print(f"chestnut firmware mismatch: {product!r}; expected {expected_product!r}", flush=True) + + flash = Flash() + try: + reconnect(flash) + config = stable_read(flash, 0, 0x100, 3) + config = saved_config(config_path(), config) + image_end = IMAGE_OFFSET + len(image) + first_sector = IMAGE_OFFSET & ~(SECTOR - 1) + span = (image_end + SECTOR - 1) & ~(SECTOR - 1) + current = stable_read(flash, first_sector, span - first_sector) + target = bytearray(current) + target[:len(config)] = config + target[IMAGE_OFFSET - first_sector:image_end - first_sector] = image + target = bytes(target) + print(f"target {len(image)} bytes at 0x{IMAGE_OFFSET:05x}, sha256={hashlib.sha256(image).hexdigest()}", flush=True) + + for addr in range(first_sector, span, SECTOR): + off = addr - first_sector + wanted = target[off:off + SECTOR] + if current[off:off + SECTOR] == wanted: + print(f"sector 0x{addr:05x}: unchanged", flush=True) + else: + print(f"sector 0x{addr:05x}: programming", flush=True) + program_sector(flash, addr, wanted) + + verified = stable_read(flash, first_sector, span - first_sector, 3) + if verified != target: + raise RuntimeError("final full-image verification failed") + print(f"verified sha256={hashlib.sha256(verified).hexdigest()}", flush=True) + finally: + flash.close() + + activate(expected_product) + + +def main(): + parser = argparse.ArgumentParser(description="check and flash the bundled chestnut firmware") + parser.add_argument("version", nargs="?", help="expected firmware version hash") + parser.add_argument("--force", action="store_true", help="reflash even when the version matches") + args = parser.parse_args() + if os.geteuid() != 0: + raise RuntimeError("flash.py must run as root") + flash_chestnut(expected_version=args.version, force=args.force) + + +if __name__ == "__main__": + try: + main() + except Exception as e: + print(f"FAIL: {type(e).__name__}: {e}", file=sys.stderr) + sys.exit(1) + diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 2a6bec5b1..a07a47e03 100644 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -3,6 +3,8 @@ import fcntl import os import queue import struct +import subprocess +import sys import threading import time from collections import OrderedDict, namedtuple @@ -23,6 +25,15 @@ from openpilot.system.statsd import statlog from openpilot.common.swaglog import cloudlog from openpilot.system.hardware.power_monitoring import PowerMonitoring from openpilot.system.hardware.fan_controller import TiciFanController +from openpilot.system.hardware.usb import ( + CHESTNUT_FW_VERSION, + CHESTNUT_PRODUCT_ID, + CHESTNUT_ROM_USB_IDS, + CHESTNUT_VENDOR_IDS, + read_int, + read_text, + usb_devices, +) from openpilot.system.version import terms_version, training_version from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID @@ -37,6 +48,56 @@ DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect PANDA_STATES_TIMEOUT = round(1000 / SERVICE_LIST['pandaStates'].frequency * 1.5) # 1.5x the expected pandaState frequency ONROAD_CYCLE_TIME = 1 # seconds to wait offroad after requesting an onroad cycle + +class Chestnut: + """Keep the ASM2464PD dock on the firmware expected by the GPU runtime.""" + MAX_ATTEMPTS = 3 + RETRY_INTERVAL = 20.0 + + def __init__(self): + self.thread: threading.Thread | None = None + self.attempts = 0 + self.last_attempt = 0.0 + self.flashed = False + + def _firmware_mismatch(self) -> bool: + expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN" + ids = tuple((vendor, CHESTNUT_PRODUCT_ID) for vendor in CHESTNUT_VENDOR_IDS) + CHESTNUT_ROM_USB_IDS + for device in usb_devices(): + usb_id = (read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) + if usb_id in ids and read_text(device / "product") != expected: + return True + return False + + def _flash(self) -> None: + script = os.path.join(os.path.dirname(__file__), "chestnut", "flash.py") + result = subprocess.run( + ["sudo", sys.executable, script, CHESTNUT_FW_VERSION], + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + check=False, + ) + cloudlog.event("chestnut flash done", returncode=result.returncode, output=result.stdout[-1000:], error=result.returncode != 0) + self.flashed = result.returncode == 0 + + def update(self, offroad: bool) -> None: + if not self._firmware_mismatch(): + self.flashed = False + return + if not offroad or self.flashed or self.attempts >= self.MAX_ATTEMPTS: + return + if self.thread is not None and self.thread.is_alive(): + return + if time.monotonic() - self.last_attempt < self.RETRY_INTERVAL: + return + + self.attempts += 1 + self.last_attempt = time.monotonic() + cloudlog.warning(f"chestnut firmware out of date, flashing (attempt {self.attempts})") + self.thread = threading.Thread(target=self._flash, name="chestnut_flash", daemon=True) + self.thread.start() + ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats', 'network_metered', 'modem_temps']) @@ -204,6 +265,7 @@ def hardware_thread(end_event, hw_queue) -> None: params = Params() power_monitor = PowerMonitoring() + chestnut = Chestnut() if AGNOS else None uptime_offroad: float = params.get("UptimeOffroad", return_default=True) uptime_onroad: float = params.get("UptimeOnroad", return_default=True) @@ -223,6 +285,9 @@ def hardware_thread(end_event, hw_queue) -> None: while not end_event.is_set(): sm.update(PANDA_STATES_TIMEOUT) + if chestnut is not None: + chestnut.update(started_ts is None) + pandaStates = sm['pandaStates'] peripheralState = sm['peripheralState'] peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown diff --git a/system/hardware/usb.py b/system/hardware/usb.py index 0c847b91c..8c2d2a79c 100644 --- a/system/hardware/usb.py +++ b/system/hardware/usb.py @@ -3,6 +3,8 @@ from pathlib import Path CHESTNUT_VENDOR_ID = 0xADD1 CHESTNUT_VENDOR_IDS = (CHESTNUT_VENDOR_ID, 0x3801) CHESTNUT_PRODUCT_ID = 0x0001 +CHESTNUT_FW_VERSION = "ed4e39b7" +CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463)) USB_DEVICES_PATH = Path("/sys/bus/usb/devices") @@ -13,6 +15,13 @@ def read_int(path: Path, base: int = 10) -> int: return 0 +def read_text(path: Path) -> str: + try: + return path.read_text().strip() + except OSError: + return "" + + def usb_devices() -> list[Path]: try: devices = (path for path in USB_DEVICES_PATH.glob("*") if (path / "idVendor").exists()) @@ -29,6 +38,16 @@ def chestnut_present() -> bool: ) +def chestnut_firmware_ready() -> bool: + expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN" + return any( + read_int(device / "idVendor", 16) in CHESTNUT_VENDOR_IDS and + read_int(device / "idProduct", 16) == CHESTNUT_PRODUCT_ID and + read_text(device / "product") == expected + for device in usb_devices() + ) + + def controller(device: Path) -> Path | None: try: return next((parent for parent in device.resolve().parents if parent.name.endswith(".ssusb")), None) diff --git a/system/manager/launch_param_migrations.py b/system/manager/launch_param_migrations.py index 35c9ede7d..888cb2384 100644 --- a/system/manager/launch_param_migrations.py +++ b/system/manager/launch_param_migrations.py @@ -21,6 +21,7 @@ DEVELOPER_METRIC_DISPLAY_KEYS = ( "SidebarMetrics", ) DEVICE_SHUTDOWN_KEY = "DeviceShutdown" +CAMERA_VIEW_KEY = "CameraView" DEFAULT_STEER_KP = 0.6 LEGACY_STEER_KP = 0.7 @@ -36,6 +37,7 @@ DEVELOPER_METRIC_DISPLAY_MIGRATION_MARKER = ".starpilot_developer_metric_display LANE_CHANGE_SMOOTHING_MIGRATION_MARKER = ".starpilot_lane_change_smoothing_default_v1" SPEED_LIMIT_VISIBILITY_MIGRATION_MARKER = ".starpilot_speed_limit_visibility_v1" DEVICE_SHUTDOWN_HOURS_MIGRATION_MARKER = ".starpilot_device_shutdown_hours_v1" +CAMERA_VIEW_DEFAULT_MIGRATION_MARKER = ".starpilot_camera_view_default_v1" MARKER_DIRNAME = ".starpilot_param_migrations" LATERAL_METHOD_PARAM_SUFFIXES = ( @@ -54,9 +56,11 @@ LEGACY_RELAXED_FOLLOW_HIGH_DEFAULT = 1.75 LEGACY_JERK_DEFAULT = 50.0 LEGACY_ACCELERATION_PROFILE_DEFAULT = 2 LEGACY_LANE_CHANGE_SMOOTHING_DEFAULT = 10 +LEGACY_CAMERA_VIEW_DEFAULT = 3 STANDARD_ACCELERATION_PROFILE = 0 DEFAULT_LANE_CHANGE_SMOOTHING = 5 +DEFAULT_CAMERA_VIEW = 2 BRANCH_BOOL_MIGRATIONS = { "CEStoppedLead": (LEGACY_CE_STOPPED_LEAD_DEFAULT, False), @@ -137,6 +141,10 @@ def _device_shutdown_hours_marker_path(params: ParamsLike) -> Path: return _marker_dir_path(params) / DEVICE_SHUTDOWN_HOURS_MIGRATION_MARKER +def _camera_view_default_marker_path(params: ParamsLike) -> Path: + return _marker_dir_path(params) / CAMERA_VIEW_DEFAULT_MIGRATION_MARKER + + def _marker_dir_path(params: ParamsLike) -> Path: params_path = Path(params.get_param_path()) # Params.clear_all() removes unknown files inside the params directory, so @@ -310,6 +318,18 @@ def _apply_device_shutdown_hours_migration(params: ParamsLike, marker: Path) -> marker.touch() +def _apply_camera_view_default_migration(params: ParamsLike, marker: Path) -> None: + if marker.exists(): + return + + marker.parent.mkdir(parents=True, exist_ok=True) + + if _should_migrate_int_param(params, CAMERA_VIEW_KEY, LEGACY_CAMERA_VIEW_DEFAULT): + params.put_int(CAMERA_VIEW_KEY, DEFAULT_CAMERA_VIEW) + + marker.touch() + + def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = None, branch_defaults_marker_path: Path | None = None, acceleration_profile_marker_path: Path | None = None, @@ -319,7 +339,8 @@ def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = developer_metric_display_marker_path: Path | None = None, lane_change_smoothing_marker_path: Path | None = None, speed_limit_visibility_marker_path: Path | None = None, - device_shutdown_hours_marker_path: Path | None = None) -> None: + device_shutdown_hours_marker_path: Path | None = None, + camera_view_default_marker_path: Path | None = None) -> None: _apply_legacy_launch_param_migrations(params, marker_path or _default_marker_path(params)) # Keep branch-default rollout on its own marker so older installs that already # have the legacy marker still receive this one-time param reset. @@ -346,6 +367,9 @@ def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = _apply_device_shutdown_hours_migration( params, device_shutdown_hours_marker_path or _device_shutdown_hours_marker_path(params) ) + _apply_camera_view_default_migration( + params, camera_view_default_marker_path or _camera_view_default_marker_path(params) + ) def main() -> int: diff --git a/system/manager/test/test_launch_param_migrations.py b/system/manager/test/test_launch_param_migrations.py index dd5368f57..f2579565f 100644 --- a/system/manager/test/test_launch_param_migrations.py +++ b/system/manager/test/test_launch_param_migrations.py @@ -3,6 +3,8 @@ from pathlib import Path from openpilot.system.manager.launch_param_migrations import ( ACCELERATION_PROFILE_MIGRATION_MARKER, BRANCH_DEFAULTS_MIGRATION_MARKER, + CAMERA_VIEW_DEFAULT_MIGRATION_MARKER, + DEFAULT_CAMERA_VIEW, DEVELOPER_METRIC_DISPLAY_KEYS, DEVELOPER_METRIC_DISPLAY_MIGRATION_MARKER, DEFAULT_LANE_CHANGE_SMOOTHING, @@ -173,6 +175,25 @@ def test_apply_launch_param_migrations_does_not_reapply_device_shutdown_conversi assert params.get_int("DeviceShutdown") == 9 +def test_apply_launch_param_migrations_updates_legacy_camera_view_default_once(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + params.put_int("CameraView", 3) + + apply_launch_param_migrations(params) + + assert params.get_int("CameraView") == DEFAULT_CAMERA_VIEW + assert marker_path(tmp_path, CAMERA_VIEW_DEFAULT_MIGRATION_MARKER).is_file() + + +def test_apply_launch_param_migrations_preserves_custom_camera_view(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + params.put_int("CameraView", 0) + + apply_launch_param_migrations(params) + + assert params.get_int("CameraView") == 0 + + def test_apply_launch_param_migrations_applies_branch_defaults_for_existing_installs(tmp_path): params = FileBackedFakeParams(tmp_path / "params") diff --git a/system/ui/README.md b/system/ui/README.md index 3b51ab9e6..696cd4f63 100644 --- a/system/ui/README.md +++ b/system/ui/README.md @@ -9,9 +9,8 @@ Quick start: * set `SCALE=1.5` to scale the entire UI by 1.5x * set `BURN_IN=1` to get a burn-in heatmap version of the UI * burn-in prevention shifts the UI by 2 pixels every 3 minutes on device; set `BURN_IN_PREVENTION=0` to disable it - or tune it with `BURN_IN_SHIFT_PIXELS` and `BURN_IN_SHIFT_INTERVAL` (seconds); on TICI/TIZI, near-white pixels are also softly capped - at 95% luminance and can be tuned or disabled with `WHITE_LUMINANCE_CAP` (set it to `1.0` to disable). MICI uses direct shifting - by default, while enabling a luminance cap or forced render texture opts it into the offscreen path. + or tune it with `BURN_IN_SHIFT_PIXELS` and `BURN_IN_SHIFT_INTERVAL` (seconds). TICI/TIZI and MICI use direct shifting + by default; setting `WHITE_LUMINANCE_CAP` below `1.0` enables the optional luminance cap and offscreen presentation path. * set `MICI_FORCE_RENDER_TEXTURE=1` to force the C4 UI through the offscreen presentation path for diagnostics * set `GRID=50` to show a 50-pixel alignment grid overlay * set `MAGIC_DEBUG=1` to show every dropped frames (only on device) diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 3bdf90e01..997172a21 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -43,7 +43,7 @@ BURN_IN_PREVENTION = os.getenv("BURN_IN_PREVENTION", "0" if PC else "1") == "1" BURN_IN_SHIFT_INTERVAL = max(1.0, float(os.getenv("BURN_IN_SHIFT_INTERVAL", "180"))) BURN_IN_SHIFT_PIXELS = max(0, int(os.getenv("BURN_IN_SHIFT_PIXELS", "2"))) WHITE_LUMINANCE_CAP = min(1.0, max(0.0, float(os.getenv( - "WHITE_LUMINANCE_CAP", "0.95" if BURN_IN_PREVENTION and DEVICE_TYPE != "mici" else "1.0" + "WHITE_LUMINANCE_CAP", "1.0" )))) SHOW_FPS = os.getenv("SHOW_FPS") == "1" SHOW_TOUCHES = os.getenv("SHOW_TOUCHES") == "1" @@ -618,7 +618,7 @@ class GuiApplication: needs_render_texture = ((self._scale != 1.0 and not PC) or BURN_IN_MODE or RECORD or MICI_FORCE_RENDER_TEXTURE or - (BURN_IN_PREVENTION and DEVICE_TYPE != "mici") or WHITE_LUMINANCE_CAP < 1.0) + WHITE_LUMINANCE_CAP < 1.0) if PC and self._scale != 1.0: rl.set_mouse_scale(1 / self._scale, 1 / self._scale) if PC: @@ -807,6 +807,10 @@ class GuiApplication: if self._progress_hook is not None: self._progress_hook(phase) + def mark_progress(self, phase: str) -> None: + """Expose lightweight phase markers to complex widgets.""" + self._mark_progress(phase) + def set_should_render(self, should_render: bool): self._should_render = should_render if should_render: