From 1863259a58ee290f06c0cb9cdfeef61b08472039 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 16 Jul 2026 23:51:45 -0500 Subject: [PATCH] The Night's Watch --- .../opendbc/car/chrysler/interface.py | 9 +- .../opendbc/car/tests/test_car_interfaces.py | 29 +++- selfdrive/controls/lib/latcontrol_torque.py | 8 + .../controls/lib/latcontrol_vehicle_tunes.py | 41 ++++- selfdrive/controls/tests/test_latcontrol.py | 29 ++++ selfdrive/ui/mici/onroad/cameraview.py | 43 +++-- .../ui/mici/tests/test_camera_cleanup.py | 72 ++++++++ selfdrive/ui/onroad/cameraview.py | 45 +++-- .../ui/onroad/starpilot/developer_sidebar.py | 125 +++++++++++--- selfdrive/ui/ui_state.py | 6 + starpilot/common/starpilot_functions.py | 4 +- starpilot/common/starpilot_variables.py | 40 +++-- .../common/tests/test_starpilot_functions.py | 45 +++++ .../common/tests/test_starpilot_variables.py | 24 +++ starpilot/system/speed_limit_vision.py | 12 +- .../system/tests/test_speed_limit_vision.py | 13 ++ starpilot/ui/qt/widgets/developer_sidebar.cc | 155 ++++++++++++++++-- system/manager/manager.py | 8 +- system/manager/test/test_manager.py | 6 + 19 files changed, 622 insertions(+), 92 deletions(-) create mode 100644 selfdrive/ui/mici/tests/test_camera_cleanup.py diff --git a/opendbc_repo/opendbc/car/chrysler/interface.py b/opendbc_repo/opendbc/car/chrysler/interface.py index ff6612b57..f44a03a16 100755 --- a/opendbc_repo/opendbc/car/chrysler/interface.py +++ b/opendbc_repo/opendbc/car/chrysler/interface.py @@ -5,7 +5,6 @@ from opendbc.car.chrysler.carstate import CarState from opendbc.car.chrysler.radar_interface import RadarInterface from opendbc.car.chrysler.values import CAR, JEEPS, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags, ChryslerSafetyFlags from opendbc.car.interfaces import CarInterfaceBase -from openpilot.common.params import Params, UnknownKeyName class CarInterface(CarInterfaceBase): @@ -15,12 +14,6 @@ class CarInterface(CarInterfaceBase): @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: - params = Params() - try: - jeep_brake_hold = params.get_bool("JeepBrakeHold") - except UnknownKeyName: - jeep_brake_hold = False - ret.brand = "chrysler" ret.dashcamOnly = candidate in RAM_HD @@ -35,7 +28,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_HD.value elif candidate in RAM_DT: ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_DT.value - elif candidate in JEEPS and jeep_brake_hold: + elif candidate in JEEPS: ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.JEEP_BRAKE_HOLD.value CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) diff --git a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py index ca56264bd..337f374eb 100644 --- a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py +++ b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py @@ -14,7 +14,7 @@ from opendbc.car.car_helpers import _apply_disable_openpilot_long, interfaces from opendbc.car.chrysler.carcontroller import CarController as ChryslerCarController from opendbc.car.chrysler.carstate import CarState as ChryslerCarState from opendbc.car.chrysler.interface import CarInterface as ChryslerCarInterface -from opendbc.car.chrysler.values import CAR as CHRYSLER_CAR, DBC as CHRYSLER_DBC, ChryslerStarPilotFlags +from opendbc.car.chrysler.values import CAR as CHRYSLER_CAR, DBC as CHRYSLER_DBC, ChryslerSafetyFlags, ChryslerStarPilotFlags from opendbc.car.fingerprints import FW_VERSIONS from opendbc.car.fw_versions import FW_QUERY_CONFIGS from opendbc.car.hyundai.interface import CarInterface as HyundaiCarInterface @@ -213,6 +213,33 @@ class TestCarInterfaces: lkas_parser.update([0, can_sends]) assert lkas_parser.vl["LKAS_COMMAND"]["LKAS_CONTROL_BIT"] == 1 + @pytest.mark.parametrize("candidate", (CHRYSLER_CAR.JEEP_GRAND_CHEROKEE, CHRYSLER_CAR.JEEP_GRAND_CHEROKEE_2019)) + def test_jeep_brake_hold_safety_capability_is_provisioned(self, candidate): + car_params = ChryslerCarInterface.get_params( + candidate, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=get_test_starpilot_toggles(), + ) + + assert car_params.safetyConfigs[0].safetyParam & ChryslerSafetyFlags.JEEP_BRAKE_HOLD.value + + def test_jeep_brake_hold_safety_capability_is_not_provisioned_for_non_jeep(self): + car_params = ChryslerCarInterface.get_params( + CHRYSLER_CAR.CHRYSLER_PACIFICA_2020, + {bus: {} for bus in range(8)}, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=get_test_starpilot_toggles(), + ) + + assert not car_params.safetyConfigs[0].safetyParam & ChryslerSafetyFlags.JEEP_BRAKE_HOLD.value + def test_gm_bolt_gen2_pedal_safety_flags(self): CarInterface = interfaces[GM_CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL] diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index aab258885..002413782 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -86,6 +86,7 @@ class LatControlTorque(LatControl): self.is_kia_niro_phev_2022 = CP.carFingerprint in KIA_NIRO_PHEV_2022_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS + self.is_kia_carnival = CP.carFingerprint in KIA_CARNIVAL_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_silverado = CP.carFingerprint in SILVERADO_CARS self.is_gm = CP.brand == "gm" @@ -231,6 +232,7 @@ class LatControlTorque(LatControl): kia_niro_phev_2022_active = self.is_kia_niro_phev_2022 kia_forte_active = self.is_kia_forte kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() + kia_carnival_active = self.is_kia_carnival volt_plexy_test_active = self.is_volt_standard and volt_plexy_lateral_testing_ground_active() ioniq_5_center_taper = get_ioniq_5_center_taper_scale(setpoint, CS.vEgo) if ioniq_5_active else 1.0 prius_center_taper = get_prius_center_taper_scale(setpoint, CS.vEgo) if prius_active else 1.0 @@ -245,6 +247,7 @@ class LatControlTorque(LatControl): kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_test_active else 1.0 + kia_carnival_center_taper = get_kia_carnival_center_taper_scale(setpoint, CS.vEgo) if kia_carnival_active else 1.0 silverado_center_taper = get_silverado_center_taper_scale(setpoint, CS.vEgo) if self.is_silverado else 1.0 civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if ( self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active() @@ -317,6 +320,9 @@ class LatControlTorque(LatControl): friction_threshold = get_kia_ev6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_kia_ev6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * kia_ev6_center_taper) + elif kia_carnival_active: + friction_threshold = get_kia_carnival_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale *= get_kia_carnival_friction_center_fade_scale(setpoint, CS.vEgo) elif self.is_silverado: ff *= silverado_center_taper elif volt_plexy_test_active: @@ -383,6 +389,8 @@ class LatControlTorque(LatControl): output_torque *= volt_plexy_center_taper elif kia_ev6_test_active: output_torque *= kia_ev6_low_speed_center_taper + elif kia_carnival_active: + output_torque *= kia_carnival_center_taper elif self.is_silverado: output_torque *= silverado_center_taper elif kia_niro_phev_2022_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 7202943b9..8aca6bdaa 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -110,6 +110,9 @@ ELANTRA_NON_SCC_CARS = ( KIA_EV6_CARS = ( HYUNDAI_CAR.KIA_EV6, ) +KIA_CARNIVAL_CARS = ( + HYUNDAI_CAR.KIA_CARNIVAL_2025, +) KIA_XCEED_CARS = ( HYUNDAI_CAR.KIA_XCEED_PHEV, ) @@ -329,6 +332,16 @@ KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22 KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06 KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12 +KIA_CARNIVAL_CENTER_TAPER_MAX = 0.10 +KIA_CARNIVAL_CENTER_TAPER_LAT = 0.16 +KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.05 +KIA_CARNIVAL_CENTER_TAPER_SPEED = 6.5 +KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH = 1.5 +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX = 16.0 +KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH = 2.5 +KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN = 0.18 +KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX = 0.25 + KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT = 1.05 KIA_FORTE_FF_REDUCTION_LEFT = 0.05 KIA_FORTE_FF_REDUCTION_RIGHT = 0.10 @@ -1598,6 +1611,30 @@ def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_acce return base_threshold * min(max(threshold_scale, 1.0), 1.18) +def _kia_carnival_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_onset = _sigmoid((v_ego - KIA_CARNIVAL_CENTER_TAPER_SPEED) / KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH) + speed_cutoff = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH) + speed_weight = speed_onset * speed_cutoff + center_weight = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + + +def get_kia_carnival_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + return 1.0 - (KIA_CARNIVAL_CENTER_TAPER_MAX * speed_weight * center_weight) + + +def get_kia_carnival_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + del desired_lateral_jerk + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + return get_hkg_canfd_base_friction_threshold(v_ego) * (1.0 + KIA_CARNIVAL_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight) + + +def get_kia_carnival_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_carnival_center_weights(desired_lateral_accel, v_ego) + return 1.0 - (KIA_CARNIVAL_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight) + + def _kia_forte_sigmoid(x: float) -> float: return _sigmoid(x) @@ -2724,13 +2761,13 @@ def get_flm_capabilities(car_fingerprint, brand: str = "", hyundai_canfd: bool = dedicated_friction = car_fingerprint in ( set(BOLT_2022_2023_CARS) | set(BOLT_2018_2021_CARS) | set(VOLT_STANDARD_CARS) | set(PALISADE_CARS) | set(PRIUS_CARS) | set(IONIQ_5_CARS) | set(IONIQ_6_CARS) | set(KIA_EV6_CARS) | set(KIA_FORTE_CARS) | - set(KIA_NIRO_PHEV_2022_CARS) | set(GENESIS_G90_CARS) + set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_CARNIVAL_CARS) | set(GENESIS_G90_CARS) ) dedicated_center_taper = car_fingerprint in ( set(PRIUS_CARS) | set(BOLT_CARS) | set(VOLT_STANDARD_CARS) | set(IONIQ_5_CARS) | set(IONIQ_EV_OLD_CARS) | set(IONIQ_6_CARS) | set(SONATA_CARS) | set(SONATA_HYBRID_CARS) | set(KIA_XCEED_CARS) | set(KIA_NIRO_PHEV_2022_CARS) | set(KIA_FORTE_CARS) | set(KIA_EV6_CARS) | - set(SILVERADO_CARS) + set(KIA_CARNIVAL_CARS) | set(SILVERADO_CARS) ) rich_knobs = [name for name, meta in FLM_SUPPORTED_VEHICLE_KNOBS.items() if meta["profile"] == profile_key] return { diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 79bd027f6..9e643982b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -72,6 +72,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_ioniq_6_low_speed_angle_assist_torque, get_kia_forte_center_taper_scale, get_kia_forte_ff_scale, + get_kia_carnival_center_taper_scale, + get_kia_carnival_friction_center_fade_scale, + get_kia_carnival_friction_threshold, get_kia_ev6_center_taper_scale, get_kia_ev6_ff_scale, get_kia_ev6_friction_scale, @@ -403,6 +406,23 @@ class TestLatControl: assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.0, 15.0) assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.20, 30.0) <= 1.0 + def test_kia_carnival_near_center_stabilization(self): + center_taper = get_kia_carnival_center_taper_scale(0.04, 8.5) + turn_taper = get_kia_carnival_center_taper_scale(0.35, 8.5) + low_speed_taper = get_kia_carnival_center_taper_scale(0.04, 2.0) + highway_taper = get_kia_carnival_center_taper_scale(0.04, 25.0) + assert center_taper < turn_taper <= 1.0 + assert center_taper < low_speed_taper <= 1.0 + assert center_taper < highway_taper <= 1.0 + + center_threshold = get_kia_carnival_friction_threshold(8.5, 0.04) + turn_threshold = get_kia_carnival_friction_threshold(8.5, 0.35) + assert center_threshold > turn_threshold >= get_hkg_canfd_base_friction_threshold(8.5) + + center_fade = get_kia_carnival_friction_center_fade_scale(0.04, 8.5) + turn_fade = get_kia_carnival_friction_center_fade_scale(0.35, 8.5) + assert center_fade < turn_fade <= 1.0 + def test_genesis_g90_ff_scale_curve(self): assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0 assert get_genesis_g90_ff_scale(0.5, 0.0, 20.0) > get_genesis_g90_ff_scale(-0.5, 0.0, 20.0) @@ -793,6 +813,15 @@ class TestLatControl: assert lac_log.active assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.10) + def test_kia_carnival_default_update_path(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_CARNIVAL_2025) + CS.vEgo = 8.5 + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert controller.is_kia_carnival + assert lac_log.active + def test_ioniq_6_update_path_does_not_post_taper_output(self, monkeypatch): base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6) base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) diff --git a/selfdrive/ui/mici/onroad/cameraview.py b/selfdrive/ui/mici/onroad/cameraview.py index 0fd1f6503..3974a1304 100644 --- a/selfdrive/ui/mici/onroad/cameraview.py +++ b/selfdrive/ui/mici/onroad/cameraview.py @@ -1,5 +1,6 @@ import os import platform +import weakref import numpy as np import pyray as rl @@ -135,6 +136,7 @@ class CameraView(Widget): self.egl_texture: rl.Texture | None = None self._placeholder_color: rl.Color | None = None + self._closed = False # Initialize EGL for zero-copy rendering when available. if self._use_egl: @@ -143,19 +145,31 @@ class CameraView(Widget): self.egl_texture = rl.load_texture_from_image(temp_image) rl.unload_image(temp_image) - ui_state.add_offroad_transition_callback(self._offroad_transition) + self_ref = weakref.ref(self) + + def offroad_transition_callback(): + if (view := self_ref()) is not None: + view._offroad_transition() + + self._offroad_transition_callback = offroad_transition_callback + ui_state.add_offroad_transition_callback(self._offroad_transition_callback) def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) + self._reset_camera_connection() + + def _reset_camera_connection(self): + # EGL images and VisionBuf objects both retain the imported camera buffer. + # Release them on every road-state transition instead of pinning the old + # camerad allocation until this view happens to render again. + self._clear_textures() self.frame = None + self.available_streams.clear() + self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) + self._target_client = None + self._target_stream_type = None + self._switching = False + self._texture_needs_update = True + self.last_connection_attempt = 0.0 def _set_placeholder_color(self, color: rl.Color): """Set a placeholder color to be drawn when no frame is available.""" @@ -182,6 +196,14 @@ class CameraView(Widget): return self._stream_type def close(self) -> None: + if self._closed: + return + self._closed = True + + callback = getattr(self, "_offroad_transition_callback", None) + if callback is not None: + ui_state.remove_offroad_transition_callback(callback) + self._offroad_transition_callback = None self._clear_textures() # Clean up EGL texture @@ -197,6 +219,7 @@ class CameraView(Widget): self.frame = None self.available_streams.clear() self.client = None + self._target_client = None def __del__(self): self.close() diff --git a/selfdrive/ui/mici/tests/test_camera_cleanup.py b/selfdrive/ui/mici/tests/test_camera_cleanup.py new file mode 100644 index 000000000..af10cdb7e --- /dev/null +++ b/selfdrive/ui/mici/tests/test_camera_cleanup.py @@ -0,0 +1,72 @@ +import gc +from types import SimpleNamespace +import weakref + +import pytest + +from openpilot.selfdrive.ui.mici.onroad import cameraview as mici_cameraview +from openpilot.selfdrive.ui.onroad import cameraview as big_cameraview + + +@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) +def test_road_transition_releases_camera_buffers(monkeypatch, module): + class FakeClient: + pass + + view = module.CameraView.__new__(module.CameraView) + old_client = FakeClient() + old_client_ref = weakref.ref(old_client) + view._name = "camerad" + view._stream_type = object() + view.client = old_client + view.frame = None + view.available_streams = [object()] + view._target_client = FakeClient() + view._target_stream_type = object() + view._switching = True + view._texture_needs_update = False + view.last_connection_attempt = 123.0 + view._closed = True + cleared = [] + view._clear_textures = lambda: cleared.append(True) + + monkeypatch.setattr(module, "VisionIpcClient", lambda *_args, **_kwargs: FakeClient()) + del old_client + + view._offroad_transition() + gc.collect() + + assert old_client_ref() is None + assert cleared == [True] + assert view.frame is None + assert view.available_streams == [] + assert view._target_client is None + assert view._target_stream_type is None + assert view._switching is False + assert view._texture_needs_update + assert view.last_connection_attempt == 0.0 + + +@pytest.mark.parametrize("module", (mici_cameraview, big_cameraview)) +def test_transition_callback_does_not_retain_camera_view(monkeypatch, module): + class FakeClient: + pass + + callbacks = [] + monkeypatch.setattr(module, "TICI", False) + monkeypatch.setattr(module, "VisionIpcClient", lambda *_args, **_kwargs: FakeClient()) + monkeypatch.setattr(module.rl, "load_shader_from_memory", lambda *_args: SimpleNamespace(id=1)) + monkeypatch.setattr(module.rl, "get_shader_location", lambda *_args: 0) + monkeypatch.setattr(module.rl, "unload_shader", lambda *_args: None) + monkeypatch.setattr(module.ui_state, "add_offroad_transition_callback", callbacks.append) + monkeypatch.setattr(module.ui_state, "remove_offroad_transition_callback", callbacks.remove) + + view = module.CameraView("camerad", object()) + view_ref = weakref.ref(view) + assert len(callbacks) == 1 + + del view + gc.collect() + + assert view_ref() is None + assert callbacks == [] diff --git a/selfdrive/ui/onroad/cameraview.py b/selfdrive/ui/onroad/cameraview.py index 544394846..03638a1c3 100644 --- a/selfdrive/ui/onroad/cameraview.py +++ b/selfdrive/ui/onroad/cameraview.py @@ -1,4 +1,5 @@ import platform +import weakref import numpy as np import pyray as rl @@ -93,6 +94,7 @@ class CameraView(Widget): self.egl_texture: rl.Texture | None = None self._placeholder_color: rl.Color | None = None + self._closed = False # Initialize EGL for zero-copy rendering on TICI if TICI: @@ -104,19 +106,31 @@ class CameraView(Widget): self.egl_texture = rl.load_texture_from_image(temp_image) rl.unload_image(temp_image) - ui_state.add_offroad_transition_callback(self._offroad_transition) + self_ref = weakref.ref(self) + + def offroad_transition_callback(): + if (view := self_ref()) is not None: + view._offroad_transition() + + self._offroad_transition_callback = offroad_transition_callback + ui_state.add_offroad_transition_callback(self._offroad_transition_callback) def _offroad_transition(self): - # Reconnect if not first time going onroad - if ui_state.is_onroad() and self.frame is not None: - # Prevent old frames from showing when going onroad. Qt has a separate thread - # which drains the VisionIpcClient SubSocket for us. Re-connecting is not enough - # and only clears internal buffers, not the message queue. - self.frame = None - self.available_streams.clear() - if self.client: - del self.client - self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) + self._reset_camera_connection() + + def _reset_camera_connection(self): + # EGL images and VisionBuf objects both retain the imported camera buffer. + # Release them on every road-state transition instead of pinning the old + # camerad allocation until this view happens to render again. + self._clear_textures() + self.frame = None + self.available_streams.clear() + self.client = VisionIpcClient(self._name, self._stream_type, conflate=True) + self._target_client = None + self._target_stream_type = None + self._switching = False + self._texture_needs_update = True + self.last_connection_attempt = 0.0 def _set_placeholder_color(self, color: rl.Color): """Set a placeholder color to be drawn when no frame is available.""" @@ -143,6 +157,14 @@ class CameraView(Widget): return self._stream_type def close(self) -> None: + if self._closed: + return + self._closed = True + + callback = getattr(self, "_offroad_transition_callback", None) + if callback is not None: + ui_state.remove_offroad_transition_callback(callback) + self._offroad_transition_callback = None self._clear_textures() # Clean up EGL texture @@ -157,6 +179,7 @@ class CameraView(Widget): self.frame = None self.available_streams.clear() self.client = None + self._target_client = None def __del__(self): self.close() diff --git a/selfdrive/ui/onroad/starpilot/developer_sidebar.py b/selfdrive/ui/onroad/starpilot/developer_sidebar.py index a8251e280..eace8d09f 100644 --- a/selfdrive/ui/onroad/starpilot/developer_sidebar.py +++ b/selfdrive/ui/onroad/starpilot/developer_sidebar.py @@ -1,6 +1,7 @@ import pyray as rl import time import re +import json from cereal import car from openpilot.common.params import Params from openpilot.selfdrive.ui.ui_state import ui_state @@ -14,6 +15,8 @@ METRIC_MARGIN = 12 FONT_SIZE = 35 METER_TO_FOOT = 3.28084 _WHITE_DIM = rl.Color(255, 255, 255, 85) +_LOCKED_VALUE_COLOR = rl.Color(34, 197, 94, 255) +_AUTO_TUNE_COLOR = rl.Color(59, 130, 246, 255) _FLM_OVERRIDE_COLOR = rl.Color(239, 68, 68, 255) def parse_hex_color(hex_str: str, default_color=rl.WHITE) -> rl.Color: @@ -48,6 +51,17 @@ def resolve_effective_torque_value(custom_enabled: bool, custom_value: float, return stock_value if stock_value != 0.0 else configured_value +def _safe_float(value, default: float = 0.0) -> float: + try: + return float(value) + except (TypeError, ValueError): + return default + + +def _setting_changed(value: float, reference: float) -> bool: + return round(value, 2) != round(reference, 2) + + class DeveloperSidebar: def __init__(self): self._params = Params() @@ -57,10 +71,16 @@ class DeveloperSidebar: self._cached_force_auto_tune_off = False self._cached_force_auto_tune = False self._cached_flm_trial_applied = False + self._cached_use_auto_steer_delay = True + self._cached_delay_stock = 0.0 + self._cached_delay = 0.0 self._cached_friction_stock = 0.0 self._cached_friction = 0.0 self._cached_lat_stock = 0.0 self._cached_lat = 0.0 + self._cached_ratio_stock = 0.0 + self._cached_ratio = 0.0 + self._flm_generic_param_keys: set[str] = set() self.lateral_engagement_time = 0 self.longitudinal_engagement_time = 0 @@ -73,7 +93,7 @@ class DeveloperSidebar: self._visible = False self._metric_color = rl.WHITE self._active_ids: list[int] = [] - self._flm_override_metric_ids: set[int] = set() + self._metric_colors: dict[int, rl.Color] = {} self._metrics: dict[int, tuple[str, str]] = {} @property @@ -98,10 +118,41 @@ class DeveloperSidebar: self._cached_force_auto_tune_off = self._params.get_bool("ForceAutoTuneOff") self._cached_force_auto_tune = self._params.get_bool("ForceAutoTune") self._cached_flm_trial_applied = self._params.get_bool("FLMTrialApplied") + self._cached_use_auto_steer_delay = self._params.get_bool("UseAutoSteerDelay", default=True) + self._cached_delay_stock = self._params.get_float("SteerDelayStock") + self._cached_delay = self._params.get_float("SteerDelay") self._cached_friction_stock = self._params.get_float("SteerFrictionStock") self._cached_friction = self._params.get_float("SteerFriction") self._cached_lat_stock = self._params.get_float("SteerLatAccelStock") self._cached_lat = self._params.get_float("SteerLatAccel") + self._cached_ratio_stock = self._params.get_float("SteerRatioStock") + self._cached_ratio = self._params.get_float("SteerRatio") + self._flm_generic_param_keys = self._read_flm_generic_param_keys() if self._cached_flm_trial_applied else set() + + def _read_flm_generic_param_keys(self) -> set[str]: + try: + raw = self._params.get("FLMTrialBaseline", encoding="utf-8") or "{}" + snapshot = raw if isinstance(raw, dict) else json.loads(raw) + applied = snapshot.get("appliedGenericParams", {}) if isinstance(snapshot, dict) else {} + return set(applied.keys()) if isinstance(applied, dict) else set() + except Exception: + return set() + + @staticmethod + def _toggle_bool(toggles: dict, key: str, fallback: bool = False) -> bool: + return bool(toggles[key]) if key in toggles else fallback + + @staticmethod + def _toggle_float(toggles: dict, key: str, fallback: float = 0.0) -> float: + return _safe_float(toggles.get(key, fallback), fallback) + + def _flm_changed(self, *keys: str) -> bool: + return self._cached_flm_trial_applied and any(key in self._flm_generic_param_keys for key in keys) + + def _tuning_color(self, *, auto: bool = False, flm: bool = False) -> rl.Color: + if flm: + return _FLM_OVERRIDE_COLOR + return _AUTO_TUNE_COLOR if auto else _LOCKED_VALUE_COLOR def _draw_metric(self, sidebar_rect: rl.Rectangle, label_first: str, label_second: str, color: rl.Color, y: float): card_x = int(sidebar_rect.x + sidebar_rect.width) - METRIC_MARGIN - METRIC_WIDTH @@ -211,26 +262,19 @@ class DeveloperSidebar: steer_label += f" - ({self.max_steer_angle}°)" torque_label += f" - ({self.max_torque}%)" - force_auto_tune_off = ui_state.starpilot_toggles.get("force_auto_tune_off", False) or self._cached_force_auto_tune_off - force_auto_tune = ui_state.starpilot_toggles.get("force_auto_tune", False) or self._cached_force_auto_tune + toggles = ui_state.starpilot_toggles + force_auto_tune_off = self._toggle_bool(toggles, "force_auto_tune_off", self._cached_force_auto_tune_off) + force_auto_tune = self._toggle_bool(toggles, "force_auto_tune", self._cached_force_auto_tune) use_params = live_torque_parameters.useParams if (live_torque_parameters and hasattr(live_torque_parameters, 'useParams')) else False - using_live_torque = not force_auto_tune_off and (use_params or force_auto_tune) + using_live_torque = live_torque_parameters is not None and not force_auto_tune_off and (use_params or force_auto_tune) live_friction = live_torque_parameters.frictionCoefficientFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'frictionCoefficientFiltered')) else 0.0 live_lat_factor = live_torque_parameters.latAccelFactorFiltered if (live_torque_parameters and hasattr(live_torque_parameters, 'latAccelFactorFiltered')) else 0.0 - custom_friction = float(ui_state.starpilot_toggles.get("friction", self._cached_friction) or 0.0) - custom_lat_factor = float(ui_state.starpilot_toggles.get("latAccelFactor", self._cached_lat) or 0.0) - use_custom_friction = bool(ui_state.starpilot_toggles.get("use_custom_friction", force_auto_tune_off)) - use_custom_lat_factor = bool(ui_state.starpilot_toggles.get("use_custom_latAccelFactor", force_auto_tune_off)) - - self._flm_override_metric_ids = set() - if self._cached_flm_trial_applied: - flm_metric_flags = { - 3: bool(ui_state.starpilot_toggles.get("use_custom_steerActuatorDelay", False)), - 4: use_custom_friction, - 5: use_custom_lat_factor, - 6: bool(ui_state.starpilot_toggles.get("use_custom_steerRatio", False)), - } - self._flm_override_metric_ids = {metric_id for metric_id, enabled in flm_metric_flags.items() if enabled} + custom_friction = self._toggle_float(toggles, "friction", self._cached_friction) + custom_lat_factor = self._toggle_float(toggles, "latAccelFactor", self._cached_lat) + fallback_use_custom_friction = force_auto_tune_off or (_setting_changed(self._cached_friction, self._cached_friction_stock) and not force_auto_tune) + fallback_use_custom_lat_factor = force_auto_tune_off or (_setting_changed(self._cached_lat, self._cached_lat_stock) and not force_auto_tune) + use_custom_friction = self._toggle_bool(toggles, "use_custom_friction", fallback_use_custom_friction) + use_custom_lat_factor = self._toggle_bool(toggles, "use_custom_latAccelFactor", fallback_use_custom_lat_factor) friction_coeff = resolve_effective_torque_value( use_custom_friction, @@ -249,7 +293,15 @@ class DeveloperSidebar: self._cached_lat, ) - lat_delay = live_delay.lateralDelay if live_delay else 0.0 + use_auto_steer_delay = self._toggle_bool(toggles, "use_auto_steer_delay", self._cached_use_auto_steer_delay) + use_custom_delay = self._toggle_bool(toggles, "use_custom_steerActuatorDelay", not use_auto_steer_delay) + custom_delay = self._toggle_float(toggles, "steerActuatorDelay", self._cached_delay) + if use_custom_delay: + lat_delay = custom_delay + elif live_delay: + lat_delay = live_delay.lateralDelay + else: + lat_delay = self._cached_delay_stock if self._cached_delay_stock != 0.0 else self._cached_delay tot_time = max(1, self.total_engagement_time) lat_pct = (self.lateral_engagement_time / tot_time) * 100.0 @@ -261,8 +313,36 @@ class DeveloperSidebar: danger_jerk = starpilot_plan.dangerJerk if starpilot_plan else 0.0 speed_jerk = starpilot_plan.speedJerk if starpilot_plan else 0.0 - steer_ratio = live_parameters.steerRatio if live_parameters else 0.0 - stiff_factor = live_parameters.stiffnessFactor if live_parameters else 0.0 + fallback_use_custom_steer_ratio = force_auto_tune_off or (_setting_changed(self._cached_ratio, self._cached_ratio_stock) and not force_auto_tune) + use_custom_steer_ratio = self._toggle_bool(toggles, "use_custom_steerRatio", fallback_use_custom_steer_ratio) + custom_steer_ratio = self._toggle_float(toggles, "steerRatio", self._cached_ratio) + if live_parameters: + steer_ratio = live_parameters.steerRatio + stiff_factor = live_parameters.stiffnessFactor + else: + steer_ratio = custom_steer_ratio if use_custom_steer_ratio else (self._cached_ratio_stock if self._cached_ratio_stock != 0.0 else self._cached_ratio) + stiff_factor = 0.0 + + force_auto_tune_flm = self._flm_changed("ForceAutoTune", "ForceAutoTuneOff") + self._metric_colors = { + 3: self._tuning_color( + auto=not use_custom_delay and live_delay is not None, + flm=self._flm_changed("SteerDelay", "UseAutoSteerDelay"), + ), + 4: self._tuning_color( + auto=using_live_torque and not use_custom_friction, + flm=self._flm_changed("SteerFriction") or (force_auto_tune_flm and (using_live_torque or use_custom_friction)), + ), + 5: self._tuning_color( + auto=using_live_torque and not use_custom_lat_factor, + flm=self._flm_changed("SteerLatAccel") or (force_auto_tune_flm and (using_live_torque or use_custom_lat_factor)), + ), + 6: self._tuning_color( + auto=live_parameters is not None and not use_custom_steer_ratio, + flm=self._flm_changed("SteerRatio") or (force_auto_tune_flm and use_custom_steer_ratio), + ), + 7: self._tuning_color(auto=live_parameters is not None), + } model_name = ui_state.starpilot_toggles.get("model_name", "N/A") model_name = re.sub(r'\(.*\)', '', model_name) @@ -305,6 +385,5 @@ class DeveloperSidebar: if metric_id <= 0 or metric_id not in self._metrics: continue label_first, label_second = self._metrics[metric_id] - flm_overridden = metric_id in self._flm_override_metric_ids - self._draw_metric(sidebar_rect, label_first, label_second, _FLM_OVERRIDE_COLOR if flm_overridden else self._metric_color, y) + self._draw_metric(sidebar_rect, label_first, label_second, self._metric_colors.get(metric_id, self._metric_color), y) y += METRIC_HEIGHT + spacing diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 993e60a40..221eb8ba0 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -120,6 +120,12 @@ class UIState: def add_offroad_transition_callback(self, callback: Callable[[], None]): self._offroad_transition_callbacks.append(callback) + def remove_offroad_transition_callback(self, callback: Callable[[], None]): + try: + self._offroad_transition_callbacks.remove(callback) + except ValueError: + pass + def add_engaged_transition_callback(self, callback: Callable[[], None]): self._engaged_transition_callbacks.append(callback) diff --git a/starpilot/common/starpilot_functions.py b/starpilot/common/starpilot_functions.py index ff94af244..b03c34b3a 100644 --- a/starpilot/common/starpilot_functions.py +++ b/starpilot/common/starpilot_functions.py @@ -302,4 +302,6 @@ def update_openpilot(thread_manager, params): while params.get_bool("IsOnroad") or thread_manager.is_thread_alive("lock_doors"): time.sleep(60) - HARDWARE.reboot() + # Manager owns the final reboot so a stale offroad read here cannot reboot + # the device while ignition is still on. + params.put_bool("DoReboot", True) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 77fedca07..bad96b30f 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -15,6 +15,7 @@ import numpy as np from cereal import car, custom, log from opendbc.car import gen_empty_fingerprint from opendbc.car.car_helpers import interfaces +from opendbc.car.chrysler.values import JEEPS as CHRYSLER_JEEPS from opendbc.car.gm.values import CAR as GM_CAR, EV_CAR as GM_EV_CAR, GMFlags from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, EV_CAR as HYUNDAI_EV_CAR, HyundaiFlags, HyundaiStarPilotSafetyFlags from opendbc.car.interfaces import TORQUE_SUBSTITUTE_PATH, CarInterfaceBase, GearShifter @@ -163,6 +164,24 @@ CANCEL_BUTTON_MAPPINGS = ( AOL_LKAS_MIGRATION_KEY = "AOLLKASMigratedToButtonControl" + +def sync_reboot_marker(marker_path: Path, enabled: bool, params: Params) -> bool: + """Synchronize a boot-time marker and ask manager for a guarded reboot.""" + if marker_path.is_file() == enabled: + return False + + marker_path.parent.mkdir(parents=True, exist_ok=True) + if enabled: + marker_path.touch() + else: + marker_path.unlink(missing_ok=True) + + # Manager defers DoReboot while onroad. Calling HARDWARE.reboot() here can + # abruptly reset the device if this constructor runs after engagement. + params.put_bool("DoReboot", True) + return True + + DEVELOPER_SIDEBAR_METRICS = { "NONE": 0, "ACCELERATION_CURRENT": 1, @@ -419,24 +438,12 @@ class StarPilotVariables: toggle.use_higher_bitrate &= not self.get_value("DisableOnroadUploads") toggle.use_higher_bitrate &= not self.vetting_branch - HD_PATH.parent.mkdir(parents=True, exist_ok=True) - if not HD_PATH.is_file() and toggle.use_higher_bitrate: - HD_PATH.touch() - HARDWARE.reboot() - elif HD_PATH.is_file() and not toggle.use_higher_bitrate: - HD_PATH.unlink() - HARDWARE.reboot() + sync_reboot_marker(HD_PATH, toggle.use_higher_bitrate, self.params_raw) toggle.use_konik_server = device_management toggle.use_konik_server &= self.get_value("UseKonikServer") - KONIK_PATH.parent.mkdir(parents=True, exist_ok=True) - if not KONIK_PATH.is_file() and toggle.use_konik_server: - KONIK_PATH.touch() - HARDWARE.reboot() - elif KONIK_PATH.is_file() and not toggle.use_konik_server: - KONIK_PATH.unlink() - HARDWARE.reboot() + sync_reboot_marker(KONIK_PATH, toggle.use_konik_server, self.params_raw) stock_colors_json = (STOCK_THEME_PATH / "colors/colors.json") self.stock_colors = json.loads(stock_colors_json.read_text()) if stock_colors_json.is_file() else {} @@ -1409,6 +1416,11 @@ class StarPilotVariables: toggle.subaru_sng = self.get_value("SubaruSNG", condition=toggle.car_make == "subaru" and not (CP.flags & SubaruFlags.GLOBAL_GEN2 or CP.flags & SubaruFlags.HYBRID)) toggle.subaru_sng_manual_parking_brake = self.get_value("SubaruSNGManualParkingBrake", condition=toggle.subaru_sng) + toggle.jeep_brake_hold = self.get_value( + "JeepBrakeHold", + condition=toggle.car_make == "chrysler" and toggle.car_model in CHRYSLER_JEEPS, + ) + toggle.tesla_cooperative_steering = self.get_value( "TeslaCoopSteering", condition=toggle.car_make == "tesla" and toggle.car_model == TESLA_CAR.TESLA_MODEL_3, diff --git a/starpilot/common/tests/test_starpilot_functions.py b/starpilot/common/tests/test_starpilot_functions.py index e9b85310d..39025b7f5 100644 --- a/starpilot/common/tests/test_starpilot_functions.py +++ b/starpilot/common/tests/test_starpilot_functions.py @@ -1,4 +1,5 @@ from openpilot.starpilot.common import connect_server as cs +from openpilot.starpilot.common import starpilot_functions as sf class FakeParams: @@ -8,6 +9,9 @@ class FakeParams: def get(self, key): return self.values.get(key) + def get_bool(self, key): + return self.values.get(key) in (True, 1, "1", b"1") + def put(self, key, value): self.values[key] = value @@ -18,6 +22,47 @@ class FakeParams: self.values.pop(key, None) +class FakeThreadManager: + def is_thread_alive(self, name): + return False + + +def test_automatic_update_requests_guarded_reboot(monkeypatch): + params = FakeParams({ + "UpdaterState": "idle", + "UpdaterFetchAvailable": True, + "UpdateAvailable": False, + "IsOnroad": False, + }) + state_reads = 0 + update_checks = 0 + + def get(key): + nonlocal state_reads + if key == "UpdaterState" and params.values[key] == "checking...": + state_reads += 1 + if state_reads % 2 == 0: + params.values[key] = "idle" + return params.values.get(key) + + def run_cmd(command, *args, **kwargs): + nonlocal update_checks + if "-SIGUSR1" in command: + update_checks += 1 + params.values["UpdaterState"] = "checking..." + params.values["UpdaterFetchAvailable"] = update_checks == 1 + elif "-SIGHUP" in command: + params.values["UpdateAvailable"] = True + + params.get = get + monkeypatch.setattr(sf, "run_cmd", run_cmd) + monkeypatch.setattr(sf.HARDWARE, "reboot", lambda: (_ for _ in ()).throw(AssertionError("direct reboot called"))) + + sf.update_openpilot(FakeThreadManager(), params) + + assert params.get("DoReboot") == b"1" + + def test_sync_konik_dongle_id_preserves_stock_id_before_switching(monkeypatch, tmp_path): monkeypatch.setattr(cs.Paths, "persist_root", staticmethod(lambda: str(tmp_path))) monkeypatch.setattr(cs, "use_konik_server", lambda: True) diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index da9059e75..5201b9fba 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -12,6 +12,13 @@ def test_legacy_volt_stock_acc_models_share_sng_and_auto_hold_scope(): } +def test_jeep_brake_hold_scope_is_grand_cherokee_only(): + assert {str(car) for car in spv.CHRYSLER_JEEPS} == { + "JEEP_GRAND_CHEROKEE", + "JEEP_GRAND_CHEROKEE_2019", + } + + def test_get_starpilot_toggles_uses_last_non_empty_broadcast(monkeypatch): params = SimpleNamespace(get_bool=lambda _key: False) monkeypatch.setattr(spv.get_starpilot_toggles, "_params", params, raising=False) @@ -66,6 +73,23 @@ class _FakeParams: self.bools.pop(key, None) +def test_sync_reboot_marker_uses_manager_guard(tmp_path): + params = _FakeParams() + marker = tmp_path / "cache" / "use_HD" + + assert spv.sync_reboot_marker(marker, True, params) is True + assert marker.is_file() + assert params.get_bool("DoReboot") is True + + params.put_bool("DoReboot", False) + assert spv.sync_reboot_marker(marker, True, params) is False + assert params.get_bool("DoReboot") is False + + assert spv.sync_reboot_marker(marker, False, params) is True + assert not marker.exists() + assert params.get_bool("DoReboot") is True + + def test_sync_stock_param_does_not_stomp_existing_custom_value_when_stock_missing(): params = _FakeParams({"SteerDelay": 0.35, "SteerDelayStock": 0.0}) variables = object.__new__(spv.StarPilotVariables) diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index c4c4a47e8..a476d4eab 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -1013,9 +1013,7 @@ class SpeedLimitVisionDaemon: stream_name = "wide camera" if desired_stream is None: - self.client = None - self.stream_type = None - self.stream_name = "" + self._disconnect_camera() return False if self.client is None or self.stream_type != desired_stream: @@ -1028,6 +1026,13 @@ class SpeedLimitVisionDaemon: return self.client.is_connected() + def _disconnect_camera(self): + # Dropping the client closes its imported VisionIPC buffer FDs. Keeping the + # client alive offroad pins the previous camerad allocation between drives. + self.client = None + self.stream_type = None + self.stream_name = "" + @staticmethod def _letterbox(image, shape=(640, 640), color=(114, 114, 114)): image_height, image_width = image.shape[:2] @@ -2459,6 +2464,7 @@ class SpeedLimitVisionDaemon: if self.started_prev: self._write_debug_event("session_end", reason="offroad") self._close_debug_session() + self._disconnect_camera() self.last_road_name = "" self.started_prev = False self.current_frame_bgr = None diff --git a/starpilot/system/tests/test_speed_limit_vision.py b/starpilot/system/tests/test_speed_limit_vision.py index 70eec0a30..892a14a96 100644 --- a/starpilot/system/tests/test_speed_limit_vision.py +++ b/starpilot/system/tests/test_speed_limit_vision.py @@ -39,6 +39,19 @@ def publishing_daemon(is_metric): return daemon +def test_disconnect_camera_releases_client_state(): + daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon) + daemon.client = object() + daemon.stream_type = object() + daemon.stream_name = "road camera" + + daemon._disconnect_camera() + + assert daemon.client is None + assert daemon.stream_type is None + assert daemon.stream_name == "" + + def test_published_sign_value_uses_configured_units(): imperial_daemon = publishing_daemon(False) metric_daemon = publishing_daemon(True) diff --git a/starpilot/ui/qt/widgets/developer_sidebar.cc b/starpilot/ui/qt/widgets/developer_sidebar.cc index ce8a348b0..91f112000 100644 --- a/starpilot/ui/qt/widgets/developer_sidebar.cc +++ b/starpilot/ui/qt/widgets/developer_sidebar.cc @@ -1,7 +1,81 @@ #include "starpilot/ui/qt/widgets/developer_sidebar.h" +#include +#include + +#include +#include +#include #include +namespace { + +const QColor LOCKED_VALUE_COLOR = QColor(34, 197, 94); +const QColor AUTO_TUNE_COLOR = QColor(59, 130, 246); +const QColor FLM_OVERRIDE_COLOR = QColor(239, 68, 68); + +bool settingChanged(float value, float reference) { + return std::round(value * 100.0f) != std::round(reference * 100.0f); +} + +bool toggleBool(const QJsonObject &toggles, const QString &key, bool fallback = false) { + return toggles.contains(key) ? toggles.value(key).toBool(fallback) : fallback; +} + +float toggleFloat(const QJsonObject &toggles, const QString &key, float fallback = 0.0f) { + return toggles.contains(key) ? static_cast(toggles.value(key).toDouble(fallback)) : fallback; +} + +float resolveEffectiveTorqueValue(bool customEnabled, float customValue, + bool liveEnabled, float liveValue, + float stockValue, float configuredValue) { + if (customEnabled) { + return customValue; + } + if (liveEnabled) { + return liveValue; + } + return stockValue != 0.0f ? stockValue : configuredValue; +} + +QSet readFlmGenericParamKeys(Params ¶ms) { + QSet keys; + const std::string raw = params.get("FLMTrialBaseline"); + if (raw.empty()) { + return keys; + } + + QJsonParseError error; + const QJsonDocument doc = QJsonDocument::fromJson(QByteArray::fromStdString(raw), &error); + if (error.error != QJsonParseError::NoError || !doc.isObject()) { + return keys; + } + + const QJsonObject appliedParams = doc.object().value("appliedGenericParams").toObject(); + for (const QString &key : appliedParams.keys()) { + keys.insert(key); + } + return keys; +} + +bool flmChanged(const QSet &keys, std::initializer_list names) { + for (const char *name : names) { + if (keys.contains(QString::fromUtf8(name))) { + return true; + } + } + return false; +} + +QColor tuningColor(bool autoTune, bool flm) { + if (flm) { + return FLM_OVERRIDE_COLOR; + } + return autoTune ? AUTO_TUNE_COLOR : LOCKED_VALUE_COLOR; +} + +} // namespace + void DeveloperSidebar::drawMetric(QPainter &p, const QPair &label, QColor c, int y) { const QRect rect = {12, y, 275, 126}; @@ -70,6 +144,7 @@ void DeveloperSidebar::updateState(const UIState &s, const StarPilotUIState &fs) const SubMaster &sm = *(s.sm); const StarPilotUIScene &starpilot_scene = fs.starpilot_scene; + const QJsonObject &starpilot_toggles = starpilot_scene.starpilot_toggles; const SubMaster &fpsm = *(fs.sm); const cereal::CarControl::Reader &carControl = fpsm["carControl"].getCarControl(); @@ -78,11 +153,12 @@ void DeveloperSidebar::updateState(const UIState &s, const StarPilotUIState &fs) const cereal::LiveDelayData::Reader &liveDelay = fpsm["liveDelay"].getLiveDelay(); const cereal::LiveParametersData::Reader &liveParameters = fpsm["liveParameters"].getLiveParameters(); const cereal::LiveTorqueParametersData::Reader &liveTorqueParameters = fpsm["liveTorqueParameters"].getLiveTorqueParameters(); - const bool forceAutoTuneOff = starpilot_scene.starpilot_toggles.value("force_auto_tune_off").toBool(); - const bool usingLiveTorqueTune = !forceAutoTuneOff && (liveTorqueParameters.getUseParams() || starpilot_scene.starpilot_toggles.value("force_auto_tune").toBool()); + const bool forceAutoTuneOff = toggleBool(starpilot_toggles, "force_auto_tune_off", params.getBool("ForceAutoTuneOff")); + const bool forceAutoTune = toggleBool(starpilot_toggles, "force_auto_tune", params.getBool("ForceAutoTune")); + const bool usingLiveTorqueTune = !forceAutoTuneOff && (liveTorqueParameters.getUseParams() || forceAutoTune); const bool is_metric = s.scene.is_metric; - const bool use_si = starpilot_scene.starpilot_toggles.value("use_si_metrics").toBool(); + const bool use_si = starpilot_toggles.value("use_si_metrics").toBool(); const QString accelerationUnit = (is_metric || use_si) ? tr(" m/s²") : tr(" ft/s²"); const float accelerationConversion = (is_metric || use_si) ? 1.0f : METER_TO_FOOT; @@ -124,34 +200,79 @@ void DeveloperSidebar::updateState(const UIState &s, const StarPilotUIState &fs) torqueLabel += QString(" - (%1%)").arg(maxTorque); } - float displayedFriction = usingLiveTorqueTune ? liveTorqueParameters.getFrictionCoefficientFiltered() : params.getFloat("SteerFrictionStock"); - if (displayedFriction == 0.0f) { - displayedFriction = forceAutoTuneOff ? params.getFloat("SteerFriction") : liveTorqueParameters.getFrictionCoefficientFiltered(); - } + const float frictionStock = params.getFloat("SteerFrictionStock"); + const float frictionConfigured = params.getFloat("SteerFriction"); + const float latAccelStock = params.getFloat("SteerLatAccelStock"); + const float latAccelConfigured = params.getFloat("SteerLatAccel"); + const float steerRatioStock = params.getFloat("SteerRatioStock"); + const float steerRatioConfigured = params.getFloat("SteerRatio"); - float displayedLatAccel = usingLiveTorqueTune ? liveTorqueParameters.getLatAccelFactorFiltered() : params.getFloat("SteerLatAccelStock"); - if (displayedLatAccel == 0.0f) { - displayedLatAccel = forceAutoTuneOff ? params.getFloat("SteerLatAccel") : liveTorqueParameters.getLatAccelFactorFiltered(); - } + const float customFriction = toggleFloat(starpilot_toggles, "friction", frictionConfigured); + const float customLatAccel = toggleFloat(starpilot_toggles, "latAccelFactor", latAccelConfigured); + const bool useCustomFriction = toggleBool(starpilot_toggles, "use_custom_friction", + forceAutoTuneOff || (settingChanged(frictionConfigured, frictionStock) && !forceAutoTune)); + const bool useCustomLatAccel = toggleBool(starpilot_toggles, "use_custom_latAccelFactor", + forceAutoTuneOff || (settingChanged(latAccelConfigured, latAccelStock) && !forceAutoTune)); + + const float displayedFriction = resolveEffectiveTorqueValue( + useCustomFriction, + customFriction, + usingLiveTorqueTune, + liveTorqueParameters.getFrictionCoefficientFiltered(), + frictionStock, + frictionConfigured + ); + const float displayedLatAccel = resolveEffectiveTorqueValue( + useCustomLatAccel, + customLatAccel, + usingLiveTorqueTune, + liveTorqueParameters.getLatAccelFactorFiltered(), + latAccelStock, + latAccelConfigured + ); + + const bool cachedUseAutoDelay = params.get("UseAutoSteerDelay").empty() ? true : params.getBool("UseAutoSteerDelay"); + const bool useAutoSteerDelay = toggleBool(starpilot_toggles, "use_auto_steer_delay", cachedUseAutoDelay); + const bool useCustomDelay = toggleBool(starpilot_toggles, "use_custom_steerActuatorDelay", !useAutoSteerDelay); + const float customDelay = toggleFloat(starpilot_toggles, "steerActuatorDelay", params.getFloat("SteerDelay")); + const float displayedDelay = useCustomDelay ? customDelay : liveDelay.getLateralDelay(); + + const bool useCustomSteerRatio = toggleBool(starpilot_toggles, "use_custom_steerRatio", + forceAutoTuneOff || (settingChanged(steerRatioConfigured, steerRatioStock) && !forceAutoTune)); + const QSet flmGenericParamKeys = params.getBool("FLMTrialApplied") ? readFlmGenericParamKeys(params) : QSet(); + const bool forceAutoTuneFlm = flmChanged(flmGenericParamKeys, {"ForceAutoTune", "ForceAutoTuneOff"}); + const QColor delayColor = tuningColor(!useCustomDelay, flmChanged(flmGenericParamKeys, {"SteerDelay", "UseAutoSteerDelay"})); + const QColor frictionColor = tuningColor( + usingLiveTorqueTune && !useCustomFriction, + flmChanged(flmGenericParamKeys, {"SteerFriction"}) || (forceAutoTuneFlm && (usingLiveTorqueTune || useCustomFriction)) + ); + const QColor latAccelColor = tuningColor( + usingLiveTorqueTune && !useCustomLatAccel, + flmChanged(flmGenericParamKeys, {"SteerLatAccel"}) || (forceAutoTuneFlm && (usingLiveTorqueTune || useCustomLatAccel)) + ); + const QColor steerRatioColor = tuningColor( + !useCustomSteerRatio, + flmChanged(flmGenericParamKeys, {"SteerRatio"}) || (forceAutoTuneFlm && useCustomSteerRatio) + ); accelerationStatus = ItemStatus(QPair(tr("ACCEL"), QString::number(acceleration, 'f', 2) + accelerationUnit), metricColor); accelerationJerkStatus = ItemStatus(QPair(tr("ACCEL JERK"), QString::number(starpilotPlan.getAccelerationJerk())), metricColor); actuatorAccelerationStatus = ItemStatus(QPair(tr("ACT ACCEL"), QString::number(carControl.getActuators().getAccel() * accelerationConversion, 'f', 2) + accelerationUnit), metricColor); dangerFactorStatus = ItemStatus(QPair(tr("DANGER %"), QString::number(starpilotPlan.getDangerFactor() * 100.0f, 'f', 2) + "%"), metricColor); dangerJerkStatus = ItemStatus(QPair(tr("DANGER JERK"), QString::number(starpilotPlan.getDangerJerk())), metricColor); - delayStatus = ItemStatus(QPair(tr("STEER DELAY"), QString::number(liveDelay.getLateralDelay(), 'f', 5)), metricColor); - frictionStatus = ItemStatus(QPair(tr("FRICTION"), QString::number(displayedFriction, 'f', 5)), metricColor); - latAccelStatus = ItemStatus(QPair(tr("LAT ACCEL"), QString::number(displayedLatAccel, 'f', 5)), metricColor); + delayStatus = ItemStatus(QPair(tr("STEER DELAY"), QString::number(displayedDelay, 'f', 5)), delayColor); + frictionStatus = ItemStatus(QPair(tr("FRICTION"), QString::number(displayedFriction, 'f', 5)), frictionColor); + latAccelStatus = ItemStatus(QPair(tr("LAT ACCEL"), QString::number(displayedLatAccel, 'f', 5)), latAccelColor); lateralEngagementStatus = ItemStatus(QPair(tr("LATERAL %"), QString::number((lateralEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor); longitudinalEngagementStatus = ItemStatus(QPair(tr("LONG %"), QString::number((longitudinalEngagementTime / totalEngagementTime) * 100.0f, 'f', 2) + "%"), metricColor); maxAccelerationStatus = ItemStatus(QPair(tr("MAX ACCEL"), QString::number(maxAcceleration, 'f', 2) + accelerationUnit), metricColor); speedJerkStatus = ItemStatus(QPair(tr("SPEED JERK"), QString::number(starpilotPlan.getSpeedJerk())), metricColor); steerAngleStatus = ItemStatus(QPair(tr("STEER ANGLE"), steerLabel), metricColor); - steerRatioStatus = ItemStatus(QPair(tr("STEER RATIO"), QString::number(liveParameters.getSteerRatio(), 'f', 5)), metricColor); - stiffnessFactorStatus = ItemStatus(QPair(tr("STEER STIFF"), QString::number(liveParameters.getStiffnessFactor(), 'f', 5)), metricColor); + steerRatioStatus = ItemStatus(QPair(tr("STEER RATIO"), QString::number(liveParameters.getSteerRatio(), 'f', 5)), steerRatioColor); + stiffnessFactorStatus = ItemStatus(QPair(tr("STEER STIFF"), QString::number(liveParameters.getStiffnessFactor(), 'f', 5)), AUTO_TUNE_COLOR); torqueStatus = ItemStatus(QPair(tr("TORQUE %"), torqueLabel), metricColor); - QString modelName = starpilot_scene.starpilot_toggles.value("model_name").toString(); + QString modelName = starpilot_toggles.value("model_name").toString(); modelName.remove(QRegularExpression("\\(.*\\)")); modelName.remove(QRegularExpression("[^a-zA-Z0-9 \\-\\.:]")); modelNameStatus = ItemStatus(QPair(modelName.trimmed(), ""), metricColor); diff --git a/system/manager/manager.py b/system/manager/manager.py index fc68ef26f..72c5e47c1 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -145,6 +145,10 @@ def update_nav_offroad_clear_state(params, started: bool, tracked_destination, t return tracked_destination, tracked_started_at +def should_defer_reboot(started: bool, ignition: bool) -> bool: + return started or ignition + + def _merge_starpilot_stat_values(existing, incoming, key=None): if existing is None: return incoming @@ -1010,10 +1014,10 @@ def manager_thread() -> None: # Exit main loop when uninstall/shutdown/reboot is needed shutdown = False for param in ("DoUninstall", "DoShutdown", "DoReboot"): - if param == "DoReboot" and started: + if param == "DoReboot" and should_defer_reboot(started, ignition): if params.get_bool(param): if not warned_onroad_reboot: - cloudlog.warning("ignoring DoReboot while onroad; deferring until offroad") + cloudlog.warning("ignoring DoReboot while started or ignition is on; deferring until offroad") warned_onroad_reboot = True continue if params.get_bool(param): diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index 31ef2b986..57b19eab2 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -106,6 +106,12 @@ class FakeManagedProcess: return SimpleNamespace(name="ui") +def test_reboot_guard_includes_raw_ignition_state(): + assert manager.should_defer_reboot(started=True, ignition=False) + assert manager.should_defer_reboot(started=False, ignition=True) + assert not manager.should_defer_reboot(started=False, ignition=False) + + class TestManager: def setup_method(self): HARDWARE.set_power_save(False)