SLC: Fix ghost confirmation/simplify

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)

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(

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