From ffab4a8321d9a6d08a3936a16f535542610b0ec5 Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Mon, 24 Aug 2026 19:11:30 -0400 Subject: [PATCH] A pitstop! --- selfdrive/car/card.py | 10 ++++- selfdrive/car/cruise.py | 20 +++++++-- selfdrive/car/tests/test_cruise_speed.py | 41 +++++++++++++++++++ .../tests/test_speed_limit_controller.py | 23 +++++++++++ 4 files changed, 89 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 9e43863f62..2fbe1fd144 100644 --- a/selfdrive/car/card.py +++ b/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) diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index e2f770bb6d..8e57fc70b3 100644 --- a/selfdrive/car/cruise.py +++ b/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) diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index f9db1b678f..299b3fc0f9 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/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 diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 46c0bf31e8..23987979bf 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/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.