diff --git a/common/libcommon.a b/common/libcommon.a index 50f2ba3de..83b60bb73 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index a453774ea..1f8bfcc43 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -547,6 +547,8 @@ inline static std::unordered_map keys = { {"FavoriteTrafficModeCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"WheelButtonBookmarkCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"WheelControlAOLCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, + {"WheelControlDisengageCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, + {"WheelControlEngageCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"WheelControlForceCoastCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"WheelControlPulseGlideCounter", {CLEAR_ON_MANAGER_START, INT, "0", "0"}}, {"openpilotMinutes", {PERSISTENT, INT, "0", "0", 0}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index f8c486627..29bb5fc47 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index f30ccb92e..b8d294e6f 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -561,6 +561,8 @@ class ModelState: self.road_key, self.wide_key = _detect_vision_keys(input_shapes) self.vision_input_names = [self.road_key, self.wide_key] + self.warped_input_shape = (2, 6, *input_shapes[self.road_key][2:]) + self.last_warp_output: Tensor | None = None self.numpy_inputs, self.prev_desired_curv_key = self._build_policy_inputs(self.policy_input_shapes) self.desire_key = next(key for key in self.numpy_inputs if key.startswith("desire")) self.off_policy_enabled = "off_policy" in self.policy_order @@ -669,6 +671,7 @@ class ModelState: if self.prev_desired_curv_key is not None: self.full_prev_desired_curv.fill(0) self._blob_cache.clear() + self.last_warp_output = None def warmup(self) -> None: dummy_frames = { @@ -694,16 +697,10 @@ class ModelState: def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool, - after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None: - frames: dict[str, Tensor] = {} - for key, buf in bufs.items(): - ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data - cache_key = (key, ptr) - if cache_key not in self._blob_cache: - self._blob_cache[cache_key] = Tensor.from_blob( - ptr, (self.frame_buf_size,), dtype="uint8", device=self.WARP_DEV, - ) - frames[key] = self._blob_cache[cache_key] + after_enqueue: Callable[[], None] | None = None, + shared_warp: Tensor | None = None) -> dict[str, np.ndarray] | None: + if shared_warp is not None and self.image_history_pipeline != IMAGE_HISTORY_IN_POLICY: + raise RuntimeError("shared camera warp requires a policy-history model artifact") inputs[self.desire_key][0] = 0 self.numpy_inputs[self.desire_key].fill(0) @@ -720,18 +717,33 @@ class ModelState: self.npy["tfm"][:] = transforms[self.road_key] self.npy["big_tfm"][:] = transforms[self.wide_key] - warp_output = self.warp_enqueue( - **{key: self.input_queues[key] for key in self.warp_input_keys}, - frame=frames[self.road_key], - big_frame=frames[self.wide_key], - ) + if shared_warp is None: + frames: dict[str, Tensor] = {} + for key, buf in bufs.items(): + ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data + cache_key = (key, ptr) + if cache_key not in self._blob_cache: + self._blob_cache[cache_key] = Tensor.from_blob( + ptr, (self.frame_buf_size,), dtype="uint8", device=self.WARP_DEV, + ) + frames[key] = self._blob_cache[cache_key] + + warp_output = self.warp_enqueue( + **{key: self.input_queues[key] for key in self.warp_input_keys}, + frame=frames[self.road_key], + big_frame=frames[self.wide_key], + ) + else: + warp_output = shared_warp if self.image_history_pipeline == IMAGE_HISTORY_IN_POLICY: + self.last_warp_output = warp_output output_tensors = self.run_policy( **{key: self.input_queues[key] for key in self.policy_input_keys}, warped=warp_output, ) else: + self.last_warp_output = None img, big_img = warp_output if prepare_only: return None @@ -841,6 +853,15 @@ def _load_model_lab_model(cam_w: int, cam_h: int, model_id: str, version: str) - return candidate +def _model_lab_shared_warp_compatible(lateral: ModelState, longitudinal: ModelState) -> bool: + return ( + lateral.image_history_pipeline == IMAGE_HISTORY_IN_POLICY + and longitudinal.image_history_pipeline == IMAGE_HISTORY_IN_POLICY + and lateral.warped_input_shape == longitudinal.warped_input_shape + and lateral.WARP_DEV == longitudinal.WARP_DEV + ) + + def _isolate_next_model_artifact_load() -> int: from tinygrad.uop.ops import Ops, UOpMetaClass @@ -865,6 +886,9 @@ def _load_model_lab_models(cam_w: int, cam_h: int, lateral_id: str, longitudinal cloudlog.info(f"Model Laboratory isolated {evicted} realized buffer UOps before loading the second model") longitudinal = _load_model_lab_model(cam_w, cam_h, longitudinal_id, longitudinal_version) longitudinal.warmup() + if not _model_lab_shared_warp_compatible(lateral, longitudinal): + raise RuntimeError("Model Laboratory artifacts cannot share camera preprocessing") + cloudlog.info("Model Laboratory will share one camera warp between both AMD model runners") return lateral, longitudinal except Exception: cloudlog.exception("Model Laboratory AMD model load or warmup failed") @@ -1269,6 +1293,8 @@ def main(demo=False): lateral_inputs, model.can_prepare_only and dropped_frame, ) + if model.last_warp_output is None: + raise RuntimeError("Model Laboratory lateral runner did not produce a shareable camera warp") longitudinal_bufs, longitudinal_transforms, longitudinal_inputs = _runner_frame_args( model_lab_longitudinal, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire, traffic_convention, lat_action_t, long_action_t, @@ -1280,6 +1306,7 @@ def main(demo=False): longitudinal_inputs, model_lab_longitudinal.can_prepare_only and dropped_frame, chestnut_state.send if send_chestnut else None, + shared_warp=model.last_warp_output, ) if ( lateral_model_output is not None diff --git a/selfdrive/modeld/tests/test_model_laboratory.py b/selfdrive/modeld/tests/test_model_laboratory.py index 8230105ed..1a864b42f 100644 --- a/selfdrive/modeld/tests/test_model_laboratory.py +++ b/selfdrive/modeld/tests/test_model_laboratory.py @@ -111,6 +111,9 @@ def test_model_lab_loads_and_warms_both_amd_models_before_returning(monkeypatch) class FakeModel: def __init__(self, model_id): self.model_id = model_id + self.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY + self.warped_input_shape = (2, 6, 128, 256) + self.WARP_DEV = "QCOM" def warmup(self): calls.append(("warmup", self.model_id)) @@ -148,6 +151,74 @@ def test_model_lab_loads_and_warms_both_amd_models_before_returning(monkeypatch) ] +def test_model_lab_requires_shareable_camera_preprocessing(): + compatible = SimpleNamespace( + image_history_pipeline=modeld.IMAGE_HISTORY_IN_POLICY, + warped_input_shape=(2, 6, 128, 256), + WARP_DEV="QCOM", + ) + legacy = SimpleNamespace( + image_history_pipeline=modeld.IMAGE_HISTORY_IN_WARP, + warped_input_shape=(2, 6, 128, 256), + WARP_DEV="QCOM", + ) + different_shape = SimpleNamespace( + image_history_pipeline=modeld.IMAGE_HISTORY_IN_POLICY, + warped_input_shape=(2, 6, 256, 512), + WARP_DEV="QCOM", + ) + + assert modeld._model_lab_shared_warp_compatible(compatible, compatible) + assert not modeld._model_lab_shared_warp_compatible(compatible, legacy) + assert not modeld._model_lab_shared_warp_compatible(compatible, different_shape) + + +def test_model_state_reuses_shared_warp_without_preprocessing_again(): + shared_warp = object() + policy_calls = [] + + class FakeOutput: + @staticmethod + def numpy(): + return np.zeros(2, dtype=np.float32) + + state = modeld.ModelState.__new__(modeld.ModelState) + state.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY + state.desire_key = "desire" + state.numpy_inputs = {"desire": np.zeros((1, modeld.ModelConstants.DESIRE_LEN), dtype=np.float32)} + state.npy = { + "desire": np.zeros(modeld.ModelConstants.DESIRE_LEN, dtype=np.float32), + "tfm": np.zeros((3, 3), dtype=np.float32), + "big_tfm": np.zeros((3, 3), dtype=np.float32), + } + state.prev_desire = np.zeros(modeld.ModelConstants.DESIRE_LEN, dtype=np.float32) + state.prev_desired_curv_key = None + state.road_key = "road" + state.wide_key = "wide" + state.input_queues = {"history": "longitudinal-history"} + state.warp_input_keys = () + state.policy_input_keys = ("history",) + state.warp_enqueue = lambda **_kwargs: (_ for _ in ()).throw(AssertionError("second warp must not run")) + state.run_policy = lambda **kwargs: policy_calls.append(kwargs) or [FakeOutput()] + state.uses_external_gpu = False + state.model_type = "supercombo" + state.parser = SimpleNamespace(parse_outputs=lambda _outputs: {"plan": np.zeros(1, dtype=np.float32)}) + state.output_slices = {"plan": slice(0, 1)} + state.last_warp_output = None + + output = state.run( + {}, + {"road": np.eye(3, dtype=np.float32), "wide": np.eye(3, dtype=np.float32)}, + {"desire": np.zeros(modeld.ModelConstants.DESIRE_LEN, dtype=np.float32)}, + False, + shared_warp=shared_warp, + ) + + assert output is not None + assert policy_calls == [{"history": "longitudinal-history", "warped": shared_warp}] + assert state.last_warp_output is shared_warp + + def test_each_runner_receives_its_own_input_names_and_shared_frame_data(): model = SimpleNamespace( road_key="road", diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index df6b96c1c..21df28cd9 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -36,6 +36,11 @@ from openpilot.starpilot.common.starpilot_utilities import contains_event_type from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available from openpilot.starpilot.common.vision_bsm import get_fresh_vasm_state +from openpilot.starpilot.system.wheel_controls import ( + CONTROLLER_ACTION_COUNTERS, + CONTROLLER_ACTION_DISENGAGE, + CONTROLLER_ACTION_ENGAGE, +) REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -78,6 +83,14 @@ def commanded_torque_at_max_for_saturation(CP, output: float) -> bool: return torque_controller and not has_controller_grace and abs(output) > 0.99 +def controller_openpilot_event(CP, CS, enabled: bool, engage_requested: bool, disengage_requested: bool): + if disengage_requested and enabled: + return EventName.buttonCancel + if engage_requested and not enabled and CS.canValid and (not CP.pcmCruise or CS.cruiseState.enabled): + return EventName.buttonEnable + return None + + def should_loud_blindspot_alert_without_lateral(CS, sm, starpilot_toggles, combined_left_bsm=None, combined_right_bsm=None) -> bool: if not getattr(starpilot_toggles, "loud_blindspot_alert_when_disengaged", False): return False @@ -255,6 +268,10 @@ class SelfdriveD: self.state_machine = StateMachine() self.rk = Ratekeeper(100, print_delay_threshold=None) self.prev_pedal_long_active = False + self._controller_openpilot_counters = { + action: self.params_memory.get_int(CONTROLLER_ACTION_COUNTERS[action]) + for action in (CONTROLLER_ACTION_ENGAGE, CONTROLLER_ACTION_DISENGAGE) + } # Determine startup event self.startup_event = StarPilotEventName.customStartupAlert @@ -341,6 +358,12 @@ class SelfdriveD: if str(extra).strip().lower() == "longitudinal": self.params.remove("Offroad_ExcessiveActuation") + def _consume_controller_openpilot_action(self, action: str) -> bool: + counter = self.params_memory.get_int(CONTROLLER_ACTION_COUNTERS[action]) + previous = self._controller_openpilot_counters[action] + self._controller_openpilot_counters[action] = counter + return counter > previous + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -348,6 +371,9 @@ class SelfdriveD: self.events.clear() self.starpilot_events.clear() + controller_engage_requested = self._consume_controller_openpilot_action(CONTROLLER_ACTION_ENGAGE) + controller_disengage_requested = self._consume_controller_openpilot_action(CONTROLLER_ACTION_DISENGAGE) + switchback_mode_enabled = self.params_memory.get_bool("SwitchbackModeEnabled") switchback_mode_cooldown = max(0.0, float(getattr(self.starpilot_toggles, "switchback_mode_cooldown", 0.0))) @@ -416,6 +442,12 @@ class SelfdriveD: if self.CP.passive: return + controller_event = controller_openpilot_event( + self.CP, CS, self.enabled, controller_engage_requested, controller_disengage_requested, + ) + if controller_event is not None: + self.events.add(controller_event) + # Block resume if cruise never previously enabled resume_pressed = any(be.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be in CS.buttonEvents) if not self.CP.pcmCruise and CS.vCruise > 250 and resume_pressed: diff --git a/selfdrive/selfdrived/tests/test_selfdrived.py b/selfdrive/selfdrived/tests/test_selfdrived.py index 9d3857ec3..ce180c6ea 100644 --- a/selfdrive/selfdrived/tests/test_selfdrived.py +++ b/selfdrive/selfdrived/tests/test_selfdrived.py @@ -11,6 +11,7 @@ from openpilot.selfdrive.selfdrived.selfdrived import ( VALID_ONLY_COMM_ISSUE_GRACE_FRAMES, SelfdriveD, commanded_torque_at_max_for_saturation, + controller_openpilot_event, evaluate_comm_issue, ) @@ -42,6 +43,26 @@ def test_dead_or_slow_comm_issue_is_immediate(): assert evaluate_comm_issue(False, True, False, 0) == (True, 0) +def test_controller_openpilot_requests_use_normal_engagement_events(): + CP = car.CarParams.new_message(pcmCruise=False) + CS = car.CarState.new_message(canValid=True) + + assert controller_openpilot_event(CP, CS, False, True, False) == log.OnroadEvent.EventName.buttonEnable + assert controller_openpilot_event(CP, CS, True, False, True) == log.OnroadEvent.EventName.buttonCancel + assert controller_openpilot_event(CP, CS, True, True, True) == log.OnroadEvent.EventName.buttonCancel + + +def test_controller_engage_requires_valid_can_and_active_pcm_cruise(): + CP = car.CarParams.new_message(pcmCruise=True) + CS = car.CarState.new_message(canValid=True) + + assert controller_openpilot_event(CP, CS, False, True, False) is None + CS.cruiseState.enabled = True + assert controller_openpilot_event(CP, CS, False, True, False) == log.OnroadEvent.EventName.buttonEnable + CS.canValid = False + assert controller_openpilot_event(CP, CS, False, True, False) is None + + def test_starpilot_selfdrive_state_uses_sampled_car_state_speed(): class FakeEvents: names = [] diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 94ffcbfc6..8052c031b 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -73,7 +73,7 @@ class StarPilotCard: self._controller_action_counters = { key: self._get_controller_action_counter(counter) for key, counter in CONTROLLER_ACTION_COUNTERS.items() - if counter != "WheelButtonBookmarkCounter" + if key in (CONTROLLER_ACTION_FORCE_COAST, CONTROLLER_ACTION_PULSE_AND_GLIDE, CONTROLLER_ACTION_TOGGLE_AOL) } self.modePressed_previously = False self.mode_counter = 0 @@ -148,8 +148,8 @@ class StarPilotCard: self._controller_action_counters[key] = current return max(0, current - previous) - def _toggle_controller_aol(self, carState, starpilot_toggles, button_aol_supported): - if not button_aol_supported or not getattr(starpilot_toggles, "always_on_lateral", False): + def _toggle_controller_aol(self, carState, starpilot_toggles): + if not self.always_on_lateral_supported or not getattr(starpilot_toggles, "always_on_lateral", False): return False if self.hyundai_aol_needs_engagement: self.hyundai_aol_ready = True @@ -158,7 +158,7 @@ class StarPilotCard: self.pause_lateral = not self.always_on_lateral_allowed return True - def _handle_controller_actions(self, carState, sm, starpilot_toggles, button_aol_supported): + def _handle_controller_actions(self, carState, sm, starpilot_toggles): force_coast_count = self._pending_controller_action_count( CONTROLLER_ACTION_FORCE_COAST ) @@ -176,7 +176,7 @@ class StarPilotCard: CONTROLLER_ACTION_TOGGLE_AOL ) if aol_count % 2: - self._toggle_controller_aol(carState, starpilot_toggles, button_aol_supported) + self._toggle_controller_aol(carState, starpilot_toggles) def _handle_favorite_traffic_mode_action(self, sm): counter = self.params_memory.get_int(FAVORITE_ACTION_TRAFFIC_MODE_COUNTER) @@ -414,7 +414,7 @@ class StarPilotCard: else: self.handle_button_event("lkas", sm, starpilot_toggles) - self._handle_controller_actions(carState, sm, starpilot_toggles, button_aol_supported) + self._handle_controller_actions(carState, sm, starpilot_toggles) if getattr(starpilot_toggles, "has_canfd_media_buttons", False): if starpilotCarState.modePressed: diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index b87b383d5..1a7a47f6a 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -366,6 +366,22 @@ def test_controller_actions_match_vehicle_button_behaviors(monkeypatch, tmp_path assert ret.alwaysOnLateralAllowed is True +def test_controller_aol_does_not_require_physical_lkas_button_mapping(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard(SimpleNamespace(brand="honda"), SimpleNamespace(alternativeExperience=0)) + card.params_memory.put_int(spc.CONTROLLER_ACTION_COUNTERS[spc.CONTROLLER_ACTION_TOGGLE_AOL], 1) + ret = card.update( + make_car_state(), + SimpleNamespace(distancePressed=False), + make_sm(), + make_toggles(always_on_lateral=True, lkas_allowed_for_aol=False), + ) + + assert ret.alwaysOnLateralAllowed is True + + def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 94358decb..09581c4ed 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -350,6 +350,8 @@ def _install_server_import_stubs(): {"key": "__starpilot_controller_action__:pulse_and_glide", "label": "Pulse and Glide", "section": "Controller Actions"}, {"key": "__starpilot_controller_action__:force_coast", "label": "Force Coasting", "section": "Controller Actions"}, {"key": "__starpilot_controller_action__:toggle_aol", "label": "Toggle AOL", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:engage_openpilot", "label": "Engage Openpilot", "section": "Controller Actions"}, + {"key": "__starpilot_controller_action__:disengage_openpilot", "label": "Disengage Openpilot", "section": "Controller Actions"}, ), CONTROLLER_ACTION_SET_SPEED="__starpilot_controller_action__:set_speed", CONTROLLER_ACTION_SLOT_COUNT=10, diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index eb72d5bbf..83b57fadd 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -228,6 +228,8 @@ def test_wheel_controls_status_includes_favorite_slots(monkeypatch): "__starpilot_controller_action__:pulse_and_glide", "__starpilot_controller_action__:force_coast", "__starpilot_controller_action__:toggle_aol", + "__starpilot_controller_action__:engage_openpilot", + "__starpilot_controller_action__:disengage_openpilot", } assert response.get_json()["speed_unit"] == "mph" @@ -249,6 +251,8 @@ def test_wheel_controls_configures_a_controller_only_action(monkeypatch): "__starpilot_controller_action__:pulse_and_glide", "__starpilot_controller_action__:force_coast", "__starpilot_controller_action__:toggle_aol", + "__starpilot_controller_action__:engage_openpilot", + "__starpilot_controller_action__:disengage_openpilot", } assert calls == [((9, "ForceOffroad", "Force Offroad", the_galaxy.params), {"value": None, "eligible_keys": expected_keys})] diff --git a/starpilot/system/wheel_controls/__init__.py b/starpilot/system/wheel_controls/__init__.py index 139ea8d01..46ac00407 100644 --- a/starpilot/system/wheel_controls/__init__.py +++ b/starpilot/system/wheel_controls/__init__.py @@ -3,6 +3,8 @@ from .wheel_controlsd import ( CONTROLLER_ACTION_OPTIONS, CONTROLLER_ACTION_BOOKMARK, CONTROLLER_ACTION_COUNTERS, + CONTROLLER_ACTION_DISENGAGE, + CONTROLLER_ACTION_ENGAGE, CONTROLLER_ACTION_FORCE_COAST, CONTROLLER_ACTION_PULSE_AND_GLIDE, CONTROLLER_ACTION_SET_SPEED, @@ -30,6 +32,8 @@ __all__ = [ "CONTROLLER_ACTION_OPTIONS", "CONTROLLER_ACTION_BOOKMARK", "CONTROLLER_ACTION_COUNTERS", + "CONTROLLER_ACTION_DISENGAGE", + "CONTROLLER_ACTION_ENGAGE", "CONTROLLER_ACTION_FORCE_COAST", "CONTROLLER_ACTION_PULSE_AND_GLIDE", "CONTROLLER_ACTION_SET_SPEED", diff --git a/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py b/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py index 993f690a2..d12bf69a6 100644 --- a/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py +++ b/starpilot/system/wheel_controls/tests/test_wheel_controlsd.py @@ -84,6 +84,8 @@ def test_controller_action_options_include_vehicle_controls(): assert options[wheel_controlsd.CONTROLLER_ACTION_PULSE_AND_GLIDE]["label"] == "Pulse and Glide" assert options[wheel_controlsd.CONTROLLER_ACTION_FORCE_COAST]["label"] == "Force Coasting" assert options[wheel_controlsd.CONTROLLER_ACTION_TOGGLE_AOL]["label"] == "Toggle AOL" + assert options[wheel_controlsd.CONTROLLER_ACTION_ENGAGE]["label"] == "Engage Openpilot" + assert options[wheel_controlsd.CONTROLLER_ACTION_DISENGAGE]["label"] == "Disengage Openpilot" def test_joystick_selection_is_explicit_and_exclusive(): @@ -235,6 +237,27 @@ def test_controller_actions_trigger_runtime_counters(): } +def test_controller_openpilot_actions_require_onroad_and_use_independent_counters(): + params = FakeParams({ + wheel_controlsd.CONTROLLER_ACTIONS_PARAM: [ + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_ENGAGE, "label": "Engage Openpilot"}, + {"enabled": True, "key": wheel_controlsd.CONTROLLER_ACTION_DISENGAGE, "label": "Disengage Openpilot"}, + ], + }) + memory = FakeParams() + + assert not wheel_controlsd.execute_controller_action(0, params, memory) + assert memory.values == {} + + params.values["IsOnroad"] = True + assert wheel_controlsd.execute_controller_action(0, params, memory) + assert wheel_controlsd.execute_controller_action(1, params, memory) + assert memory.values == { + "WheelControlEngageCounter": 1, + "WheelControlDisengageCounter": 1, + } + + def test_learning_accepts_the_tenth_controller_action(): params = FakeParams({"IsOffroad": True}) memory = FakeParams() diff --git a/starpilot/system/wheel_controls/wheel_controlsd.py b/starpilot/system/wheel_controls/wheel_controlsd.py index d5ddfe630..ab15ba892 100644 --- a/starpilot/system/wheel_controls/wheel_controlsd.py +++ b/starpilot/system/wheel_controls/wheel_controlsd.py @@ -37,11 +37,15 @@ CONTROLLER_ACTION_BOOKMARK = "__starpilot_controller_action__:bookmark" CONTROLLER_ACTION_PULSE_AND_GLIDE = "__starpilot_controller_action__:pulse_and_glide" CONTROLLER_ACTION_FORCE_COAST = "__starpilot_controller_action__:force_coast" CONTROLLER_ACTION_TOGGLE_AOL = "__starpilot_controller_action__:toggle_aol" +CONTROLLER_ACTION_ENGAGE = "__starpilot_controller_action__:engage_openpilot" +CONTROLLER_ACTION_DISENGAGE = "__starpilot_controller_action__:disengage_openpilot" CONTROLLER_ACTION_COUNTERS = { CONTROLLER_ACTION_BOOKMARK: "WheelButtonBookmarkCounter", CONTROLLER_ACTION_PULSE_AND_GLIDE: "WheelControlPulseGlideCounter", CONTROLLER_ACTION_FORCE_COAST: "WheelControlForceCoastCounter", CONTROLLER_ACTION_TOGGLE_AOL: "WheelControlAOLCounter", + CONTROLLER_ACTION_ENGAGE: "WheelControlEngageCounter", + CONTROLLER_ACTION_DISENGAGE: "WheelControlDisengageCounter", } CONTROLLER_ACTION_OPTIONS = ( { @@ -82,6 +86,18 @@ CONTROLLER_ACTION_OPTIONS = ( "description": "Toggles Always On Lateral like the vehicle LKAS button; it does not change the AOL setting.", "section": "Controller Actions", }, + { + "key": CONTROLLER_ACTION_ENGAGE, + "label": "Engage Openpilot", + "description": "Requests engagement through the normal openpilot readiness and safety checks.", + "section": "Controller Actions", + }, + { + "key": CONTROLLER_ACTION_DISENGAGE, + "label": "Disengage Openpilot", + "description": "Immediately disengages openpilot like the vehicle cancel button.", + "section": "Controller Actions", + }, ) CONTROLLER_ACTION_KEYS = {option["key"] for option in CONTROLLER_ACTION_OPTIONS} LEARN_TIMEOUT_SECONDS = 20.0 @@ -485,6 +501,8 @@ def execute_controller_action(index: int, params: Params, params_memory: Params) return set_controller_cruise_speed(slot.get("value"), params, params_memory) if slot.get("key") == CONTROLLER_ACTION_SELFIE: return request_comma_selfie() + if slot.get("key") in (CONTROLLER_ACTION_ENGAGE, CONTROLLER_ACTION_DISENGAGE) and not params.get_bool("IsOnroad"): + return False if slot.get("key") in CONTROLLER_ACTION_COUNTERS: return trigger_controller_action(slot["key"], params_memory) return execute_favorite_key(slot.get("key"), params, params_memory)