A pitstop!

selfdrive/car/card.py

@@ -39,6 +39,7 @@ from openpilot.starpilot.controls.starpilot_card import StarPilotCard
 REPLAY = "REPLAY" in os.environ
 OPENPILOT_LEAD_MIN_DISTANCE = 0.1
 REDNECK_DECREASE_LOOKAHEAD_POINTS = 10
+SLC_SOURCE_NONE = "None"
 EventName = log.OnroadEvent.EventName
 
 # forward
@@ -276,14 +277,19 @@ class Car:
       self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
     )
     if not preap_software_cruise:
-      speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(self.sm['starpilotPlan'])
+      starpilot_plan = self.sm['starpilotPlan']
+      speed_limit_confirmation_pending = is_speed_limit_confirmation_pending(starpilot_plan)
+      slc_target_with_offset = 0.0
+      if self.starpilot_toggles.speed_limit_controller and starpilot_plan.slcSpeedLimitSource != SLC_SOURCE_NONE:
+        slc_target_with_offset = starpilot_plan.slcSpeedLimit + starpilot_plan.slcSpeedLimitOffset
       self.v_cruise_helper.update_v_cruise(
         CS,
         self.sm['carControl'].enabled,
         self.is_metric,
         speed_limit_confirmation_pending,
         self.starpilot_toggles,
-        FPCS,
+        starpilot_car_state=FPCS,
+        slc_target_with_offset=slc_target_with_offset,
       )
     else:
       preap_v_cruise_kph = float(CS.cruiseState.speed * CV.MS_TO_KPH)

selfdrive/car/cruise.py

@@ -89,13 +89,15 @@ class VCruiseHelper:
       return bool(getattr(starpilot_car_state, "decelHardCruise", False))
     return False
 
-  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None,
+                      slc_target_with_offset=0.0):
     self.v_cruise_kph_last = self.v_cruise_kph
 
     if CS.cruiseState.available:
       if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise:
         # if stock cruise is completely disabled, then we can use our own set speed logic
-        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state)
+        self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state,
+                                      slc_target_with_offset)
         self.v_cruise_cluster_kph = self.v_cruise_kph
         self.update_button_timers(CS, enabled, starpilot_car_state)
       else:
@@ -111,7 +113,8 @@ class VCruiseHelper:
       self.v_cruise_kph = V_CRUISE_UNSET
       self.v_cruise_cluster_kph = V_CRUISE_UNSET
 
-  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None):
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, starpilot_toggles, starpilot_car_state=None,
+                                slc_target_with_offset=0.0):
     # handle button presses. TODO: this should be in state_control, but a decelCruise press
     # would have the effect of both enabling and changing speed is checked after the state transition
     if not enabled:
@@ -169,11 +172,22 @@ class VCruiseHelper:
     short_interval, long_interval = self._get_cruise_delta_intervals(starpilot_toggles)
     v_cruise_delta_interval = long_interval if long_press or button_is_hard else short_interval
     v_cruise_delta = v_cruise_delta * v_cruise_delta_interval
+    previous_v_cruise_kph = self.v_cruise_kph
     if v_cruise_delta_interval % 5 == 0 and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
       self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
     else:
       self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
 
+    # Round to stored cruise-speed precision so the SLC target can be matched exactly.
+    slc_target_with_offset_kph = round(slc_target_with_offset * CV.MS_TO_KPH, 1)
+    # Preserve an active SLC target when an accel increment would otherwise skip it.
+    crossed_slc_target = (
+      button_type in ACCEL_CRUISE_BUTTONS and
+      previous_v_cruise_kph < slc_target_with_offset_kph < self.v_cruise_kph
+    )
+    if crossed_slc_target:
+      self.v_cruise_kph = slc_target_with_offset_kph
+
     # If set is pressed while overriding, clip cruise speed to minimum of vEgo
     if CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
       self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)

selfdrive/car/tests/test_cruise_speed.py

@@ -119,6 +119,47 @@ class TestVCruiseHelper:
         )
         assert pressed == (self.v_cruise_helper.v_cruise_kph == self.v_cruise_helper.v_cruise_kph_last)
 
+  def test_accel_stops_at_slc_target_before_crossing_it(self):
+    self.starpilot_toggles.cruise_increase = 5
+    self.v_cruise_helper.v_cruise_kph = 30
+    self.v_cruise_helper.v_cruise_cluster_kph = 30
+    slc_target_with_offset = 33 * CV.KPH_TO_MS
+
+    # A 30 km/h limit with a +3 km/h SLC offset should be an intermediate stop.
+    for expected_kph in (33, 35, 40):
+      for pressed in (True, False):
+        CS = car.CarState(cruiseState={"available": True})
+        CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
+        self.v_cruise_helper.update_v_cruise(
+          CS,
+          enabled=True,
+          is_metric=True,
+          speed_limit_changed=False,
+          starpilot_toggles=self.starpilot_toggles,
+          slc_target_with_offset=slc_target_with_offset,
+        )
+
+      assert self.v_cruise_helper.v_cruise_kph == pytest.approx(expected_kph)
+
+  def test_accel_uses_normal_interval_without_an_active_slc_target(self):
+    self.starpilot_toggles.cruise_increase = 5
+    self.v_cruise_helper.v_cruise_kph = 30
+    self.v_cruise_helper.v_cruise_cluster_kph = 30
+
+    # Card passes zero when SLC is disabled or has no active speed-limit source.
+    for pressed in (True, False):
+      CS = car.CarState(cruiseState={"available": True})
+      CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)]
+      self.v_cruise_helper.update_v_cruise(
+        CS,
+        enabled=True,
+        is_metric=True,
+        speed_limit_changed=False,
+        starpilot_toggles=self.starpilot_toggles,
+      )
+
+    assert self.v_cruise_helper.v_cruise_kph == pytest.approx(35)
+
   def test_hard_press_uses_long_press_interval(self):
     self.enable(52 * CV.MPH_TO_MS, False)
     initial_v_cruise_kph = self.v_cruise_helper.v_cruise_kph

selfdrive/controls/tests/test_speed_limit_controller.py

@@ -557,6 +557,29 @@ def test_set_speed_mode_overrides_on_raise_without_gas():
     controller.shutdown()
 
 
+def test_set_speed_mode_waits_until_above_slc_target_with_offset():
+  controller = make_controller(
+    is_metric=True,
+    speed_limit_controller_override_manual=False,
+    speed_limit_controller_override_set_speed=True,
+    speed_limit_offset2=3 * CV.KPH_TO_MS,
+  )
+  try:
+    controller.source = "Dashboard"
+    controller.target = 30 * CV.KPH_TO_MS
+    controller.last_valid_limit = controller.target
+
+    controller.update_override(30 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
+    controller.update_override(33 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
+    assert not controller.override_slc
+
+    controller.update_override(35 * CV.KPH_TO_MS, 0.0, 30 * CV.KPH_TO_MS, 0.0, make_sm(gas_pressed=False))
+    assert controller.override_slc
+    assert controller.overridden_speed == pytest.approx(35 * CV.KPH_TO_MS)
+  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.