diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 52d5a3781..29b93bb51 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/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: diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index cb9af6ca9..7c64967ae 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/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