Prevent rejected SLC limit auto-application

selfdrive/controls/tests/test_speed_limit_controller.py

@@ -96,6 +96,30 @@ def mph(value):
   return value * CV.MPH_TO_MS
 
 
+def update_dashboard_limit(controller, now, current_limit, desired_limit, *, 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),
+  )
+
+
+def make_pending_lower_limit(current_limit, desired_limit):
+  controller = make_controller(
+    speed_limit_priority1="Dashboard",
+    speed_limit_confirmation_lower=True,
+  )
+  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))
+  return controller, now
+
+
 @pytest.mark.parametrize("limit_mph", [15, 25])
 def test_low_vision_limit_filter_blocks_configured_boundary(limit_mph):
   controller = make_controller(
@@ -439,6 +463,70 @@ 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)
+  try:
+    update_dashboard_limit(controller, now, 65, 45, decel_pressed=True)
+    assert controller.denied_target == pytest.approx(mph(45))
+
+    update_dashboard_limit(controller, now, 65, 45)
+
+    assert controller.source == "None"
+    assert controller.target == pytest.approx(mph(65))
+    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)
+  try:
+    for _ in range(int(30 / DT_MDL)):
+      update_dashboard_limit(controller, now, 55, 45)
+
+    assert controller.denied_target == pytest.approx(mph(45))
+
+    update_dashboard_limit(controller, now, 55, 45)
+
+    assert controller.source == "None"
+    assert controller.target == pytest.approx(mph(55))
+    assert controller.unconfirmed_speed_limit == 0
+  finally:
+    controller.shutdown()
+
+
+def test_new_lower_limit_prompts_after_denial():
+  controller, now = make_pending_lower_limit(65, 45)
+  try:
+    update_dashboard_limit(controller, now, 65, 45, decel_pressed=True)
+    update_dashboard_limit(controller, now, 65, 45)
+
+    update_dashboard_limit(controller, now, 65, 40)
+
+    assert controller.source == "None"
+    assert controller.target == pytest.approx(mph(65))
+    assert controller.unconfirmed_speed_limit == pytest.approx(mph(40))
+    assert controller.denied_target == 0
+  finally:
+    controller.shutdown()
+
+
+def test_denial_discards_stale_widget_acceptance():
+  controller, now = make_pending_lower_limit(65, 45)
+  try:
+    controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True
+    update_dashboard_limit(controller, now, 65, 45, decel_pressed=True)
+    update_dashboard_limit(controller, now, 65, 45)
+
+    update_dashboard_limit(controller, now, 65, 40)
+    update_dashboard_limit(controller, now, 65, 40)
+
+    assert controller.target == pytest.approx(mph(65))
+    assert controller.unconfirmed_speed_limit == pytest.approx(mph(40))
+  finally:
+    controller.shutdown()
+
+
 def test_set_speed_override_handles_higher_limit_changes():
   controller = make_controller()
   try:

starpilot/controls/lib/speed_limit_controller.py

@@ -303,6 +303,7 @@ class SpeedLimitController:
       self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
 
     elif speed_limit_denied:
+      self.starpilot_planner.params_memory.remove("SpeedLimitAccepted")
       self.denied_target = desired_target
 
       self.previous_source = desired_source
@@ -436,8 +437,22 @@ 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)
+    )
+    denied_same_limit = (
+      confirmation_required and self.denied_target > 0 and
+      abs(desired_target - self.denied_target) < 1
+    )
 
-    if not is_fallback and not same_speed and (abs(desired_target - self.previous_target) >= 1 or current_speed == 0):
+    if not denied_same_limit:
+      self.denied_target = 0
+
+    if denied_same_limit:
+      self.speed_limit_changed_timer = 0
+      self.unconfirmed_speed_limit = 0
+    elif not is_fallback and not same_speed and (abs(desired_target - self.previous_target) >= 1 or current_speed == 0):
       self.handle_limit_change(desired_source, desired_target, current_road_name, v_ego, sm)
     else:
       self.speed_limit_changed_timer = 0