From 65c8581db38e3faac20a30842b182f720d74771b Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Mon, 14 Sep 2026 16:07:33 -0400 Subject: [PATCH] SLC: Fix ghost confirmation/simplify --- selfdrive/car/tests/test_cruise_speed.py | 52 +++-- .../tests/test_speed_limit_controller.py | 208 ++++++++++++++++-- .../controls/lib/speed_limit_controller.py | 34 +-- 3 files changed, 239 insertions(+), 55 deletions(-) diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index 60158281d5..bdf21fecfe 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -415,32 +415,40 @@ class TestVCruiseHelper: assert self.v_cruise_helper.v_cruise_kph == initial_v_cruise_kph - def test_stale_speed_limit_change_does_adjust_cruise(self): - self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) - initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph + def test_stale_speed_limit_change_does_not_suppress_normal_cruise_buttons(self): plan = SimpleNamespace(speedLimitChanged=True, unconfirmedSlcSpeedLimit=0.0) - pressed_cs = car.CarState(cruiseState={"available": True}) - pressed_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=True)] - self.v_cruise_helper.update_v_cruise( - pressed_cs, - enabled=True, - is_metric=False, - speed_limit_changed=is_speed_limit_confirmation_pending(plan), - starpilot_toggles=self.starpilot_toggles, - ) + for button_type, increases_speed in ( + (ButtonType.accelCruise, True), + (ButtonType.decelCruise, False), + ): + self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) + initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph - released_cs = car.CarState(cruiseState={"available": True}) - released_cs.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=False)] - self.v_cruise_helper.update_v_cruise( - released_cs, - enabled=True, - is_metric=False, - speed_limit_changed=is_speed_limit_confirmation_pending(plan), - starpilot_toggles=self.starpilot_toggles, - ) + pressed_cs = car.CarState(cruiseState={"available": True}) + pressed_cs.buttonEvents = [ButtonEvent(type=button_type, pressed=True)] + self.v_cruise_helper.update_v_cruise( + pressed_cs, + enabled=True, + is_metric=False, + speed_limit_changed=is_speed_limit_confirmation_pending(plan), + starpilot_toggles=self.starpilot_toggles, + ) - assert self.v_cruise_helper.v_cruise_kph > initial_v_cruise_kph + released_cs = car.CarState(cruiseState={"available": True}) + released_cs.buttonEvents = [ButtonEvent(type=button_type, pressed=False)] + self.v_cruise_helper.update_v_cruise( + released_cs, + enabled=True, + is_metric=False, + speed_limit_changed=is_speed_limit_confirmation_pending(plan), + starpilot_toggles=self.starpilot_toggles, + ) + + if increases_speed: + assert self.v_cruise_helper.v_cruise_kph > initial_v_cruise_kph + else: + assert self.v_cruise_helper.v_cruise_kph < initial_v_cruise_kph def test_missing_custom_cruise_toggles_fall_back_to_single_step(self): self.enable(V_CRUISE_INITIAL * CV.KPH_TO_MS, False) diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 19f7b87ebe..07a517423b 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -96,17 +96,17 @@ def mph(value): return value * CV.MPH_TO_MS -def update_dashboard_limit(controller, now, current_limit, desired_limit, *, decel_pressed=False): +def update_dashboard_limit(controller, now, current_limit, desired_limit, *, accel_pressed=False, decel_pressed=False): controller.update_limits( mph(desired_limit), now, False, mph(current_limit), mph(current_limit), - make_sm(gas_pressed=False, decel_pressed=decel_pressed), + make_sm(gas_pressed=False, accel_pressed=accel_pressed, decel_pressed=decel_pressed), ) -def make_pending_lower_limit(current_limit, desired_limit): +def make_pending_limit(current_limit, desired_limit, confirmation_toggle): controller = make_controller( speed_limit_priority1="Dashboard", - speed_limit_confirmation_lower=True, + **{confirmation_toggle: True}, ) controller.source = "Dashboard" controller.target = mph(current_limit) @@ -120,6 +120,10 @@ def make_pending_lower_limit(current_limit, desired_limit): return controller, now +def make_pending_lower_limit(current_limit, desired_limit): + return make_pending_limit(current_limit, desired_limit, "speed_limit_confirmation_lower") + + @pytest.mark.parametrize("limit_mph", [15, 25]) def test_low_vision_limit_filter_blocks_configured_boundary(limit_mph): controller = make_controller( @@ -463,33 +467,51 @@ def test_unconfirmed_lower_limit_keeps_existing_override(): controller.shutdown() -def test_rejected_lower_limit_does_not_auto_apply_on_next_update(): - controller, now = make_pending_lower_limit(65, 45) +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "confirmation_toggle"), + [ + (65, 45, "speed_limit_confirmation_lower"), + (35, 45, "speed_limit_confirmation_higher"), + ], +) +def test_rejected_confirmation_does_not_auto_apply_on_next_update( + current_limit, desired_limit, confirmation_toggle, +): + controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) try: - update_dashboard_limit(controller, now, 65, 45, decel_pressed=True) - assert controller.denied_target == pytest.approx(mph(45)) + update_dashboard_limit(controller, now, current_limit, desired_limit, decel_pressed=True) + assert controller.denied_target == pytest.approx(mph(desired_limit)) - update_dashboard_limit(controller, now, 65, 45) + update_dashboard_limit(controller, now, current_limit, desired_limit) assert controller.source == "None" - assert controller.target == pytest.approx(mph(65)) + assert controller.target == pytest.approx(mph(current_limit)) assert controller.unconfirmed_speed_limit == 0 finally: controller.shutdown() -def test_timed_out_lower_limit_does_not_auto_apply(): - controller, now = make_pending_lower_limit(55, 45) +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "confirmation_toggle"), + [ + (55, 45, "speed_limit_confirmation_lower"), + (35, 45, "speed_limit_confirmation_higher"), + ], +) +def test_timed_out_confirmation_does_not_auto_apply( + current_limit, desired_limit, confirmation_toggle, +): + controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) try: for _ in range(int(30 / DT_MDL)): - update_dashboard_limit(controller, now, 55, 45) + update_dashboard_limit(controller, now, current_limit, desired_limit) - assert controller.denied_target == pytest.approx(mph(45)) + assert controller.denied_target == pytest.approx(mph(desired_limit)) - update_dashboard_limit(controller, now, 55, 45) + update_dashboard_limit(controller, now, current_limit, desired_limit) assert controller.source == "None" - assert controller.target == pytest.approx(mph(55)) + assert controller.target == pytest.approx(mph(current_limit)) assert controller.unconfirmed_speed_limit == 0 finally: controller.shutdown() @@ -704,6 +726,160 @@ def test_confirmation_accel_press_does_not_arm_set_speed_override(): controller.shutdown() +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "accel_pressed", "decel_pressed"), + [ + (65, 45, False, True), + (35, 45, True, False), + ], +) +def test_disabled_confirmation_does_not_consume_wheel_input( + current_limit, desired_limit, accel_pressed, decel_pressed, +): + controller = make_controller() + try: + controller.source = "Dashboard" + controller.target = mph(current_limit) + controller.previous_source = "Dashboard" + controller.previous_target = mph(current_limit) + controller.last_valid_limit = mph(current_limit) + controller._slc_adopt_counter = 1 + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + + controller.handle_limit_change( + "Dashboard", mph(desired_limit), "", mph(current_limit), + make_sm( + gas_pressed=False, + accel_pressed=accel_pressed, + decel_pressed=decel_pressed, + v_cruise_kph=current_limit * CV.MPH_TO_KPH, + ), + ) + + assert controller.target == pytest.approx(mph(desired_limit)) + assert controller.denied_target == 0 + assert controller.unconfirmed_speed_limit == 0 + assert not controller._set_speed_override_input_consumed + assert "SpeedLimitAccepted" not in controller.starpilot_planner.params_memory.values + assert "SLCForceCruiseSpeed" not in controller.starpilot_planner.params_memory.values + finally: + controller.shutdown() + + +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "confirmation_toggle", "confirmation_enabled"), + [ + (65, 45, "speed_limit_confirmation_lower", True), + (35, 45, "speed_limit_confirmation_higher", True), + (65, 45, "speed_limit_confirmation_lower", False), + (35, 45, "speed_limit_confirmation_higher", False), + ], +) +def test_directional_limit_changes_follow_confirmation_mode( + current_limit, desired_limit, confirmation_toggle, confirmation_enabled, +): + controller = make_controller( + speed_limit_priority1="Dashboard", + **{confirmation_toggle: confirmation_enabled}, + ) + try: + controller.source = "Dashboard" + controller.target = mph(current_limit) + controller.previous_source = "Dashboard" + controller.previous_target = mph(current_limit) + controller.last_valid_limit = mph(current_limit) + + update_dashboard_limit(controller, datetime.now(timezone.utc), current_limit, desired_limit) + + if confirmation_enabled: + assert controller.source == "None" + assert controller.target == pytest.approx(mph(current_limit)) + assert controller.unconfirmed_speed_limit == pytest.approx(mph(desired_limit)) + else: + assert controller.source == "Dashboard" + assert controller.target == pytest.approx(mph(desired_limit)) + assert controller.unconfirmed_speed_limit == 0 + finally: + controller.shutdown() + + +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "confirmation_toggle", "accel_pressed", "decel_pressed", "accepted"), + [ + (65, 45, "speed_limit_confirmation_lower", True, False, True), + (65, 45, "speed_limit_confirmation_lower", False, True, False), + (35, 45, "speed_limit_confirmation_higher", True, False, True), + (35, 45, "speed_limit_confirmation_higher", False, True, False), + ], +) +def test_confirmation_wheel_actions_accept_or_decline_pending_limit( + current_limit, desired_limit, confirmation_toggle, accel_pressed, decel_pressed, accepted, +): + controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) + try: + update_dashboard_limit( + controller, now, current_limit, desired_limit, + accel_pressed=accel_pressed, + decel_pressed=decel_pressed, + ) + + if accepted: + assert controller.source == "Dashboard" + assert controller.target == pytest.approx(mph(desired_limit)) + assert controller._set_speed_override_input_consumed + else: + assert controller.source == "None" + assert controller.target == pytest.approx(mph(current_limit)) + assert controller.denied_target == pytest.approx(mph(desired_limit)) + + # The following planner update clears the one-frame confirmation handoff state. + update_dashboard_limit(controller, now, current_limit, desired_limit) + assert controller.unconfirmed_speed_limit == 0 + finally: + controller.shutdown() + + +@pytest.mark.parametrize( + ("current_limit", "desired_limit", "confirmation_toggle", "accel_pressed", "decel_pressed"), + [ + (65, 45, "speed_limit_confirmation_lower", False, True), + (35, 45, "speed_limit_confirmation_higher", True, False), + ], +) +def test_disabling_confirmation_clears_pending_confirmation_immediately( + current_limit, desired_limit, confirmation_toggle, accel_pressed, decel_pressed, +): + controller = make_controller( + speed_limit_priority1="Dashboard", + **{confirmation_toggle: True}, + ) + try: + controller.source = "Dashboard" + controller.target = mph(current_limit) + controller.previous_source = "Dashboard" + controller.previous_target = mph(current_limit) + controller.last_valid_limit = mph(current_limit) + now = datetime.now(timezone.utc) + + update_dashboard_limit(controller, now, current_limit, desired_limit) + assert controller.unconfirmed_speed_limit == pytest.approx(mph(desired_limit)) + + setattr(controller.starpilot_toggles, confirmation_toggle, False) + + update_dashboard_limit( + controller, now, current_limit, desired_limit, + accel_pressed=accel_pressed, + decel_pressed=decel_pressed, + ) + + assert controller.target == pytest.approx(mph(desired_limit)) + assert controller.unconfirmed_speed_limit == 0 + assert controller.denied_target == 0 + assert not controller._set_speed_override_input_consumed + finally: + controller.shutdown() + + @pytest.mark.parametrize("accepted_by_accel_button", [True, False]) def test_higher_confirmation_raises_cruise_speed_to_target_with_offset(accepted_by_accel_button): controller = make_controller( diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 5aa9b4219e..aa647904bc 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -124,6 +124,12 @@ class SpeedLimitController: 0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0) ) + def _confirmation_required(self, desired_source, desired_target): + return desired_source != "None" and ( + (desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or + (desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher) + ) + def clear_override(self): self.override_slc = False self.overridden_speed = 0 @@ -281,15 +287,17 @@ class SpeedLimitController: long_active = sm["carControl"].longActive accepted_by_accel_button = sm["starpilotCarState"].accelPressed and long_active - confirmation_required = desired_source != "None" and ( - (desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or - (desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher) - ) + confirmation_required = self._confirmation_required(desired_source, desired_target) higher_confirmation = confirmation_required and desired_target > self.target - speed_limit_accepted = accepted_by_accel_button - if not speed_limit_accepted and self._slc_adopt_counter % 4 == 0: + speed_limit_accepted = confirmation_required and accepted_by_accel_button + if confirmation_required and not speed_limit_accepted and self._slc_adopt_counter % 4 == 0: speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted") - speed_limit_denied = sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active) + if not confirmation_required: + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + self.unconfirmed_speed_limit = 0 + speed_limit_denied = confirmation_required and ( + sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active) + ) if not long_active and not sm["selfdriveState"].enabled: speed_limit_accepted = True @@ -320,12 +328,7 @@ class SpeedLimitController: self.previous_target = desired_target self.previous_road_name = current_road_name - elif desired_target < self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_lower): - self.source = desired_source - self.target = desired_target - self.clear_persistent_override_for_limit_change(previous_limit, desired_target) - - elif desired_target > self.target and (desired_source == "None" or not self.starpilot_toggles.speed_limit_confirmation_higher): + elif desired_target != self.target and not confirmation_required: self.source = desired_source self.target = desired_target self.clear_persistent_override_for_limit_change(previous_limit, desired_target) @@ -447,10 +450,7 @@ class SpeedLimitController: # Do not trigger alerts when shifting to fallback or when re-obtaining the same speed limit is_fallback = desired_source == "None" or desired_target == 0 same_speed = desired_target > 0 and current_speed > 0 and abs(desired_target - current_speed) < 1 - confirmation_required = desired_source != "None" and ( - (desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or - (desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher) - ) + confirmation_required = self._confirmation_required(desired_source, desired_target) denied_same_limit = ( confirmation_required and self.denied_target > 0 and abs(desired_target - self.denied_target) < 1