SLC override with -/+ bug

selfdrive/controls/tests/test_speed_limit_controller.py

@@ -424,6 +424,91 @@ def test_higher_limit_does_not_clear_override():
     controller.shutdown()
 
 
+def test_set_speed_mode_overrides_on_raise_without_gas():
+  # Max Set Speed mode: raising the set speed (+/-) above the posted limit must override
+  # the SLC hold with no gas pedal, targeting the set speed.
+  controller = make_controller(
+    speed_limit_controller_override_manual=False,
+    speed_limit_controller_override_set_speed=True,
+  )
+  try:
+    controller.source = "Dashboard"
+    controller.target = mph(45)
+    controller.last_valid_limit = mph(45)
+
+    # Baseline frame at the limit establishes the previous set speed (no rising edge yet).
+    controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
+    assert not controller.override_slc
+
+    # Driver presses + to 60 (rising edge): override arms and targets the set speed.
+    controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
+    assert controller.override_slc
+    assert controller.overridden_speed == pytest.approx(mph(60))
+
+    # Holding 60 with no further press: override stays latched.
+    controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
+    assert controller.override_slc
+    assert controller.overridden_speed == pytest.approx(mph(60))
+  finally:
+    controller.shutdown()
+
+
+def test_set_speed_override_clears_on_new_speed_zone():
+  # Entering a new (lower) posted limit clears the override; a steady high set speed must not
+  # re-arm it. Only a fresh +/- press re-arms.
+  controller = make_controller(
+    speed_limit_controller_override_manual=False,
+    speed_limit_controller_override_set_speed=True,
+  )
+  try:
+    controller.source = "Dashboard"
+    controller.target = mph(45)
+    controller.previous_source = "Dashboard"
+    controller.previous_target = mph(45)
+    controller.last_valid_limit = mph(45)
+
+    controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
+    controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
+    assert controller.override_slc
+
+    # New lower zone (35): update_limits clears the override for the new segment.
+    controller.update_limits(mph(35), datetime.now(timezone.utc), False, mph(60), mph(58), make_sm(gas_pressed=False))
+    controller.update_override(mph(60), 0.0, mph(58), 0.0, make_sm(gas_pressed=False))
+    assert controller.target == pytest.approx(mph(35))
+    # Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 35).
+    assert not controller.override_slc
+    assert controller.overridden_speed == 0
+
+    # A fresh + press (60 -> 65) re-arms against the new limit.
+    controller.update_override(mph(65), 0.0, mph(35), 0.0, make_sm(gas_pressed=False))
+    assert controller.override_slc
+    assert controller.overridden_speed == pytest.approx(mph(65))
+  finally:
+    controller.shutdown()
+
+
+def test_gas_pedal_mode_ignores_set_speed_without_gas():
+  # Set With Gas Pedal mode: a high set speed alone must NOT override; gas is still required.
+  controller = make_controller(
+    speed_limit_controller_override_manual=True,
+    speed_limit_controller_override_set_speed=False,
+  )
+  try:
+    controller.source = "Dashboard"
+    controller.target = mph(45)
+    controller.last_valid_limit = mph(45)
+
+    controller.update_override(mph(60), 0.0, mph(45), 0.0, make_sm(gas_pressed=False))
+    assert not controller.override_slc
+    assert controller.overridden_speed == 0
+
+    controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True))
+    assert controller.override_slc
+    assert controller.overridden_speed == pytest.approx(mph(50))
+  finally:
+    controller.shutdown()
+
+
 def test_manual_override_survives_brief_enabled_flicker():
   controller = make_controller()
   try:

starpilot/controls/lib/speed_limit_controller.py

@@ -37,6 +37,9 @@ OFFSET_MAP_METRIC = [
 ]
 
 SLC_OVERRIDE_DISABLE_CLEAR_TIME = 0.75
+# Minimum set-speed increase (m/s) counted as a deliberate +/- press. Below the smallest
+# real step (1 km/h ≈ 0.28 m/s), above cluster/float jitter.
+SET_SPEED_RAISE_EPS = 0.1
 VISION_LARGE_REFERENCE_SPEED_DELTA = 30 * CV.MPH_TO_MS
 VISION_LARGE_SET_SPEED_MIN_SUPPORT = 3
 VISION_SUPPORT_SPEED_TOLERANCE = 0.5 * CV.MPH_TO_MS
@@ -50,6 +53,7 @@ class SpeedLimitController:
     self.override_slc = False
     self.override_requires_gas_release = False
     self.override_disable_timer = 0.0
+    self._prev_v_cruise = None
 
     self.denied_target = 0
     self.map_speed_limit = 0
@@ -478,6 +482,14 @@ class SpeedLimitController:
         self.map_speed_limit = self.next_speed_limit
 
   def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm):
+    # A +/- press that raises the set speed is the gesture that (re)arms Max Set Speed
+    # override. Detect the rising edge on the raw set speed (button-driven, no cluster jitter);
+    # requiring a fresh edge is what makes the override clear per speed zone — once a new posted
+    # limit wipes it (clear_override_for_source_limit), a steady high set speed will not re-arm.
+    prev_v_cruise = self._prev_v_cruise
+    self._prev_v_cruise = v_cruise
+    set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS
+
     if not sm["selfdriveState"].enabled:
       self.override_disable_timer += DT_MDL
       if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME:
@@ -493,8 +505,14 @@ class SpeedLimitController:
 
     target_to_use = self.target_to_use
     offset = self.get_offset(target_to_use)
+    set_speed = v_cruise + v_cruise_diff
     self.override_slc = self.override_slc and self.overridden_speed > target_to_use + offset > 0
     self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > target_to_use + offset > 0
+    # Max Set Speed mode: raising the set speed (+/-) above the posted limit overrides the
+    # SLC hold directly, no gas pedal required. Only a fresh +/- press arms it, so entering a
+    # new speed zone clears the override until the driver raises the set speed again.
+    self.override_slc |= (self.starpilot_toggles.speed_limit_controller_override_set_speed
+                          and set_speed_raised and set_speed > target_to_use + offset > 0)
 
     if self.override_slc:
       if self.starpilot_toggles.speed_limit_controller_override_manual:
@@ -502,6 +520,6 @@ class SpeedLimitController:
           self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed)
         self.overridden_speed = float(np.clip(self.overridden_speed, target_to_use + offset, v_cruise + v_cruise_diff))
       elif self.starpilot_toggles.speed_limit_controller_override_set_speed:
-        self.overridden_speed = v_cruise + v_cruise_diff
+        self.overridden_speed = set_speed
     else:
       self.overridden_speed = 0