From a2efcb6d5229cdf5bdfca20357716cda18a267ba Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 19 Apr 2026 01:06:41 -0500 Subject: [PATCH] SLC --- .../tests/test_speed_limit_controller.py | 139 ++++++++++++++++++ .../controls/lib/speed_limit_controller.py | 46 +++++- 2 files changed, 179 insertions(+), 6 deletions(-) create mode 100644 selfdrive/controls/tests/test_speed_limit_controller.py diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py new file mode 100644 index 000000000..edb7ee573 --- /dev/null +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -0,0 +1,139 @@ +from datetime import datetime, timezone +from types import SimpleNamespace + +import pytest + +from openpilot.common.constants import CV +from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController + + +class FakeParams: + def __init__(self, initial=None): + self.values = dict(initial or {}) + + def get(self, key, encoding=None): + return self.values.get(key) + + def get_bool(self, key): + return bool(self.values.get(key, False)) + + def get_float(self, key): + return float(self.values.get(key, 0) or 0) + + def put_nonblocking(self, key, value): + self.values[key] = value + + def remove(self, key): + self.values.pop(key, None) + + +def make_toggles(**overrides): + defaults = { + "is_metric": False, + "map_speed_lookahead_higher": 0.0, + "map_speed_lookahead_lower": 0.0, + "slc_fallback_previous_speed_limit": False, + "slc_fallback_set_speed": False, + "slc_mapbox_filler": False, + "speed_limit_confirmation_higher": False, + "speed_limit_confirmation_lower": False, + "speed_limit_controller_override_manual": True, + "speed_limit_controller_override_set_speed": False, + "speed_limit_filler": False, + "speed_limit_offset1": 0.0, + "speed_limit_offset2": 0.0, + "speed_limit_offset3": 0.0, + "speed_limit_offset4": 0.0, + "speed_limit_offset5": 0.0, + "speed_limit_offset6": 0.0, + "speed_limit_offset7": 0.0, + "speed_limit_priority1": "Dashboard", + "speed_limit_priority2": "Map Data", + "speed_limit_priority_highest": False, + "speed_limit_priority_lowest": False, + "vision_speed_limit_detection": False, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False): + return { + "carControl": SimpleNamespace(longActive=True), + "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0), + "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), + "mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0), + "selfdriveState": SimpleNamespace(enabled=enabled), + "starpilotCarState": SimpleNamespace(accelPressed=accel_pressed, decelPressed=decel_pressed), + } + + +def make_controller(**toggle_overrides): + params = FakeParams() + planner = SimpleNamespace( + gps_position={}, + gps_valid=False, + params=params, + params_memory=FakeParams(), + ) + controller = SpeedLimitController(SimpleNamespace(starpilot_planner=planner)) + controller.starpilot_toggles = make_toggles(**toggle_overrides) + return controller + + +def mph(value): + return value * CV.MPH_TO_MS + + +def test_new_source_limit_clears_override_until_gas_release(): + controller = make_controller() + try: + controller.source = "Dashboard" + controller.target = mph(55) + controller.previous_source = "Dashboard" + controller.previous_target = mph(55) + controller.overridden_speed = mph(65) + + sm = make_sm(gas_pressed=True) + controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.target == pytest.approx(mph(45)) + assert controller.source == "Dashboard" + assert controller.overridden_speed == 0 + assert not controller.override_slc + + controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.overridden_speed == 0 + assert not controller.override_slc + + controller.update_override(mph(75), 0.0, mph(65), 0.0, make_sm(gas_pressed=False)) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.overridden_speed == pytest.approx(mph(65)) + assert controller.override_slc + finally: + controller.shutdown() + + +def test_unconfirmed_lower_limit_keeps_existing_override(): + controller = make_controller(speed_limit_confirmation_lower=True) + try: + controller.source = "Dashboard" + controller.target = mph(55) + controller.previous_source = "Dashboard" + controller.previous_target = mph(55) + controller.overridden_speed = mph(65) + + sm = make_sm(gas_pressed=True) + controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.update_override(mph(75), 0.0, mph(65), 0.0, sm) + + assert controller.target == pytest.approx(mph(55)) + assert controller.unconfirmed_speed_limit == pytest.approx(mph(45)) + assert controller.overridden_speed == pytest.approx(mph(65)) + assert controller.override_slc + finally: + controller.shutdown() diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 02193536e..94ab0ce0e 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -41,6 +41,7 @@ class SpeedLimitController: self.calling_mapbox = False self.override_slc = False + self.override_requires_gas_release = False self.denied_target = 0 self.map_speed_limit = 0 @@ -100,6 +101,28 @@ class SpeedLimitController: next_map_speed_limit = {} return next_map_speed_limit if isinstance(next_map_speed_limit, dict) else {} + @property + def override_mode_enabled(self): + if self.starpilot_toggles is None: + return False + return self.starpilot_toggles.speed_limit_controller_override_manual or self.starpilot_toggles.speed_limit_controller_override_set_speed + + def override_active(self, v_ego, gas_pressed): + target_with_offset = self.target + self.offset + if target_with_offset <= 0 or not self.override_mode_enabled: + return False + return self.overridden_speed > target_with_offset or (gas_pressed and v_ego > target_with_offset) + + def clear_override_for_source_limit(self, desired_source, desired_target, had_override): + if desired_source == "None" or desired_target <= 0 or not had_override: + return + + # A new posted limit starts a new segment, so the previous segment's gas override + # should not carry through until the driver releases and reapplies the pedal. + self.override_slc = False + self.overridden_speed = 0 + self.override_requires_gas_release = True + def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm): if not self.starpilot_planner.gps_valid or not self.mapbox_token or (sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45: self.mapbox_limit = 0 @@ -227,17 +250,17 @@ class SpeedLimitController: self.mapbox_future = future future.add_done_callback(complete_request) - def handle_limit_change(self, desired_source, desired_target, sm): + def handle_limit_change(self, desired_source, desired_target, v_ego, sm): self.speed_limit_changed_timer += DT_MDL + had_override = self.override_active(v_ego, sm["carState"].gasPressed) speed_limit_accepted = (sm["starpilotCarState"].accelPressed and sm["carControl"].longActive) or self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted") speed_limit_denied = sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30) if speed_limit_accepted: - self.overridden_speed = 0 - self.source = desired_source self.target = desired_target + self.clear_override_for_source_limit(desired_source, desired_target, had_override) self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") @@ -250,10 +273,12 @@ class SpeedLimitController: elif desired_target < self.target and not self.starpilot_toggles.speed_limit_confirmation_lower: self.source = desired_source self.target = desired_target + self.clear_override_for_source_limit(desired_source, desired_target, had_override) elif desired_target > self.target and not self.starpilot_toggles.speed_limit_confirmation_higher: self.source = desired_source self.target = desired_target + self.clear_override_for_source_limit(desired_source, desired_target, had_override) else: self.source = "None" @@ -329,6 +354,7 @@ class SpeedLimitController: self.speed_limit_changed_timer = 0 self.unconfirmed_speed_limit = 0 self.overridden_speed = 0 + self.override_requires_gas_release = False if desired_target >= 1: self.source = desired_source @@ -340,7 +366,7 @@ class SpeedLimitController: return if abs(desired_target - self.previous_target) >= 1: - self.handle_limit_change(desired_source, desired_target, sm) + self.handle_limit_change(desired_source, desired_target, v_ego, sm) elif desired_source != self.source and abs(desired_target - self.target) < 1: self.source = desired_source else: @@ -385,9 +411,17 @@ 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): + if not sm["selfdriveState"].enabled: + self.override_slc = False + self.overridden_speed = 0 + self.override_requires_gas_release = False + return + + if not sm["carState"].gasPressed: + self.override_requires_gas_release = False + self.override_slc = self.overridden_speed > self.target + self.offset > 0 - self.override_slc |= sm["carState"].gasPressed and v_ego > self.target + self.offset > 0 - self.override_slc &= sm["selfdriveState"].enabled + self.override_slc |= not self.override_requires_gas_release and sm["carState"].gasPressed and v_ego > self.target + self.offset > 0 if self.override_slc: if self.starpilot_toggles.speed_limit_controller_override_manual: