From eff1c5dfcb00c58c4300c96ffa99c2c128369d8d Mon Sep 17 00:00:00 2001 From: firestarsdog <229254897+firestarsdog@users.noreply.github.com> Date: Mon, 28 Sep 2026 23:21:10 -0400 Subject: [PATCH] SLC --- cereal/custom.capnp | 2 + selfdrive/car/tests/test_cruise_speed.py | 15 +- .../tests/test_speed_limit_controller.py | 1528 ++++++----------- .../tests/test_starpilot_acceleration.py | 1 + .../controls/tests/test_starpilot_vcruise.py | 55 +- .../settings/starpilot/longitudinal.py | 2 +- .../ui/onroad/starpilot/slc_speed_limit.py | 335 +--- .../onroad/starpilot/starpilot_onroad_view.py | 24 +- .../starpilot/unified_speed_presentation.py | 60 + .../onroad/starpilot/widget_layout_manager.py | 4 +- .../ui/onroad/starpilot/widgets/__init__.py | 6 +- .../ui/onroad/starpilot/widgets/set_speed.py | 72 - .../onroad/starpilot/widgets/speed_limit.py | 67 - .../onroad/starpilot/widgets/unified_speed.py | 324 ++++ .../ui/tests/test_onroad_render_layers.py | 3 +- selfdrive/ui/tests/test_slc_sources_bubble.py | 38 +- .../tests/test_unified_speed_presentation.py | 186 ++ .../ui/tests/test_unified_speed_widget.py | 495 ++++++ .../ui/tests/test_widget_layout_manager.py | 13 + .../common/assets/device_settings_layout.json | 8 +- starpilot/common/starpilot_variables.py | 2 + starpilot/controls/lib/mapbox_speed_limit.py | 130 ++ .../controls/lib/speed_limit_controller.py | 845 ++++----- .../controls/lib/starpilot_acceleration.py | 3 - starpilot/controls/lib/starpilot_events.py | 2 +- starpilot/controls/lib/starpilot_vcruise.py | 45 +- starpilot/controls/starpilot_card.py | 28 +- starpilot/controls/starpilot_planner.py | 4 +- .../test_personality_longitudinal_profiles.py | 4 +- .../controls/tests/test_starpilot_card.py | 33 +- 30 files changed, 2350 insertions(+), 1984 deletions(-) create mode 100644 selfdrive/ui/onroad/starpilot/unified_speed_presentation.py delete mode 100644 selfdrive/ui/onroad/starpilot/widgets/set_speed.py delete mode 100644 selfdrive/ui/onroad/starpilot/widgets/speed_limit.py create mode 100644 selfdrive/ui/onroad/starpilot/widgets/unified_speed.py create mode 100644 selfdrive/ui/tests/test_unified_speed_presentation.py create mode 100644 selfdrive/ui/tests/test_unified_speed_widget.py create mode 100644 starpilot/controls/lib/mapbox_speed_limit.py diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 908dee7d56..6dda23c1c5 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -237,6 +237,8 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off + slcPresentedSpeedLimitSource @43 :Text; # source of the shown accepted or pending posted limit + slcIsLimitingMaxSet @44 :Bool; # SLC target is below the configured Max Set } struct StarPilotRadarState @0xb86e6369214c01c8 { diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index bdf21fecfe..69b0048236 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -118,14 +118,17 @@ 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): + @pytest.mark.parametrize( + ("starting_kph", "waypoint_kph", "expected_speeds"), + [(30, 33, (33, 35, 40)), (65, 68, (68, 70, 75))], + ) + def test_accel_stops_at_slc_target_before_crossing_it(self, starting_kph, waypoint_kph, expected_speeds): 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 + self.v_cruise_helper.v_cruise_kph = starting_kph + self.v_cruise_helper.v_cruise_cluster_kph = starting_kph + slc_target_with_offset = waypoint_kph * 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 expected_kph in expected_speeds: for pressed in (True, False): CS = car.CarState(cruiseState={"available": True}) CS.buttonEvents = [ButtonEvent(type=ButtonType.accelCruise, pressed=pressed)] diff --git a/selfdrive/controls/tests/test_speed_limit_controller.py b/selfdrive/controls/tests/test_speed_limit_controller.py index 319f3e8224..e3723ebaa1 100644 --- a/selfdrive/controls/tests/test_speed_limit_controller.py +++ b/selfdrive/controls/tests/test_speed_limit_controller.py @@ -1,16 +1,26 @@ -from datetime import UTC, datetime, timezone +from datetime import UTC, datetime +from concurrent.futures import Future from types import SimpleNamespace import pytest +from cereal import custom from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL -from openpilot.starpilot.controls.lib.speed_limit_controller import SpeedLimitController +from openpilot.starpilot.controls.lib.speed_limit_controller import ( + SOURCE_DASHBOARD, SOURCE_MAP, SOURCE_MAPBOX, SOURCE_NONE, SOURCE_PREVIOUS_LIMIT, SOURCE_VISION, SpeedLimitController, +) +from openpilot.starpilot.controls.lib.mapbox_speed_limit import MapboxSpeedLimit + + +def mph(speed): + return speed * CV.MPH_TO_MS class FakeParams: - def __init__(self, initial=None): - self.values = dict(initial or {}) + def __init__(self, values=None): + self.values = dict(values or {}) + self.writes = [] def get(self, key, encoding=None): return self.values.get(key) @@ -27,11 +37,9 @@ class FakeParams: def put_float(self, key, value): self.values[key] = value - def put_int(self, key, value): - self.values[key] = value - def put_nonblocking(self, key, value): self.values[key] = value + self.writes.append((key, value)) def remove(self, key): self.values.pop(key, None) @@ -42,1038 +50,642 @@ def make_toggles(**overrides): "is_metric": False, "map_speed_lookahead_higher": 0.0, "map_speed_lookahead_lower": 0.0, + "slc_fallback_experimental_mode": False, "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, "redneck_cruise": 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_priority1": SOURCE_DASHBOARD, + "speed_limit_priority2": SOURCE_MAP, "speed_limit_priority_highest": False, "speed_limit_priority_lowest": False, "vision_speed_limit_detection": False, "vision_speed_limit_low_limit_filter": False, "vision_speed_limit_low_limit_threshold": mph(25), } + defaults.update({f"speed_limit_offset{i}": 0.0 for i in range(1, 8)}) defaults.update(overrides) return SimpleNamespace(**defaults) -def make_sm(*, gas_pressed, enabled=True, accel_pressed=False, decel_pressed=False, long_active=True, standstill=False, v_cruise_kph=255.0): - return { +@pytest.fixture +def controller_factory(): + controllers = [] + + def create(*, persisted=0.0, **toggles): + params = FakeParams({"PreviousSpeedLimit": persisted} if persisted else {}) + planner = SimpleNamespace( + gps_position={}, gps_valid=False, params=params, params_memory=FakeParams(), + ) + controller = SpeedLimitController(SimpleNamespace(starpilot_planner=planner)) + controller.starpilot_toggles = make_toggles(**toggles) + controllers.append(controller) + return controller + + yield create + for controller in controllers: + controller.shutdown() + + +def step(controller, *, dashboard=0.0, map_limit=0.0, way=custom.WaySelectionType.fail, + next_limit=0.0, next_distance=0.0, road="", cruise=None, ego=None, + enabled=True, long_active=True, accel=False, decel=False, gas=False, + standstill=False, active=True, display_only=False, vision=None, support_count=0, + support_speed=0.0, cruise_diff=0.0, ego_diff=0.0): + cruise = mph(60) if cruise is None else cruise + ego = mph(50) if ego is None else ego + memory = controller.starpilot_planner.params_memory + if vision is not None: + memory.put_float("VisionSpeedLimit", vision) + memory.values["VisionSpeedLimitSupportCount"] = support_count + memory.put_float("VisionSpeedLimitSupportSpeed", support_speed) + sm = { "carControl": SimpleNamespace(longActive=long_active), - "carState": SimpleNamespace(gasPressed=gas_pressed, steeringAngleDeg=0.0, standstill=standstill, vCruise=v_cruise_kph), + "carState": SimpleNamespace(gasPressed=gas, steeringAngleDeg=0.0, standstill=standstill, + vCruise=cruise / CV.KPH_TO_MS), "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), - "mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, waySelectionType=0, roadName=""), + "mapdOut": SimpleNamespace(nextSpeedLimitDistance=next_distance, nextSpeedLimit=next_limit, + speedLimit=map_limit, waySelectionType=way, roadName=road), "selfdriveState": SimpleNamespace(enabled=enabled), - "starpilotCarState": SimpleNamespace(accelPressed=accel_pressed, decelPressed=decel_pressed), + "starpilotCarState": SimpleNamespace(accelPressed=accel, decelPressed=decel), } + controller.update(dashboard, datetime.now(UTC), False, cruise, cruise_diff, ego, ego_diff, sm, + active=active, display_only=display_only) + return sm -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 update_dashboard_limit(controller, now, current_limit, desired_limit, *, accel_pressed=False, decel_pressed=False): - controller.update_limits( - mph(desired_limit), now, False, mph(current_limit), mph(current_limit), - make_sm(gas_pressed=False, accel_pressed=accel_pressed, decel_pressed=decel_pressed), - ) - - -def make_pending_limit(current_limit, desired_limit, confirmation_toggle): - controller = make_controller( - speed_limit_priority1="Dashboard", - **{confirmation_toggle: True}, - ) - controller.source = "Dashboard" - controller.target = mph(current_limit) - controller.previous_source = "Dashboard" - controller.previous_target = mph(current_limit) - controller.last_valid_limit = mph(current_limit) - - now = datetime.now(timezone.utc) - update_dashboard_limit(controller, now, current_limit, desired_limit) - assert controller.unconfirmed_speed_limit == pytest.approx(mph(desired_limit)) - return controller, now - - -def make_pending_lower_limit(current_limit, desired_limit): - return make_pending_limit(current_limit, desired_limit, "speed_limit_confirmation_lower") - - -@pytest.mark.parametrize("limit_mph", [15, 25]) -def test_low_vision_limit_filter_blocks_configured_boundary(limit_mph): - controller = make_controller( - speed_limit_priority1="Vision", +@pytest.mark.parametrize( + ("priority1", "priority2", "highest", "lowest", "expected_source", "expected_speed"), + [ + (SOURCE_DASHBOARD, SOURCE_MAP, False, False, SOURCE_DASHBOARD, 45), + (SOURCE_MAP, SOURCE_DASHBOARD, False, False, SOURCE_MAP, 55), + (SOURCE_DASHBOARD, SOURCE_MAP, True, False, SOURCE_MAP, 55), + (SOURCE_DASHBOARD, SOURCE_MAP, False, True, SOURCE_DASHBOARD, 45), + (SOURCE_VISION, SOURCE_DASHBOARD, False, False, SOURCE_VISION, 65), + ], +) +def test_source_selection(controller_factory, priority1, priority2, highest, lowest, expected_source, expected_speed): + controller = controller_factory( + speed_limit_priority1=priority1, speed_limit_priority2=priority2, + speed_limit_priority_highest=highest, speed_limit_priority_lowest=lowest, vision_speed_limit_detection=True, + ) + step(controller, dashboard=mph(45), map_limit=mph(55), way=custom.WaySelectionType.current, + vision=mph(65), cruise=mph(65)) + assert controller.source == expected_source + assert controller.target == pytest.approx(mph(expected_speed)) + + +def test_vision_only_participates_when_configured(controller_factory): + controller = controller_factory(vision_speed_limit_detection=True) + step(controller, vision=mph(45), cruise=mph(45)) + assert controller.source == SOURCE_NONE + controller.starpilot_toggles.speed_limit_priority2 = SOURCE_VISION + step(controller, vision=mph(45), cruise=mph(45)) + assert controller.source == SOURCE_VISION + + +def test_vision_filter_and_large_discrepancy_support(controller_factory): + controller = controller_factory( + speed_limit_priority1=SOURCE_VISION, vision_speed_limit_detection=True, vision_speed_limit_low_limit_filter=True, - vision_speed_limit_low_limit_threshold=mph(25), ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(limit_mph)) - sm = make_sm(gas_pressed=False, v_cruise_kph=25 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(25), mph(20), sm) - - assert controller.vision_limit == pytest.approx(mph(limit_mph)) - assert controller.target == 0 - assert controller.source == "None" - finally: - controller.shutdown() + step(controller, vision=mph(25), cruise=mph(25)) + assert controller.vision_limit == pytest.approx(mph(25)) + assert controller.source == SOURCE_NONE + step(controller, vision=mph(70), cruise=mph(30), ego=mph(30), support_count=2, support_speed=mph(70)) + assert controller.source == SOURCE_NONE + step(controller, vision=mph(70), cruise=mph(30), ego=mph(30), support_count=3, support_speed=mph(70)) + assert controller.source == SOURCE_VISION -def test_low_vision_limit_filter_allows_limit_above_threshold(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, +def test_display_only_keeps_raw_vision_but_clears_control_state(controller_factory): + controller = controller_factory( + speed_limit_priority1=SOURCE_VISION, vision_speed_limit_detection=True, vision_speed_limit_low_limit_filter=True, - vision_speed_limit_low_limit_threshold=mph(25), ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(30)) - sm = make_sm(gas_pressed=False, v_cruise_kph=30 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(30), mph(25), sm) - - assert controller.target == pytest.approx(mph(30)) - assert controller.source == "Vision" - finally: - controller.shutdown() + step(controller, vision=mph(15), cruise=mph(15), active=False, display_only=True) + assert controller.source == SOURCE_VISION + assert controller.target == pytest.approx(mph(15)) + assert controller.last_valid_limit == 0 + assert not controller.confirmation_pending + assert controller.overridden_speed == 0 -def test_low_vision_limit_filter_is_action_only_for_display(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - vision_speed_limit_low_limit_filter=True, - vision_speed_limit_low_limit_threshold=mph(25), +def test_map_lookahead_and_explicit_failure(controller_factory): + controller = controller_factory(map_speed_lookahead_lower=5.0) + step(controller, map_limit=mph(55), next_limit=mph(45), next_distance=100, + way=custom.WaySelectionType.current, ego=mph(50)) + assert controller.map_speed_limit == pytest.approx(mph(45)) + assert controller.next_speed_limit == pytest.approx(mph(45)) + assert controller.last_valid_limit == pytest.approx(mph(45)) + step(controller, way=custom.WaySelectionType.fail) + assert controller.map_speed_limit == 0 + assert controller.next_speed_limit == 0 + assert controller.source == SOURCE_NONE + assert controller.last_valid_limit == pytest.approx(mph(45)) + + +def test_predicted_map_retains_only_lower_candidate(controller_factory): + controller = controller_factory() + step(controller, map_limit=mph(55), way=custom.WaySelectionType.current) + step(controller, map_limit=mph(65), way=custom.WaySelectionType.predicted) + assert controller.map_speed_limit == pytest.approx(mph(55)) + step(controller, map_limit=mph(45), way=custom.WaySelectionType.possible) + assert controller.map_speed_limit == pytest.approx(mph(45)) + + +def test_mapbox_fills_absence_and_resets_for_primary_source(controller_factory): + controller = controller_factory(slc_mapbox_filler=True) + controller.starpilot_planner.gps_valid = True + controller.mapbox.token = "test" + step(controller, ego=0) + controller.mapbox.limit = mph(45) + controller.mapbox.segment_distance = 1000 + step(controller) + assert controller.source == SOURCE_MAPBOX + assert controller.last_valid_source == SOURCE_MAPBOX + step(controller, dashboard=mph(55)) + assert controller.source == SOURCE_DASHBOARD + assert controller.mapbox.limit == 0 + + +def test_real_source_invalidates_inflight_mapbox_result(controller_factory): + controller = controller_factory(slc_mapbox_filler=True) + controller.starpilot_planner.gps_valid = True + controller.mapbox.token = "test" + old = Future() + old.set_running_or_notify_cancel() + controller.mapbox.future = old + step(controller, dashboard=mph(45)) + assert controller.mapbox.future is None + old.set_result((mph(65), 100.0)) + step(controller, dashboard=mph(45)) + assert controller.mapbox.limit == 0 + assert controller.source == SOURCE_DASHBOARD + + +@pytest.mark.parametrize("first_display_only", [False, True]) +def test_mode_transition_invalidates_inflight_mapbox_result(controller_factory, first_display_only): + controller = controller_factory(slc_mapbox_filler=True) + controller.starpilot_planner.gps_valid = True + controller.mapbox.token = "test" + step(controller, ego=0, active=not first_display_only, display_only=first_display_only) + old = Future() + old.set_running_or_notify_cancel() + controller.mapbox.future = old + + step(controller, ego=0, active=first_display_only, display_only=not first_display_only) + assert controller.mapbox.future is None + old.set_result((mph(65), 100.0)) + step(controller, ego=0, active=first_display_only, display_only=not first_display_only) + assert controller.mapbox.limit == 0 + assert controller.source == SOURCE_NONE + + +def test_previous_fallback_keeps_real_history_and_session_source(controller_factory): + controller = controller_factory(slc_fallback_previous_speed_limit=True) + step(controller, dashboard=mph(45)) + writes = list(controller.starpilot_planner.params.writes) + step(controller) + assert controller.target == pytest.approx(mph(45)) + assert controller.source == SOURCE_DASHBOARD + assert controller.last_valid_limit == pytest.approx(mph(45)) + assert controller.starpilot_planner.params.writes == writes + + +def test_previous_fallback_startup_has_unknown_source(controller_factory): + controller = controller_factory(persisted=mph(45), slc_fallback_previous_speed_limit=True) + step(controller) + assert controller.target == pytest.approx(mph(45)) + assert controller.source == SOURCE_NONE + assert controller.last_valid_source == SOURCE_NONE + assert controller.presented_source == SOURCE_PREVIOUS_LIMIT + + +def test_set_speed_fallback_does_not_present_a_posted_limit(controller_factory): + controller = controller_factory(persisted=mph(45), slc_fallback_set_speed=True) + step(controller, cruise=mph(60)) + assert controller.target == pytest.approx(mph(60)) + assert controller.presented_source == SOURCE_NONE + + +@pytest.mark.parametrize("fallback", ["set", "experimental"]) +def test_fallback_never_enters_accepted_history(controller_factory, fallback): + controller = controller_factory( + slc_fallback_set_speed=fallback == "set", + slc_fallback_experimental_mode=fallback == "experimental", ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(20), mph(15), sm, display_only=True) - - assert controller.target == pytest.approx(mph(15)) - assert controller.source == "Vision" - finally: - controller.shutdown() + step(controller, cruise=mph(60)) + assert controller.source == SOURCE_NONE + assert controller.target == (mph(60) if fallback == "set" else 0.0) + assert controller.experimental_mode == (fallback == "experimental") + assert controller.last_valid_limit == 0 + assert controller.starpilot_planner.params.writes == [] -def test_low_vision_limit_filter_does_not_filter_dashboard_source(): - controller = make_controller( - speed_limit_priority1="Vision", - speed_limit_priority2="Dashboard", - vision_speed_limit_detection=True, - vision_speed_limit_low_limit_filter=True, - vision_speed_limit_low_limit_threshold=mph(25), - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH) - - controller.update_limits(mph(15), datetime.now(timezone.utc), False, mph(20), mph(15), sm) - - assert controller.target == pytest.approx(mph(15)) - assert controller.source == "Dashboard" - finally: - controller.shutdown() +def test_adopt_uses_real_candidate_only(controller_factory): + controller = controller_factory(slc_fallback_set_speed=True) + memory = controller.starpilot_planner.params_memory + memory.values["SLCAdoptSpeedLimit"] = True + step(controller, cruise=mph(60)) + assert controller.last_valid_limit == 0 + assert "SLCForceCruiseSpeed" not in memory.values + assert "SLCAdoptSpeedLimit" not in memory.values + memory.values["SLCAdoptSpeedLimit"] = True + step(controller, dashboard=mph(45)) + assert controller.last_valid_limit == pytest.approx(mph(45)) + assert memory.get_float("SLCForceCruiseSpeed") == pytest.approx(mph(45)) -def test_low_vision_limit_filter_does_not_restore_filtered_vision_fallback(): - controller = make_controller( - speed_limit_priority1="Vision", - slc_fallback_previous_speed_limit=True, - vision_speed_limit_detection=True, - vision_speed_limit_low_limit_filter=True, - vision_speed_limit_low_limit_threshold=mph(25), - ) - try: - controller.previous_source = "Vision" - controller.previous_target = mph(15) - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - sm = make_sm(gas_pressed=False, v_cruise_kph=20 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(20), mph(15), sm) - - assert controller.target == 0 - assert controller.source == "None" - finally: - controller.shutdown() +def test_pending_candidate_changes_speed_and_source(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True, speed_limit_priority1=SOURCE_MAP, + speed_limit_priority2=SOURCE_DASHBOARD) + step(controller, dashboard=mph(65)) + step(controller, dashboard=mph(45)) + assert controller.confirmation_pending + assert controller.pending_source == SOURCE_DASHBOARD + assert controller.limit_change_started + first_time = controller.confirmation_time + step(controller, dashboard=mph(45)) + assert not controller.limit_change_started + assert controller.confirmation_time > first_time + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current) + assert controller.pending_source == SOURCE_MAP + assert not controller.limit_change_started + assert controller.confirmation_time > first_time + step(controller, map_limit=mph(55), way=custom.WaySelectionType.current) + assert controller.pending_limit == pytest.approx(mph(55)) + assert controller.pending_source == SOURCE_MAP + assert controller.limit_change_started + assert controller.confirmation_time == pytest.approx(DT_MDL) -def test_large_vision_delta_requires_three_detector_frames(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) - sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) - assert controller.target == 0 - - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) - assert controller.target == pytest.approx(mph(15)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_pending_disappearance_discards_stale_acceptance(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45)) + assert controller.confirmation_pending + step(controller) + assert not controller.confirmation_pending + assert controller.confirmation_time == 0 + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + step(controller) + assert controller.last_valid_limit == pytest.approx(mph(55)) + assert "SpeedLimitAccepted" not in controller.starpilot_planner.params_memory.values -def test_normal_vision_delta_keeps_fast_path(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(50)) - sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm) - assert controller.target == pytest.approx(mph(50)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_ui_acceptance_only_accepts_stored_pending_candidate(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(65)) + step(controller, dashboard=mph(45)) + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + step(controller, dashboard=mph(55)) + assert controller.pending_limit == pytest.approx(mph(55)) + assert controller.last_valid_limit == pytest.approx(mph(65)) + step(controller, dashboard=mph(55)) + assert controller.confirmation_pending + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + step(controller, dashboard=mph(55)) + assert controller.target == pytest.approx(mph(55)) + assert controller.last_valid_limit == pytest.approx(mph(55)) -def test_inactive_valid_cruise_still_applies_large_delta_guard(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(20)) - sm = make_sm(gas_pressed=False, long_active=False, v_cruise_kph=60 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(60), mph(25), sm) - assert controller.target == 0 - assert controller.source == "None" - finally: - controller.shutdown() +def test_accel_on_replacement_frame_does_not_accept_unshown_speed(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(65)) + step(controller, dashboard=mph(45)) + step(controller, dashboard=mph(55), accel=True) + assert controller.pending_limit == pytest.approx(mph(55)) + assert controller.last_valid_limit == pytest.approx(mph(65)) + assert controller.confirmation_button_consumed + step(controller, dashboard=mph(55), accel=True) + assert controller.target == pytest.approx(mph(55)) + assert controller.last_valid_limit == pytest.approx(mph(55)) -def test_unset_active_cruise_uses_vehicle_speed(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(55)) - sm = make_sm(gas_pressed=False) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(50), sm) - assert controller.target == pytest.approx(mph(55)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_accel_confirmation_consumes_adjacent_set_speed_edge(controller_factory): + controller = controller_factory(speed_limit_confirmation_higher=True) + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(50), cruise=mph(45)) + step(controller, dashboard=mph(50), cruise=mph(45), accel=True) + assert controller.target == pytest.approx(mph(50)) + assert controller.confirmation_button_consumed + step(controller, dashboard=mph(50), cruise=mph(55)) + assert controller.overridden_speed == 0 + step(controller, dashboard=mph(50), cruise=mph(60), accel=True) + assert controller.overridden_speed == pytest.approx(mph(60)) -def test_unset_cruise_applies_vehicle_speed_large_delta_guard(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) - sm = make_sm(gas_pressed=False) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(75), sm) - assert controller.target == 0 - assert controller.source == "None" - - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(90), mph(75), sm) - assert controller.target == pytest.approx(mph(15)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_higher_confirmation_forces_cruise_only_when_needed(controller_factory): + controller = controller_factory(speed_limit_confirmation_higher=True, speed_limit_offset4=mph(5)) + step(controller, dashboard=mph(35), cruise=mph(40)) + step(controller, dashboard=mph(45), cruise=mph(40)) + controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True + step(controller, dashboard=mph(45), cruise=mph(40)) + assert controller.target == pytest.approx(mph(45)) + assert controller.starpilot_planner.params_memory.get_float("SLCForceCruiseSpeed") == pytest.approx(mph(50)) -def test_standstill_ignores_vehicle_speed_jitter_for_vision_limit_guard(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(35)) - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(35)) - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 2) - sm = make_sm(gas_pressed=False, standstill=True) - - for v_ego in (-0.0067, 0.0005): - controller.update_limits(0.0, datetime.now(UTC), False, mph(35), v_ego, sm) - assert controller.target == pytest.approx(mph(35)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_enabled_without_long_control_does_not_time_out_confirmation(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + for _ in range(int(30 / DT_MDL) + 1): + step(controller, dashboard=mph(45), long_active=False, enabled=True) + assert controller.confirmation_pending + assert controller.denied_limit == 0 -def test_display_only_applies_large_delta_guard(): - controller = make_controller( - speed_limit_priority1="Vision", - vision_speed_limit_detection=True, - ) - try: - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimit", mph(15)) - controller.starpilot_planner.params_memory.put_float("VisionSpeedLimitSupportSpeed", mph(15)) - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 1) - sm = make_sm(gas_pressed=False, v_cruise_kph=75 * CV.MPH_TO_KPH) - - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm, display_only=True) - assert controller.target == 0 - assert controller.source == "None" - - controller.starpilot_planner.params_memory.put_int("VisionSpeedLimitSupportCount", 3) - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(75), mph(70), sm, display_only=True) - assert controller.target == pytest.approx(mph(15)) - assert controller.source == "Vision" - finally: - controller.shutdown() +def test_rejection_and_timeout_do_not_change_history(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + writes = list(controller.starpilot_planner.params.writes) + step(controller, dashboard=mph(45)) + step(controller, dashboard=mph(45), decel=True) + assert controller.denied_limit == pytest.approx(mph(45)) + assert controller.presented_source == SOURCE_DASHBOARD + assert controller.last_valid_limit == pytest.approx(mph(55)) + assert controller.starpilot_planner.params.writes == writes + step(controller, dashboard=mph(45)) + assert not controller.confirmation_pending + step(controller, dashboard=mph(40)) + for _ in range(int(30 / DT_MDL) + 1): + step(controller, dashboard=mph(40)) + assert controller.denied_limit == pytest.approx(mph(40)) + assert controller.last_valid_limit == pytest.approx(mph(55)) -def test_set_speed_override_survives_source_changes_and_fallback_until_driver_clears(): - controller = make_controller( - speed_limit_priority1="Map Data", - speed_limit_priority2="Dashboard", +def test_rejected_limit_does_not_label_set_speed_fallback_as_posted(controller_factory): + controller = controller_factory( + speed_limit_confirmation_lower=True, slc_fallback_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(55), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - - # Dashboard 45 -> Map Data 45 is not a new speed zone. - map_sm = make_sm(gas_pressed=False) - map_sm["mapdOut"].speedLimit = mph(45) - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), map_sm) - controller.update_override(mph(55), 0.0, mph(50), 0.0, map_sm) - assert controller.source == "Map Data" - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - # A temporary fallback, and even a complete source dropout, do not clear the override. - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert controller.source == "None" - assert controller.target == pytest.approx(mph(55)) - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - controller.starpilot_toggles.slc_fallback_set_speed = False - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert controller.target == 0 - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert controller.source == "Dashboard" - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - # Set-speed fallback does not clear passively, but a fresh - to the retained target does. - controller.starpilot_toggles.slc_fallback_set_speed = True - controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - controller.update_override(mph(45), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert not controller.override_slc - assert controller.overridden_speed == 0 - finally: - controller.shutdown() + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45)) + step(controller, dashboard=mph(45), decel=True) + assert controller.presented_source == SOURCE_DASHBOARD + step(controller, cruise=mph(60)) + assert controller.target == pytest.approx(mph(60)) + assert controller.presented_source == SOURCE_NONE -def test_unconfirmed_lower_limit_keeps_existing_override(): - # First, verify startup behavior where target is 0 and priority limit is detected - startup_controller = make_controller( - speed_limit_priority1="Dashboard", - slc_fallback_previous_speed_limit=True, - ) - try: - startup_controller.previous_target = mph(55) - startup_controller.previous_source = "Dashboard" - startup_controller.target = 0 +def test_disabling_confirmation_accepts_a_previously_denied_limit(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45)) + step(controller, dashboard=mph(45), decel=True) + assert controller.denied_limit == pytest.approx(mph(45)) - sm = make_sm(gas_pressed=False) - startup_controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(75), mph(65), sm) + controller.starpilot_toggles.speed_limit_confirmation_lower = False + step(controller, dashboard=mph(45)) + assert controller.source == SOURCE_DASHBOARD + assert controller.target == pytest.approx(mph(45)) + assert controller.denied_limit == 0 + assert not controller.limit_change_started - assert startup_controller.target == pytest.approx(mph(45)) - assert startup_controller.source == "Dashboard" - finally: - startup_controller.shutdown() - # Verify Bug 3: Fallback transitions should bypass confirmation checks - fallback_confirm_controller = make_controller( - slc_fallback_set_speed=True, - speed_limit_confirmation_higher=True - ) - try: - fallback_confirm_controller.source = "Dashboard" - fallback_confirm_controller.target = mph(35) - fallback_confirm_controller.previous_target = mph(35) +def test_road_change_clears_denial_before_comparison(controller_factory): + controller = controller_factory(speed_limit_priority1=SOURCE_MAP, speed_limit_confirmation_lower=True) + step(controller, map_limit=mph(55), way=custom.WaySelectionType.current, road="Road A") + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current, road="Road A", decel=True) + assert controller.denied_limit == pytest.approx(mph(45)) + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current, road="Road B") + assert controller.confirmation_pending + assert controller.pending_limit == pytest.approx(mph(45)) - sm = make_sm(gas_pressed=False) - fallback_confirm_controller.update_limits(0.0, datetime.now(timezone.utc), False, mph(60), mph(35), sm) - assert fallback_confirm_controller.target == pytest.approx(mph(60)) - assert fallback_confirm_controller.source == "None" - assert fallback_confirm_controller.unconfirmed_speed_limit == 0 - finally: - fallback_confirm_controller.shutdown() +def test_same_speed_source_change_does_not_alert_or_disturb_override(controller_factory): + controller = controller_factory(speed_limit_priority1=SOURCE_MAP, speed_limit_priority2=SOURCE_DASHBOARD) + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(60)) + assert controller.set_speed_override == pytest.approx(mph(60)) + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current, cruise=mph(60)) + assert controller.source == SOURCE_MAP + assert controller.last_valid_source == SOURCE_MAP + assert not controller.limit_change_started + assert controller.set_speed_override == pytest.approx(mph(60)) - # Verify Bug 1: Boundaries are correctly mapped and not falling back to 0 - boundary_controller = make_controller() - boundary_controller.starpilot_toggles.speed_limit_offset1 = 1.0 - boundary_controller.starpilot_toggles.speed_limit_offset2 = 2.0 - # Exact boundary speed: 11.2 m/s is the *start* of band 2 (25–34 mph range). - # With low <= target < high: 11.2 <= 11.2 < 15.2 → True → maps to offset2 (not 0). - offset = boundary_controller.get_offset(11.2) - assert offset != 0.0 +def test_equivalent_speed_churn_does_not_rewrite_persisted_limit(controller_factory): + controller = controller_factory() + step(controller, dashboard=mph(45)) + initial_writes = list(controller.starpilot_planner.params.writes) + for speed in (45.5, 45, 45.5, 45): + step(controller, dashboard=mph(speed)) + assert controller.starpilot_planner.params.writes == initial_writes + assert controller.last_valid_limit == pytest.approx(mph(45)) + step(controller, dashboard=mph(50)) + assert controller.starpilot_planner.params.writes[-1] == ("PreviousSpeedLimit", mph(50)) + assert len(controller.starpilot_planner.params.writes) == len(initial_writes) + 1 - 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) +def test_override_pedal_layers_over_persistent_and_new_lower_clears(controller_factory): + controller = controller_factory() + step(controller, dashboard=mph(45), cruise=mph(45), ego=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(60), ego=mph(45)) + assert controller.overridden_speed == pytest.approx(mph(60)) + step(controller, dashboard=mph(45), cruise=mph(60), ego=mph(65), gas=True) + assert controller.overridden_speed == pytest.approx(mph(65)) + step(controller, dashboard=mph(45), cruise=mph(60), ego=mph(50)) + assert controller.overridden_speed == pytest.approx(mph(60)) + step(controller, dashboard=mph(40), cruise=mph(60)) + assert controller.set_speed_override == 0 + assert controller.overridden_speed == 0 - 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() + +def test_higher_limit_clears_override_only_when_target_reaches_it(controller_factory): + controller = controller_factory() + step(controller, dashboard=mph(35), cruise=mph(35)) + step(controller, dashboard=mph(35), cruise=mph(55)) + step(controller, dashboard=mph(45), cruise=mph(55)) + assert controller.set_speed_override == pytest.approx(mph(55)) + step(controller, dashboard=mph(55), cruise=mph(55)) + assert controller.set_speed_override == 0 + + +def test_higher_limit_with_offset_clears_reached_override(controller_factory): + controller = controller_factory(speed_limit_offset4=mph(5)) + step(controller, dashboard=mph(35), cruise=mph(35)) + step(controller, dashboard=mph(35), cruise=mph(50)) + assert controller.set_speed_override == pytest.approx(mph(50)) + step(controller, dashboard=mph(45), cruise=mph(50)) + assert controller.set_speed_override == 0 + + +def test_plus_after_automatic_acceptance_is_a_real_override_edge(controller_factory): + controller = controller_factory() + step(controller, dashboard=mph(45), cruise=mph(45)) + assert not controller.confirmation_pending + step(controller, dashboard=mph(45), cruise=mph(55), accel=True) + assert controller.set_speed_override == pytest.approx(mph(55)) + + +def test_redneck_override_can_move_below_target(controller_factory): + controller = controller_factory(redneck_cruise=True) + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(60)) + step(controller, dashboard=mph(45), cruise=mph(35)) + assert controller.overridden_speed == pytest.approx(mph(35)) @pytest.mark.parametrize( - ("current_limit", "desired_limit", "confirmation_toggle"), - [ - (65, 45, "speed_limit_confirmation_lower"), - (35, 45, "speed_limit_confirmation_higher"), - ], + ("manual_setting", "set_speed_setting"), + [(False, False), (True, False), (False, True)], ) -def test_rejected_confirmation_does_not_auto_apply_on_next_update( - current_limit, desired_limit, confirmation_toggle, +def test_legacy_slc_override_setting_does_not_change_current_override_policy( + controller_factory, manual_setting, set_speed_setting, ): - controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) - try: - update_dashboard_limit(controller, now, current_limit, desired_limit, decel_pressed=True) - assert controller.denied_target == pytest.approx(mph(desired_limit)) - - update_dashboard_limit(controller, now, current_limit, desired_limit) - - assert controller.source == "None" - assert controller.target == pytest.approx(mph(current_limit)) - assert controller.unconfirmed_speed_limit == 0 - finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "confirmation_toggle"), - [ - (55, 45, "speed_limit_confirmation_lower"), - (35, 45, "speed_limit_confirmation_higher"), - ], -) -def test_timed_out_confirmation_does_not_auto_apply( - current_limit, desired_limit, confirmation_toggle, -): - controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) - try: - for _ in range(int(30 / DT_MDL)): - update_dashboard_limit(controller, now, current_limit, desired_limit) - - assert controller.denied_target == pytest.approx(mph(desired_limit)) - - update_dashboard_limit(controller, now, current_limit, desired_limit) - - assert controller.source == "None" - assert controller.target == pytest.approx(mph(current_limit)) - assert controller.unconfirmed_speed_limit == 0 - finally: - controller.shutdown() - - -def test_new_lower_limit_prompts_after_denial(): - controller, now = make_pending_lower_limit(65, 45) - try: - update_dashboard_limit(controller, now, 65, 45, decel_pressed=True) - update_dashboard_limit(controller, now, 65, 45) - - update_dashboard_limit(controller, now, 65, 40) - - assert controller.source == "None" - assert controller.target == pytest.approx(mph(65)) - assert controller.unconfirmed_speed_limit == pytest.approx(mph(40)) - assert controller.denied_target == 0 - finally: - controller.shutdown() - - -def test_denial_discards_stale_widget_acceptance(): - controller, now = make_pending_lower_limit(65, 45) - try: - controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True - update_dashboard_limit(controller, now, 65, 45, decel_pressed=True) - update_dashboard_limit(controller, now, 65, 45) - - update_dashboard_limit(controller, now, 65, 40) - update_dashboard_limit(controller, now, 65, 40) - - assert controller.target == pytest.approx(mph(65)) - assert controller.unconfirmed_speed_limit == pytest.approx(mph(40)) - finally: - controller.shutdown() - - -def test_set_speed_override_handles_higher_limit_changes(): - controller = make_controller() - try: - controller.source = "Dashboard" - controller.target = mph(35) - controller.previous_source = "Dashboard" - controller.previous_target = mph(35) - controller.last_valid_limit = mph(35) - - controller.update_override(mph(35), 0.0, mph(35), 0.0, make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(35), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(55)) - - # A higher limit below the selected override preserves it. - controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - - assert controller.target == pytest.approx(mph(45)) - assert controller.source == "Dashboard" - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - # A higher effective target that reaches the override clears it without re-arming. - controller.update_limits(mph(55), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - controller.update_override(mph(55), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert controller.target == pytest.approx(mph(55)) - assert controller.overridden_speed == 0 - assert not controller.override_slc - finally: - controller.shutdown() - - -def test_pedal_and_set_speed_overrides_are_independent(): - controller = make_controller() - try: - controller.source = "Dashboard" - controller.target = mph(45) - controller.last_valid_limit = mph(45) - - # A pedal pass is temporary; a set-speed increase is the fixed persistent action. - controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) - controller.update_override(mph(45), 0.0, mph(55), 0.0, make_sm(gas_pressed=True)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(55)) - - controller.update_override(mph(45), 0.0, mph(55), 0.0, make_sm(gas_pressed=False)) - assert not controller.override_slc - assert controller.overridden_speed == 0 - - # A fresh + above the effective SLC target arms the override. - controller.update_override(mph(55), 0.0, mph(55), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(55)) - - # Pedaling temporarily takes priority, then returns to the selected set speed. - controller.update_override(mph(55), 0.0, mph(60), 0.0, make_sm(gas_pressed=True)) - assert controller.overridden_speed == pytest.approx(mph(60)) - - controller.update_override(mph(55), 0.0, mph(60), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(55)) - - 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)) - - controller.update_override(mph(50), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(50)) - - # Returning to the effective SLC target ends the persistent override. - controller.update_override(mph(45), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) - assert not controller.override_slc - assert controller.overridden_speed == 0 - finally: - controller.shutdown() - - -def test_persistent_override_waits_until_above_slc_target_with_offset(): - controller = make_controller( - is_metric=True, - speed_limit_offset2=3 * CV.KPH_TO_MS, + controller = controller_factory( + speed_limit_controller_override_manual=manual_setting, + speed_limit_controller_override_set_speed=set_speed_setting, ) + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(55)) + assert controller.set_speed_override == pytest.approx(mph(55)) + step(controller, dashboard=mph(45), cruise=mph(55), ego=mph(60), gas=True) + assert controller.pedal_override == pytest.approx(mph(60)) + + +@pytest.mark.parametrize("display_only", [False, True]) +def test_inactive_mode_clears_override_and_reseeds_set_speed(controller_factory, display_only): + controller = controller_factory() + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(60)) + assert controller.set_speed_override > 0 + step(controller, dashboard=mph(45), cruise=mph(70), active=False, display_only=display_only) + assert controller.set_speed_override == 0 + assert controller.previous_set_speed is None + step(controller, dashboard=mph(45), cruise=mph(70)) + assert controller.set_speed_override == 0 + assert controller.previous_set_speed == pytest.approx(mph(70)) + + +def test_inactive_mode_consumes_stale_one_shots(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45)) + memory = controller.starpilot_planner.params_memory + memory.values["SpeedLimitAccepted"] = True + memory.values["SLCAdoptSpeedLimit"] = True + step(controller, active=False) + assert not controller.confirmation_pending + assert "SpeedLimitAccepted" not in memory.values + assert "SLCAdoptSpeedLimit" not in memory.values + assert controller.last_valid_limit == pytest.approx(mph(55)) + + +def test_brief_disengage_keeps_override_then_sustained_disengage_clears_it(controller_factory): + controller = controller_factory() + step(controller, dashboard=mph(45), cruise=mph(45)) + step(controller, dashboard=mph(45), cruise=mph(60)) + for _ in range(int(0.5 / DT_MDL)): + step(controller, dashboard=mph(45), cruise=mph(60), enabled=False) + assert controller.set_speed_override == pytest.approx(mph(60)) + for _ in range(int(0.5 / DT_MDL) + 1): + step(controller, dashboard=mph(45), cruise=mph(60), enabled=False) + assert controller.set_speed_override == 0 + + +def test_disengaged_confirmation_auto_accepts_without_timing_out(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45), enabled=False, long_active=False) + assert controller.target == pytest.approx(mph(45)) + assert not controller.confirmation_pending + + +def test_fully_disengaged_auto_accept_takes_precedence_over_decel(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45), enabled=False, long_active=False, decel=True) + assert controller.target == pytest.approx(mph(45)) + assert controller.last_valid_limit == pytest.approx(mph(45)) + assert controller.denied_limit == 0 + + +def test_presented_source_tracks_pending_candidate_and_rejected_accepted_limit(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + assert controller.presented_source == SOURCE_DASHBOARD + + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current) + assert controller.confirmation_pending + assert controller.source == SOURCE_NONE + assert controller.presented_source == SOURCE_MAP + + step(controller, map_limit=mph(45), way=custom.WaySelectionType.current, decel=True) + assert not controller.confirmation_pending + assert controller.presented_source == SOURCE_DASHBOARD + + step(controller) + assert controller.presented_source == SOURCE_NONE + + +def test_explicit_accept_takes_precedence_over_simultaneous_reject(controller_factory): + controller = controller_factory(speed_limit_confirmation_lower=True) + step(controller, dashboard=mph(55)) + step(controller, dashboard=mph(45)) + step(controller, dashboard=mph(45), accel=True, decel=True) + assert controller.target == pytest.approx(mph(45)) + assert controller.denied_limit == 0 + + +def test_offset_bucket_boundary(controller_factory): + controller = controller_factory(speed_limit_offset1=1.0, speed_limit_offset2=2.0) + assert controller.get_offset(11.2) == 2.0 + + +def test_mapbox_reset_discards_obsolete_future(): + helper = MapboxSpeedLimit(FakeParams({"MapboxSecretKey": "test"})) try: - controller.source = "Dashboard" - controller.target = 30 * CV.KPH_TO_MS - controller.last_valid_limit = controller.target + old = Future() + old.set_running_or_notify_cancel() + helper.future = old + helper.reset() + old.set_result((mph(45), 100.0)) + helper.update(datetime.now(UTC), False, 0.0, True, {}, 0.0, 0.0) + assert helper.limit == 0 - 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) - - 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 - assert controller.overridden_speed == 0 + newer = Future() + newer.set_result((mph(55), 100.0)) + helper.future = newer + helper.update(datetime.now(UTC), False, mph(40), True, {}, 0.0, 0.0) + assert helper.limit == pytest.approx(mph(55)) + helper.requests["total_requests"] = helper.requests["max_requests"] + helper.update(datetime.now(UTC), False, mph(40), True, {}, 0.0, 0.0) + assert helper.limit == 0 finally: - controller.shutdown() + helper.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() +def test_mapbox_ping_failure_keeps_previous_retry_distance(monkeypatch): + import openpilot.starpilot.controls.lib.mapbox_speed_limit as mapbox_module + + helper = MapboxSpeedLimit(FakeParams({"MapboxSecretKey": "test"})) 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 - - # A 1 mph lower zone takes the same-limit fast path but still clears the override. - controller.update_limits(mph(44), 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(44)) - # Set speed unchanged at 60 -> no rising edge -> override stays cleared (car slows to 44). - 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(44), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(65)) + monkeypatch.setattr(mapbox_module, "is_url_pingable", lambda _host: False) + assert helper._request({}, mph(45)) == (0.0, mph(45)) finally: - controller.shutdown() + helper.shutdown() -def test_confirmation_accel_press_does_not_arm_set_speed_override(): - controller = make_controller( - speed_limit_confirmation_higher=True, - ) +def test_mapbox_parses_first_segment_without_worker_state_mutation(monkeypatch): + import openpilot.starpilot.controls.lib.mapbox_speed_limit as mapbox_module + + helper = MapboxSpeedLimit(FakeParams({"MapboxSecretKey": "test"})) 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_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), make_sm(gas_pressed=False)) - assert controller.source == "None" - assert controller.unconfirmed_speed_limit == pytest.approx(mph(50)) - - # The button arrives before the corresponding cruise-speed update. This + accepts the - # pending 50 mph limit, but its delayed 55 mph set-speed update must not arm an override. - confirm_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=45 * CV.MPH_TO_KPH) - controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(45), mph(45), confirm_sm) - controller.update_override(mph(45), 0.0, mph(45), 0.0, confirm_sm) - assert controller.source == "Dashboard" - assert controller.target == pytest.approx(mph(50)) - assert not controller.override_slc - assert controller.overridden_speed == 0 - - delayed_speed_sm = make_sm(gas_pressed=False, v_cruise_kph=55 * CV.MPH_TO_KPH) - controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(55), mph(45), delayed_speed_sm) - controller.update_override(mph(55), 0.0, mph(45), 0.0, delayed_speed_sm) - assert not controller.override_slc - assert controller.overridden_speed == 0 - - # A second fresh + is allowed to establish the override. - second_press_sm = make_sm(gas_pressed=False, accel_pressed=True, v_cruise_kph=60 * CV.MPH_TO_KPH) - controller.update_limits(mph(50), datetime.now(timezone.utc), False, mph(60), mph(45), second_press_sm) - controller.update_override(mph(60), 0.0, mph(45), 0.0, second_press_sm) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(60)) - finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "accel_pressed", "decel_pressed"), - [ - (65, 45, False, True), - (35, 45, True, False), - ], -) -def test_disabled_confirmation_does_not_consume_wheel_input( - current_limit, desired_limit, accel_pressed, decel_pressed, -): - controller = make_controller() - try: - controller.source = "Dashboard" - controller.target = mph(current_limit) - controller.previous_source = "Dashboard" - controller.previous_target = mph(current_limit) - controller.last_valid_limit = mph(current_limit) - controller._slc_adopt_counter = 1 - controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True - - controller.handle_limit_change( - "Dashboard", mph(desired_limit), "", mph(current_limit), - make_sm( - gas_pressed=False, - accel_pressed=accel_pressed, - decel_pressed=decel_pressed, - v_cruise_kph=current_limit * CV.MPH_TO_KPH, - ), + monkeypatch.setattr(mapbox_module, "is_url_pingable", lambda _host: True) + monkeypatch.setattr(mapbox_module, "calculate_bearing_offset", lambda *_args: (1.0, 2.0)) + response = SimpleNamespace( + raise_for_status=lambda: None, + json=lambda: {"matchings": [{"legs": [{"annotation": { + "distance": [150.0], "maxspeed": [{"speed": 45, "unit": "mph"}], + }}]}]}, ) - - assert controller.target == pytest.approx(mph(desired_limit)) - assert controller.denied_target == 0 - assert controller.unconfirmed_speed_limit == 0 - assert not controller._set_speed_override_input_consumed - assert "SpeedLimitAccepted" not in controller.starpilot_planner.params_memory.values - assert "SLCForceCruiseSpeed" not in controller.starpilot_planner.params_memory.values + monkeypatch.setattr(helper.session, "get", lambda *_args, **_kwargs: response) + assert helper._request({"bearing": 0, "latitude": 0, "longitude": 0}, mph(40)) == pytest.approx((mph(45), 150.0)) + assert helper.limit == 0 finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "confirmation_toggle", "confirmation_enabled"), - [ - (65, 45, "speed_limit_confirmation_lower", True), - (35, 45, "speed_limit_confirmation_higher", True), - (65, 45, "speed_limit_confirmation_lower", False), - (35, 45, "speed_limit_confirmation_higher", False), - ], -) -def test_directional_limit_changes_follow_confirmation_mode( - current_limit, desired_limit, confirmation_toggle, confirmation_enabled, -): - controller = make_controller( - speed_limit_priority1="Dashboard", - **{confirmation_toggle: confirmation_enabled}, - ) - try: - controller.source = "Dashboard" - controller.target = mph(current_limit) - controller.previous_source = "Dashboard" - controller.previous_target = mph(current_limit) - controller.last_valid_limit = mph(current_limit) - - update_dashboard_limit(controller, datetime.now(timezone.utc), current_limit, desired_limit) - - if confirmation_enabled: - assert controller.source == "None" - assert controller.target == pytest.approx(mph(current_limit)) - assert controller.unconfirmed_speed_limit == pytest.approx(mph(desired_limit)) - else: - assert controller.source == "Dashboard" - assert controller.target == pytest.approx(mph(desired_limit)) - assert controller.unconfirmed_speed_limit == 0 - finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "confirmation_toggle", "accel_pressed", "decel_pressed", "accepted"), - [ - (65, 45, "speed_limit_confirmation_lower", True, False, True), - (65, 45, "speed_limit_confirmation_lower", False, True, False), - (35, 45, "speed_limit_confirmation_higher", True, False, True), - (35, 45, "speed_limit_confirmation_higher", False, True, False), - ], -) -def test_confirmation_wheel_actions_accept_or_decline_pending_limit( - current_limit, desired_limit, confirmation_toggle, accel_pressed, decel_pressed, accepted, -): - controller, now = make_pending_limit(current_limit, desired_limit, confirmation_toggle) - try: - update_dashboard_limit( - controller, now, current_limit, desired_limit, - accel_pressed=accel_pressed, - decel_pressed=decel_pressed, - ) - - if accepted: - assert controller.source == "Dashboard" - assert controller.target == pytest.approx(mph(desired_limit)) - assert controller._set_speed_override_input_consumed - else: - assert controller.source == "None" - assert controller.target == pytest.approx(mph(current_limit)) - assert controller.denied_target == pytest.approx(mph(desired_limit)) - - # The following planner update clears the one-frame confirmation handoff state. - update_dashboard_limit(controller, now, current_limit, desired_limit) - assert controller.unconfirmed_speed_limit == 0 - finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "confirmation_toggle", "accel_pressed", "decel_pressed"), - [ - (65, 45, "speed_limit_confirmation_lower", False, True), - (35, 45, "speed_limit_confirmation_higher", True, False), - ], -) -def test_disabling_confirmation_clears_pending_confirmation_immediately( - current_limit, desired_limit, confirmation_toggle, accel_pressed, decel_pressed, -): - controller = make_controller( - speed_limit_priority1="Dashboard", - **{confirmation_toggle: True}, - ) - try: - controller.source = "Dashboard" - controller.target = mph(current_limit) - controller.previous_source = "Dashboard" - controller.previous_target = mph(current_limit) - controller.last_valid_limit = mph(current_limit) - now = datetime.now(timezone.utc) - - update_dashboard_limit(controller, now, current_limit, desired_limit) - assert controller.unconfirmed_speed_limit == pytest.approx(mph(desired_limit)) - - setattr(controller.starpilot_toggles, confirmation_toggle, False) - - update_dashboard_limit( - controller, now, current_limit, desired_limit, - accel_pressed=accel_pressed, - decel_pressed=decel_pressed, - ) - - assert controller.target == pytest.approx(mph(desired_limit)) - assert controller.unconfirmed_speed_limit == 0 - assert controller.denied_target == 0 - assert not controller._set_speed_override_input_consumed - finally: - controller.shutdown() - - -@pytest.mark.parametrize("accepted_by_accel_button", [True, False]) -def test_higher_confirmation_raises_cruise_speed_to_target_with_offset(accepted_by_accel_button): - controller = make_controller( - speed_limit_confirmation_higher=True, - speed_limit_offset4=mph(5), - ) - try: - controller.source = "Dashboard" - controller.target = mph(35) - controller.previous_source = "Dashboard" - controller.previous_target = mph(35) - controller.last_valid_limit = mph(35) - if not accepted_by_accel_button: - controller.starpilot_planner.params_memory.values["SpeedLimitAccepted"] = True - - controller.handle_limit_change( - "Dashboard", mph(45), "", mph(40), - make_sm( - gas_pressed=False, - accel_pressed=accepted_by_accel_button, - v_cruise_kph=40 * CV.MPH_TO_KPH, - ), - ) - - assert controller.target == pytest.approx(mph(45)) - assert controller.starpilot_planner.params_memory.get_float("SLCForceCruiseSpeed") == pytest.approx(mph(50)) - finally: - controller.shutdown() - - -@pytest.mark.parametrize( - ("current_limit", "desired_limit", "set_speed", "toggle_overrides", "sm_overrides"), - [ - (45, 65, 75, {"speed_limit_confirmation_higher": True}, {"accel_pressed": True}), - (65, 45, 65, {"speed_limit_confirmation_lower": True}, {"accel_pressed": True}), - (35, 45, 40, {}, {}), - (35, 45, 40, {"speed_limit_confirmation_higher": True}, {"long_active": False, "enabled": False}), - ], -) -def test_limit_changes_that_do_not_raise_max_do_not_force_cruise_speed( - current_limit, desired_limit, set_speed, toggle_overrides, sm_overrides, -): - controller = make_controller(**toggle_overrides) - try: - controller.source = "Dashboard" - controller.target = mph(current_limit) - controller.previous_source = "Dashboard" - controller.previous_target = mph(current_limit) - controller.last_valid_limit = mph(current_limit) - - controller.handle_limit_change( - "Dashboard", mph(desired_limit), "", mph(set_speed), - make_sm( - gas_pressed=False, - v_cruise_kph=set_speed * CV.MPH_TO_KPH, - **sm_overrides, - ), - ) - - assert controller.target == pytest.approx(mph(desired_limit)) - assert "SLCForceCruiseSpeed" not in controller.starpilot_planner.params_memory.values - finally: - controller.shutdown() - - -def test_adopt_speed_limit_clears_complete_override_state(): - controller = make_controller() - try: - controller.source = "Dashboard" - controller.target = mph(45) - controller.previous_source = "Dashboard" - controller.previous_target = mph(45) - controller.last_valid_limit = mph(45) - controller.override_slc = True - controller.overridden_speed = mph(55) - controller._slc_adopt_counter = 3 - controller.starpilot_planner.params_memory.values["SLCAdoptSpeedLimit"] = True - - controller.update_limits(mph(45), datetime.now(timezone.utc), False, mph(55), mph(50), make_sm(gas_pressed=False)) - - assert controller.overridden_speed == 0 - assert not controller.override_slc - finally: - controller.shutdown() - - -def test_redneck_set_speed_override_is_bidirectional(): - controller = make_controller( - redneck_cruise=True, - ) - try: - controller.source = "Dashboard" - controller.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 - assert controller.overridden_speed == pytest.approx(mph(60)) - - # A manual decrease below the posted limit must become the new redneck target. - controller.update_override(mph(35), 0.0, mph(45), 0.0, make_sm(gas_pressed=False)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(35)) - finally: - controller.shutdown() - - -def test_manual_override_tracks_current_speed_and_ends_on_release(): - controller = make_controller() - 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(55), 0.0, make_sm(gas_pressed=True)) - assert controller.override_slc - assert controller.overridden_speed == pytest.approx(mph(55)) - - # The temporary override follows the current speed rather than a historical peak. - controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=True)) - assert controller.overridden_speed == pytest.approx(mph(50)) - - controller.update_override(mph(60), 0.0, mph(50), 0.0, make_sm(gas_pressed=False)) - assert not controller.override_slc - assert controller.overridden_speed == 0 - finally: - controller.shutdown() - - -def test_manual_override_survives_brief_enabled_flicker(): - controller = make_controller() - 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(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True)) - - disabled_sm = make_sm(gas_pressed=True, enabled=False) - for _ in range(int(0.5 / DT_MDL)): - controller.update_override(mph(60), 0.0, mph(55), 0.0, disabled_sm) - - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - - controller.update_override(mph(60), 0.0, mph(55), 0.0, make_sm(gas_pressed=True, enabled=True)) - - assert controller.overridden_speed == pytest.approx(mph(55)) - assert controller.override_slc - finally: - controller.shutdown() - - -def test_override_clears_after_sustained_disengage(): - controller = make_controller() - try: - controller.source = "Dashboard" - controller.target = mph(45) - controller.previous_source = "Dashboard" - controller.previous_target = mph(45) - controller.last_valid_limit = mph(45) - controller.overridden_speed = mph(55) - controller.override_slc = True - - disabled_sm = make_sm(gas_pressed=False, enabled=False) - for _ in range(int(1.0 / DT_MDL) + 1): - controller.update_override(mph(75), 0.0, mph(65), 0.0, disabled_sm) - - assert controller.overridden_speed == 0 - assert not controller.override_slc - finally: - controller.shutdown() + helper.shutdown() diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 93ba5e68c9..8e1af1463f 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -64,6 +64,7 @@ def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=Fal "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill, vEgoCluster=v_ego_cluster), "carControl": SimpleNamespace(orientationNED=[0.0, pitch, 0.0]), "controlsState": SimpleNamespace(forceDecel=force_decel), + "selfdriveState": SimpleNamespace(personality=1), "radarState": SimpleNamespace( leadOne=lead_one or make_lead(), leadTwo=lead_two or make_lead(), diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index 3e69d620a2..fd76ff5169 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -2,6 +2,7 @@ import datetime import pytest +from cereal import custom from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME @@ -36,15 +37,26 @@ class FakeParams: def get_float(self, *args, **kwargs): return 0.0 + def get_bool(self, key): + return bool(self.values.get(key, False)) + + def remove(self, key): + self.values.pop(key, None) + def put_nonblocking(self, key, value): self.values[key] = value self.writes.append((key, value)) + def put_float(self, key, value): + self.values[key] = value + def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None, road_curvature=0.0): planner = SimpleNamespace( params=FakeParams(), params_memory=FakeParams({"NavInstructionState": nav_state or {}}), + gps_position={}, + gps_valid=False, lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0), starpilot_cem=SimpleNamespace(stop_light_detected=red_light), starpilot_following=SimpleNamespace(following_lead=False), @@ -77,9 +89,14 @@ def make_sm(*, standstill=True, min_steer_speed=0.0, car_fingerprint=""): leftBlinker=False, rightBlinker=False, steeringAngleDeg=0.0, + vCruise=72.0, ), + "liveParameters": SimpleNamespace(angleOffsetDeg=0.0), + "mapdOut": SimpleNamespace(nextSpeedLimitDistance=0.0, nextSpeedLimit=0.0, speedLimit=0.0, + waySelectionType=custom.WaySelectionType.fail, roadName=""), + "selfdriveState": SimpleNamespace(enabled=True), "carParams": SimpleNamespace(minSteerSpeed=min_steer_speed, carFingerprint=car_fingerprint), - "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), + "starpilotCarState": SimpleNamespace(accelPressed=False, decelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0), "onroadEvents": [], } @@ -105,6 +122,27 @@ def make_toggles(): nav_longitudinal_allowed=False, speed_limit_controller=False, show_speed_limits=False, + 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_fallback_experimental_mode=False, + slc_mapbox_filler=False, + speed_limit_confirmation_higher=False, + speed_limit_confirmation_lower=False, + speed_limit_priority1="Dashboard", + speed_limit_priority2="Map Data", + speed_limit_priority_highest=False, + speed_limit_priority_lowest=False, + vision_speed_limit_detection=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, force_stop_distance_offset=0, ) @@ -112,7 +150,6 @@ def make_toggles(): def test_active_slc_control_target_does_not_require_set_speed_limit(): target = get_active_slc_control_target( speed_limit_controller=True, - set_speed_limit=False, slc_target=45.0 * CV.MPH_TO_MS, slc_offset=3.0 * CV.MPH_TO_MS, overridden_speed=0.0, @@ -145,8 +182,7 @@ def test_active_slc_target_constrains_vcruise_below_csc_minimum(slc_target_mph, vcruise.slc.target = slc_target_mph * CV.MPH_TO_MS vcruise.slc.source = "Dashboard" - vcruise.slc.update_limits = lambda *_args, **_kwargs: None - vcruise.slc.update_override = lambda *_args, **_kwargs: None + vcruise.slc.update = lambda *_args, **_kwargs: None result = update_vcruise( vcruise, @@ -158,6 +194,7 @@ def test_active_slc_target_constrains_vcruise_below_csc_minimum(slc_target_mph, ) assert result == pytest.approx(expected_v_cruise_mph * CV.MPH_TO_MS) + assert vcruise.slc_is_limiting_max_set == (expected_v_cruise_mph < 35.0) def test_elantra_gets_lead_veto_margin_before_force_stop(): @@ -417,6 +454,8 @@ def test_csc_res_press_defers_to_slc_confirmation(): sm = make_sm(standstill=False) toggles = make_toggles() toggles.curve_speed_controller = True + toggles.speed_limit_controller = True + toggles.speed_limit_confirmation_higher = True def set_curve_target(_v_ego, _v_cruise): vcruise.csc.target = 14.0 @@ -426,10 +465,12 @@ def test_csc_res_press_defers_to_slc_confirmation(): assert vcruise.csc_controlling_speed - vcruise.slc.speed_limit_changed_timer = 1.0 - vcruise.slc.unconfirmed_speed_limit = 25.0 + # The candidate and accel press arrive together. SLC consumes the press in + # this frame, even though accepting immediately clears the pending state. + sm["starpilotCarState"].dashboardSpeedLimit = 45.0 * CV.MPH_TO_MS sm["starpilotCarState"].accelPressed = True update_vcruise(vcruise, sm, toggles, now=80.05, v_ego=20.0) + assert vcruise.slc.confirmation_button_consumed assert not vcruise.csc_override sm["starpilotCarState"].accelPressed = False @@ -622,7 +663,6 @@ def test_curve_speed_controller_hysteresis_keeps_glow_off_for_marginal_targets() def test_active_slc_control_target_applies_offset_and_cluster_diff(): target = get_active_slc_control_target( speed_limit_controller=True, - set_speed_limit=True, slc_target=45.0 * CV.MPH_TO_MS, slc_offset=3.0 * CV.MPH_TO_MS, overridden_speed=0.0, @@ -635,7 +675,6 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff(): def test_active_slc_control_target_allows_lower_redneck_override(): target = get_active_slc_control_target( speed_limit_controller=True, - set_speed_limit=False, slc_target=65.0 * CV.MPH_TO_MS, slc_offset=0.0, overridden_speed=35.0 * CV.MPH_TO_MS, diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index 70b90afab3..3a2e0a3f8c 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -628,7 +628,7 @@ class StarPilotLongitudinalLayout(_SettingsPage): set_state=lambda s: self._params.put_bool("SLCMapboxFiller", s), visible=self._mapbox_available), SettingRow("ShowSLCOffset", "toggle", tr_noop("Show SLC Offset"), - subtitle="", + subtitle=tr_noop("Compact display only; the unified card always shows nonzero offsets."), get_state=lambda: self._params.get_bool("ShowSLCOffset"), set_state=lambda s: self._params.put_bool("ShowSLCOffset", s)), SettingRow("SpeedLimitSources", "toggle", tr_noop("Show Sources"), diff --git a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py index 22dcfb59bc..fc035741df 100644 --- a/selfdrive/ui/onroad/starpilot/slc_speed_limit.py +++ b/selfdrive/ui/onroad/starpilot/slc_speed_limit.py @@ -1,16 +1,13 @@ import math -from typing import Optional import pyray as rl from openpilot.common.constants import CV -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS +from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr from openpilot.system.ui.lib.text_measure import measure_text_cached from openpilot.selfdrive.ui.onroad.starpilot.widget_style import ( - CONTROL_BG, CONTROL_BORDER, CONTROL_BORDER_WIDTH, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, SLC_HEIGHT, - draw_control_card, roundness_for, + CONTROL_BORDER, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, ) from openpilot.selfdrive.ui.onroad.starpilot.source_bubble_layout import ( enabled_source_titles, fit_source_label, source_abbreviated_value_text, @@ -23,14 +20,6 @@ _WHITE = rl.Color(255, 255, 255, 255) # ── Constants ───────────────────────────────────────────────────────── -# EU Vienna sign -EU_SIGN_SIZE = 176 -EU_SIGN_WIDTH = 176 -RED_RING_WIDTH = 20 - -# Pending sign blink cadence — 1s period, 50% duty cycle. -PENDING_BLINK_MS = 500 - # Source display metadata: source name, main label, value key, bubble label, icon. SOURCE_DEFS = [ ("Dashboard", "Dash", "dashboard_sl", "Dashboard", "dashboard"), @@ -39,16 +28,12 @@ SOURCE_DEFS = [ ("Mapbox", "MBOX", "mapbox_sl", "Mapbox", "map"), ("Upcoming", "NEXT", "next_sl", "Next", "next"), ] +_SOURCE_ICON_KEYS = {source: icon for source, _, _, _, icon in SOURCE_DEFS} -# Fonts -FONT_LABEL = 30 -FONT_SOURCE = 40 # Set Speed MAX label size. -FONT_SPEED = 90 # Set Speed value size. -FONT_OFFSET = 29 # Compact offset text. -OFFSET_CHIP_SEGMENTS = 8 # Capsule curve segments. -FONT_EU_LARGE = 70 -FONT_EU_SMALL = 60 -FONT_EU_OFFSET = 40 + +def source_icon_key(source: str) -> str | None: + """Use the same source glyph as the detailed source diagnostics.""" + return _SOURCE_ICON_KEYS.get(source) # Vision speed-limit pulse — one-shot purple highlight when the active source # is "Vision" and the resolved value just changed. @@ -90,8 +75,21 @@ def _speed_limit_pulse_color(base: rl.Color, alpha: int) -> rl.Color: # ── State ───────────────────────────────────────────────────────────── +def _is_slc_enabled() -> bool: + toggles = getattr(ui_state, "starpilot_toggles", {}) + if "speed_limit_controller" in toggles: + return bool(toggles["speed_limit_controller"]) + return ui_state.ui_params.get_bool("SpeedLimitController") + + def _get_slc_state(): """Extract SLC state from SubMaster. Returns dict or None if stale/hidden.""" + slc_enabled = _is_slc_enabled() + params = ui_state.ui_params + if not (slc_enabled or params.get_bool("ShowSpeedLimits")): + _pulse.clear() + return None + sm = ui_state.sm if sm.recv_frame["starpilotPlan"] < ui_state.started_frame: _pulse.clear() @@ -99,18 +97,11 @@ def _get_slc_state(): plan = sm["starpilotPlan"] speed_limit_changed = plan.speedLimitChanged + presented_source = getattr(plan, 'slcPresentedSpeedLimitSource', '') - params = ui_state.ui_params - show_slc = params.get_bool("ShowSpeedLimits") unconfirmed_valid = plan.unconfirmedSlcSpeedLimit > 1 - if not show_slc: - _pulse.clear() - return None - speed_conversion = CV.MS_TO_KPH if ui_state.is_metric else CV.MS_TO_MPH - show_offset = params.get_bool("ShowSLCOffset") - dashboard_sl = sm["starpilotCarState"].dashboardSpeedLimit if sm.valid.get("starpilotCarState", False) else 0.0 vision_enabled = params.get_bool("VisionSpeedLimitDetection") vision_sl = ui_state.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0.0 @@ -120,41 +111,24 @@ def _get_slc_state(): params.get("MapboxSecretKey", encoding="utf-8") ) - slc_overridden_speed = plan.slcOverriddenSpeed - # Keep the source limit visible when overridden. - speed_limit = plan.slcSpeedLimit - - # Resolved limit in m/s (pre-conversion, pre-offset) — feeds the vision pulse - # change detector so the comparison is unit-stable across km/h ↔ mph flips. - resolved_ms = speed_limit - - # Add the per-limit offset to the displayed value only when NOT overridden - # AND ShowSLCOffset is off (when the offset toggle is on, it's rendered as - # a separate field below the speed number instead). - if slc_overridden_speed == 0 and not show_offset: - speed_limit += plan.slcSpeedLimitOffset - speed_limit *= speed_conversion - - speed_limit_offset = plan.slcSpeedLimitOffset * speed_conversion - offset_str = f"{'+' if speed_limit_offset > 0 else '-'}{abs(int(round(speed_limit_offset)))}" if speed_limit_offset != 0 else "\u2013" - - # Update the vision-source pulse once per frame, after resolved_ms is known - # and before any sign colors are computed downstream. - _tick_pulse(plan.slcSpeedLimitSource, resolved_ms) - + # The pulse uses the accepted raw limit, so unit changes cannot retrigger it. + _tick_pulse(plan.slcSpeedLimitSource, plan.slcSpeedLimit) return { - 'speed_limit': speed_limit, - 'speed_limit_str': "\u2013" if speed_limit <= 1 else str(int(round(speed_limit))), - 'slc_overridden_speed': slc_overridden_speed, + 'accepted_speed_limit_ms': plan.slcSpeedLimit, + # Match the control target's non-negative base before cluster compensation. + 'effective_target_ms': max(0.0, plan.slcSpeedLimit + plan.slcSpeedLimitOffset), + 'offset_ms': plan.slcSpeedLimitOffset, + 'slc_overridden_speed': plan.slcOverriddenSpeed, 'speed_limit_source': plan.slcSpeedLimitSource, + # Older publishers/replays decode the new Text field as "", rather than omitting the attribute. + 'presented_source': presented_source or plan.slcSpeedLimitSource, + 'slc_enabled': slc_enabled, + # Both UI fields were added together; older plans have no published limiting state. + 'slc_is_limiting_max_set': bool(getattr(plan, 'slcIsLimitingMaxSet', False)) if presented_source else None, 'unconfirmed_speed_limit': max(0.0, plan.unconfirmedSlcSpeedLimit * speed_conversion), 'unconfirmed_valid': unconfirmed_valid, 'speed_limit_changed': speed_limit_changed, - 'show_offset': show_offset, - 'use_vienna': params.get_bool("UseVienna"), - 'offset_str': offset_str, 'speed_conversion': speed_conversion, - 'speed_unit': " km/h" if ui_state.is_metric else " mph", 'slc_abbreviated_sources': params.get_bool("SLCAbbreviatedSources"), 'slc_active_sources_only': params.get_bool("SLCActiveSourcesOnly"), 'slc_enabled_sources': enabled_source_titles( @@ -191,203 +165,6 @@ def _get_semi_bold(): return _font_semi_bold -_ACTIVE_SOURCE_LABELS = {title: abbrev.upper() for title, abbrev, *_ in SOURCE_DEFS} - - -def _active_source_label(state: dict) -> str: - source = state.get("speed_limit_source") - if not source or source == "None": - return tr("LIMIT") - return _ACTIVE_SOURCE_LABELS.get(source, source.upper()) - - -def _source_label_color(alpha: int, is_overridden: bool = False) -> rl.Color: - """Match Set Speed's MAX label color.""" - if is_overridden or ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE): - base = COLORS.DISENGAGED - elif ui_state.status == UIStatus.ENGAGED: - base = COLORS.ENGAGED - else: - base = COLORS.GREY - return _speed_limit_pulse_color(base, alpha) - - -# ── US MUTCD Sign ───────────────────────────────────────────────────── - -def _draw_offset_chip(rect: rl.Rectangle, offset_str: str, color: rl.Color) -> None: - """Draw the optional SLC offset as a compact accent chip.""" - font = _get_semi_bold() - text_size = measure_text_cached(font, offset_str, FONT_OFFSET) - chip_w = max(64.0, text_size.x + 24.0) - chip_h = 36.0 - chip_rect = rl.Rectangle( - rect.x + (rect.width - chip_w) / 2, - rect.y + rect.height - chip_h - 10, - chip_w, - chip_h, - ) - chip_fill = rl.Color(0, 0, 0, min(120, color.a)) - roundness = roundness_for(chip_rect, 18) - rl.draw_rectangle_rounded(chip_rect, roundness, OFFSET_CHIP_SEGMENTS, chip_fill) - rl.draw_rectangle_rounded_lines_ex(chip_rect, roundness, OFFSET_CHIP_SEGMENTS, 2, color) - rl.draw_text_ex( - font, - offset_str, - rl.Vector2(chip_rect.x + (chip_w - text_size.x) / 2, chip_rect.y + (chip_h - text_size.y) / 2), - FONT_OFFSET, - 0, - color, - ) - - -def _draw_us_sign(x: float, y: float, sign_width: float, sign_height: float, - speed_text: str, offset_str: str, - source_label: str, alpha: int, show_offset: bool, *, - pending: bool = False, is_overridden: bool = False): - """Draw the NA control card at (x, y). - - The card keeps the SLC's label/value hierarchy while sharing the exact - visible frame geometry with Set Speed. Border and text colors continue to - use the existing Vision pulse and pending blink behavior. - """ - # Pending: blink white/red. Active: shared blue-grey. - if pending: - blink_on = int(rl.get_time() * 1000) % 1000 < PENDING_BLINK_MS - base_border = rl.Color(255, 255, 255, alpha) if blink_on else rl.Color(201, 34, 49, alpha) - else: - base_border = rl.Color(CONTROL_BORDER.r, CONTROL_BORDER.g, CONTROL_BORDER.b, - min(alpha, CONTROL_BORDER.a)) - - # Compose the blink base with the active vision pulse (no-op outside window). - border_color = _speed_limit_pulse_color(base_border, base_border.a) - # White value text reads on the translucent road background. - text_color = _speed_limit_pulse_color(rl.Color(255, 255, 255, 255), alpha) - - card_rect = rl.Rectangle(x, y, sign_width, sign_height) - card_fill = rl.Color(CONTROL_BG.r, CONTROL_BG.g, CONTROL_BG.b, min(CONTROL_BG.a, alpha)) - draw_control_card(card_rect, fill=card_fill, border=border_color, - border_width=CONTROL_BORDER_WIDTH) - - font_bold = _get_bold() - font_semi = _get_semi_bold() - cx = x + sign_width / 2 - - # Pending layout: "PENDING" + "LIMIT" + speed (no offset shown when pending). - if pending: - pending_size = measure_text_cached(font_semi, tr("PENDING"), FONT_LABEL - 2) - rl.draw_text_ex(font_semi, tr("PENDING"), rl.Vector2(cx - pending_size.x / 2, y + 20), FONT_LABEL - 2, 0, text_color) - limit_size = measure_text_cached(font_semi, tr("LIMIT"), FONT_LABEL) - rl.draw_text_ex(font_semi, tr("LIMIT"), rl.Vector2(cx - limit_size.x / 2, y + 48), FONT_LABEL, 0, text_color) - speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED - 6) - rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 85), FONT_SPEED - 6, 0, text_color) - elif show_offset: - # Offset ON: source at the top, speed below it, and the offset in a chip. - source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE) - source_color = _source_label_color(alpha, is_overridden=is_overridden) - rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 8), FONT_SOURCE, 0, source_color) - - speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED) - rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 44), FONT_SPEED, 0, text_color) - _draw_offset_chip(card_rect, offset_str, text_color) - else: - # Offset OFF: match Set Speed typography. - source_size = measure_text_cached(font_semi, source_label, FONT_SOURCE) - source_color = _source_label_color(alpha, is_overridden=is_overridden) - rl.draw_text_ex(font_semi, source_label, rl.Vector2(cx - source_size.x / 2, y + 27), FONT_SOURCE, 0, source_color) - - speed_size = measure_text_cached(font_bold, speed_text, FONT_SPEED) - rl.draw_text_ex(font_bold, speed_text, rl.Vector2(cx - speed_size.x / 2, y + 77), FONT_SPEED, 0, text_color) - - -# ── EU Vienna Sign ──────────────────────────────────────────────────── - -def _draw_eu_sign(x: float, y: float, speed_text: str, offset_str: str, - source_label: str, text_alpha: int, show_offset: bool, *, pending: bool = False): - """Draw EU-style (Vienna) speed limit sign at (x, y). - - White disk with a pulsable red ring and pulsable black text. The pre-existing - pending-text blink (black <-> red) composes with the vision pulse: outside the - pulse window the blink is unchanged, inside it both colors are eased toward - VISION_SPEED_LIMIT_PULSE_COLOR. - """ - center_x = x + EU_SIGN_SIZE / 2 - center_y = y + EU_SIGN_SIZE / 2 - radius = EU_SIGN_SIZE / 2 - - # White disk fill. - rl.draw_circle(int(center_x), int(center_y), radius, rl.Color(255, 255, 255, text_alpha)) - # Red ring; eased toward VISION_SPEED_LIMIT_PULSE_COLOR when a Vision-sourced - # limit just changed. - ring_color = _speed_limit_pulse_color(rl.Color(201, 34, 49, 255), text_alpha) - rl.draw_ring(rl.Vector2(center_x, center_y), radius - RED_RING_WIDTH, radius, - 0, 360, 64, ring_color) - - font_bold = _get_bold() - - eu_font = FONT_EU_LARGE if len(speed_text) <= 2 else FONT_EU_SMALL - - # EU pending: text blinks black/red, composed with the vision pulse. - if pending: - blink_on = int(rl.get_time() * 1000) % 1000 < PENDING_BLINK_MS - base_text = rl.Color(0, 0, 0, 255) if blink_on else rl.Color(201, 34, 49, 255) - else: - base_text = rl.Color(0, 0, 0, 255) - text_color = _speed_limit_pulse_color(base_text, text_alpha) - - # Pending: text centered (no offset display) - if pending: - speed_size = measure_text_cached(font_bold, speed_text, eu_font) - speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2) - rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color) - elif not show_offset: - font_semi = _get_semi_bold() - source_size = measure_text_cached(font_semi, source_label, FONT_LABEL - 4) - source_pos = rl.Vector2(center_x - source_size.x / 2, y + 16) - rl.draw_text_ex(font_semi, source_label, source_pos, FONT_LABEL - 4, 0, text_color) - - speed_size = measure_text_cached(font_bold, speed_text, eu_font) - speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2) - rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color) - else: - # Offset ON: source at the top, speed below it, offset at the bottom. - font_semi = _get_semi_bold() - source_size = measure_text_cached(font_semi, source_label, FONT_LABEL - 4) - source_pos = rl.Vector2(center_x - source_size.x / 2, y + 16) - rl.draw_text_ex(font_semi, source_label, source_pos, FONT_LABEL - 4, 0, text_color) - - speed_size = measure_text_cached(font_bold, speed_text, eu_font) - speed_pos = rl.Vector2(center_x - speed_size.x / 2, center_y - speed_size.y / 2 - 5) - rl.draw_text_ex(font_bold, speed_text, speed_pos, eu_font, 0, text_color) - - offset_size = measure_text_cached(font_semi, offset_str, FONT_EU_OFFSET) - offset_pos = rl.Vector2(center_x - offset_size.x / 2, y + 122) - rl.draw_text_ex(font_semi, offset_str, offset_pos, FONT_EU_OFFSET, 0, text_color) - - -# ── Dispatcher (pending and active sign share the same rect) ───────── - -def _draw_sign(state: dict, rect: rl.Rectangle, *, pending: bool = False): - """Draw either the pending or active sign in the given rect.""" - if pending: - # Pending shows the unconfirmed value, full opacity - speed_text = ("\u2013" if state['unconfirmed_speed_limit'] <= 1 - else str(int(round(state['unconfirmed_speed_limit'])))) - else: - speed_text = state['speed_limit_str'] - - text_alpha = 255 - is_overridden = not pending and state['slc_overridden_speed'] != 0 - source_label = _active_source_label(state) - - if state['use_vienna']: - _draw_eu_sign(rect.x, rect.y, speed_text, state['offset_str'], source_label, text_alpha, - state['show_offset'], pending=pending) - else: - _draw_us_sign(rect.x, rect.y, rect.width, rect.height, speed_text, state['offset_str'], - source_label, text_alpha, state['show_offset'], pending=pending, - is_overridden=is_overridden) - - # ── Sources Bubble (expandable overlay) ──────────────────────────────── # Fixed outer footprint; the content scale adapts to the visible row count. @@ -417,7 +194,7 @@ _SOURCE_COMPACT_LABELS = { def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl.Color) -> None: - """Draw the small, intentionally simple source glyphs used by the panel.""" + """Draw the existing source glyph for both the header and diagnostics.""" cx = x + size / 2 cy = y + size / 2 stroke = max(2.5, size / 12.0) @@ -480,11 +257,20 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl. color, ) rl.draw_circle_v(pin_center, size * 0.09, _SOURCE_PANEL_BG) - else: # Dashboard / fallback - dashboard_scale = 1.22 + elif icon_key == "dashboard": + # The Dashboard speed-limit source is a vehicle glyph, distinct from Max Set's gauge. + body = rl.Rectangle(x + size * 0.10, y + size * 0.43, size * 0.80, size * 0.29) + rl.draw_rectangle_rounded_lines_ex(body, 0.30, 8, stroke, color) + rl.draw_line_ex(rl.Vector2(x + size * 0.25, body.y), rl.Vector2(x + size * 0.36, y + size * 0.27), stroke, color) + rl.draw_line_ex(rl.Vector2(x + size * 0.36, y + size * 0.27), rl.Vector2(x + size * 0.68, y + size * 0.27), stroke, color) + rl.draw_line_ex(rl.Vector2(x + size * 0.68, y + size * 0.27), rl.Vector2(x + size * 0.79, body.y), stroke, color) + for wheel_x in (x + size * 0.27, x + size * 0.73): + rl.draw_circle_v(rl.Vector2(wheel_x, y + size * 0.75), size * 0.07, color) + elif icon_key == "speedometer": + gauge_scale = 1.22 pivot = rl.Vector2(cx, cy + size * 0.17) - inner_radius = size * 0.27 * dashboard_scale - outer_radius = size * 0.34 * dashboard_scale + inner_radius = size * 0.27 * gauge_scale + outer_radius = size * 0.34 * gauge_scale ring_segments = max(24, int(size * 0.25)) rl.draw_ring(pivot, inner_radius, outer_radius, 190, 350, ring_segments, color) cap_radius = (outer_radius - inner_radius) / 2 @@ -509,7 +295,7 @@ def _draw_source_icon(icon_key: str, x: float, y: float, size: float, color: rl. stroke, color, ) - rl.draw_circle_v(pivot, max(2.0, size * 0.06 * dashboard_scale), color) + rl.draw_circle_v(pivot, max(2.0, size * 0.06 * gauge_scale), color) def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None: @@ -523,7 +309,7 @@ def _draw_sources_bubble_empty_state(panel_rect: rl.Rectangle) -> None: total_h = sum(sz.y for sz in line_sizes) + line_gap * (len(lines) - 1) curr_y = round(panel_rect.y + (panel_rect.height - total_h) / 2) - for line, sz in zip(lines, line_sizes): + for line, sz in zip(lines, line_sizes, strict=True): pos_x = round(panel_rect.x + (panel_rect.width - sz.x) / 2) rl.draw_text_ex(font, line, rl.Vector2(pos_x, curr_y), font_size, 0, _WHITE) curr_y += round(sz.y + line_gap) @@ -608,7 +394,7 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle): f"{tr(compact_label)}-{source_abbreviated_value_text(value)}", "", content_right - label_left, - lambda text: measure_text_cached(text_font, text, font_size).x, + lambda text, font=text_font: measure_text_cached(font, text, font_size).x, ) label_size = measure_text_cached(text_font, label_text, font_size) text_y = round(row_y + (row_h - label_size.y) / 2) @@ -647,24 +433,3 @@ def _draw_sources_bubble(state: dict, sign_rect: rl.Rectangle): value_pos = rl.Vector2(round(content_right - value_size.x), text_y) rl.draw_text_ex(font_semi, label_text, label_pos, font_size, 0, text_color) rl.draw_text_ex(font_bold, value_text, value_pos, font_size, 0, text_color) - - -# ── Public API ──────────────────────────────────────────────────────── - -def render_speed_limit_at(state: dict, rect: rl.Rectangle, expanded: bool = False) -> Optional[rl.Rectangle]: - """Render the SLC sign and optional source bubble at a layout rect.""" - flashing_pending = state['speed_limit_changed'] and state['unconfirmed_valid'] - - if flashing_pending: - _draw_sign(state, rect, pending=True) - return None - - _draw_sign(state, rect, pending=False) - - use_vienna = state['use_vienna'] - visual_rect = rl.Rectangle(rect.x, rect.y, EU_SIGN_SIZE, EU_SIGN_SIZE) if use_vienna else rect - - if expanded: - _draw_sources_bubble(state, visual_rect) - - return visual_rect diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 688e0c2d1d..6af0edff30 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar from openpilot.selfdrive.ui.onroad.starpilot.rivian_lateral_mode import rivian_lateral_mode from openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager import WidgetLayoutManager from openpilot.selfdrive.ui.onroad.starpilot.widgets import ( - SetSpeedWidget, SpeedLimitWidget, PedalIconsWidget, + UnifiedSpeedWidget, PedalIconsWidget, AetherGaugeWidget, PersonalityButtonWidget, DriverMonitorWidget, SteeringWheelWidget, StoppedTimerWidget, ModelSourceWidget ) @@ -25,7 +25,6 @@ from openpilot.starpilot.common.favorite_slots import ( build_favorite_slot_options, filter_favorite_slot_options, favorite_key_is_valid, - is_bool_param, ) from openpilot.system.ui.lib.application import MousePos, gui_app, FontWeight @@ -64,8 +63,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._hud_renderer.draw_exp_button = False # Initialize layout widgets - self._set_speed_widget = SetSpeedWidget(self._hud_renderer) - self._speed_limit_widget = SpeedLimitWidget() + self._unified_speed_widget = UnifiedSpeedWidget(self._hud_renderer) self._aethergauge_widget = AetherGaugeWidget(self._hud_renderer) self._steering_wheel_widget = SteeringWheelWidget(self._hud_renderer._exp_button) self._pedals_widget = PedalIconsWidget() @@ -75,8 +73,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._stopped_timer_widget = StoppedTimerWidget(self.is_in_reverse) # Register to layout zones - self.layout_manager.register_widget("left", self._set_speed_widget) - self.layout_manager.register_widget("left", self._speed_limit_widget) + self.layout_manager.register_widget("left", self._unified_speed_widget) self.layout_manager.register_widget("left", self._aethergauge_widget) self.layout_manager.register_widget("right", self._steering_wheel_widget) self.layout_manager.register_widget("right", self._pedals_widget) @@ -85,8 +82,7 @@ class StarPilotOnroadView(AugmentedRoadView): self.layout_manager.register_widget("bottom", self._driver_monitor_widget) # Register as child widgets for click propagation - self._child(self._set_speed_widget) - self._child(self._speed_limit_widget) + self._child(self._unified_speed_widget) self._child(self._aethergauge_widget) self._child(self._steering_wheel_widget) self._child(self._pedals_widget) @@ -133,7 +129,7 @@ class StarPilotOnroadView(AugmentedRoadView): if self._draw_hud_controls: dm = self.driver_state_renderer self.layout_manager.update_layout(self._content_rect, is_rhd=dm.is_rhd if dm else False) - self._render_slc() + self._render_speed_card() self._render_overlays() self._render_road_name() @@ -167,13 +163,11 @@ class StarPilotOnroadView(AugmentedRoadView): render_background_effects(rect, border_width) render_overlay(border_rect, border_width) - def _render_slc(self): + def _render_speed_card(self): if self._full_alert_showing(): return - if self._speed_limit_widget.is_visible: - self._speed_limit_widget.render(self._speed_limit_widget.rect) - if self._set_speed_widget.is_visible: - self._set_speed_widget.render(self._set_speed_widget.rect) + if self._unified_speed_widget.is_visible: + self._unified_speed_widget.render(self._unified_speed_widget.rect) def _render_overlays(self): alert_showing, _ = self.alert_renderer.will_render() @@ -186,7 +180,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._render_developer_metrics() - self.layout_manager.render_widgets(exclude={"speed_limit", "set_speed"}) + self.layout_manager.render_widgets(exclude={"unified_speed"}) self._render_torque_bar() self._render_bottom_row_widgets() diff --git a/selfdrive/ui/onroad/starpilot/unified_speed_presentation.py b/selfdrive/ui/onroad/starpilot/unified_speed_presentation.py new file mode 100644 index 0000000000..8eab3336a1 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/unified_speed_presentation.py @@ -0,0 +1,60 @@ +"""Displayed Max Set and posted-limit values for the Big UI speed card.""" + +from dataclasses import dataclass + + +@dataclass(frozen=True) +class UnifiedSpeedPresentation: + mode: str + max_speed_text: str + posted_speed_text: str + effective_speed_text: str + offset_text: str | None + unit_text: str + source: str + confirmation_pending: bool + active_side: str + + +def resolve_unified_speed(show_max: bool, cruise_set: bool, max_speed: float, + slc_state: dict | None, slc_enabled: bool, is_metric: bool) -> UnifiedSpeedPresentation: + """Compare the rounded values the driver sees; ignore override speed for layout.""" + unit = "km/h" if is_metric else "mph" + max_text = str(round(max_speed)) if cruise_set else "–" + posted_text = effective_text = "–" + offset_text = None + source = "None" + pending = has_limit = slc_is_limiting = False + if slc_state is not None: + conversion = slc_state['speed_conversion'] + accepted = slc_state['accepted_speed_limit_ms'] + pending = bool(slc_state['speed_limit_changed'] and slc_state['unconfirmed_valid']) + source = slc_state['presented_source'] + has_limit = (source not in ("", "None") and accepted > 1) or pending + if has_limit: + posted_text = str(round(slc_state['unconfirmed_speed_limit'])) if pending else str(round(accepted * conversion)) + effective = slc_state['effective_target_ms'] + slc_is_limiting = slc_state['slc_is_limiting_max_set'] + if slc_is_limiting is None: + slc_is_limiting = cruise_set and accepted > 1 and 0 < effective * conversion < max_speed + effective_text = str(round(effective * conversion)) if effective > 0 else "–" + offset_display = round(slc_state['offset_ms'] * conversion) + offset_text = f"{offset_display:+d}" if offset_display else None + else: + source = "None" + + # Max-only is valid only when SLC is disabled. + if pending: + mode = "split" + elif slc_enabled: + mode = "merged" if show_max and cruise_set and has_limit and max_text == effective_text else "split" if show_max else "limit_only" + elif has_limit: + mode = "split" if show_max else "limit_only" + else: + mode = "max_only" + + active_side = "none" if slc_state is not None and slc_state['slc_overridden_speed'] else "shared" if mode == "merged" else ( + "slc" if slc_enabled and cruise_set and slc_is_limiting else + "max" if (show_max or pending) and cruise_set else "none" + ) + return UnifiedSpeedPresentation(mode, max_text, posted_text, effective_text, offset_text, unit, source, pending, active_side) diff --git a/selfdrive/ui/onroad/starpilot/widget_layout_manager.py b/selfdrive/ui/onroad/starpilot/widget_layout_manager.py index 99c34aa544..14f6fec3b9 100644 --- a/selfdrive/ui/onroad/starpilot/widget_layout_manager.py +++ b/selfdrive/ui/onroad/starpilot/widget_layout_manager.py @@ -30,12 +30,12 @@ class WidgetLayoutManager: active_widgets = [w for w in self.zones["left"] if w.is_visible] # Left zone stacks vertically from the top-left offset - # X anchor is the shared left-control center (content x + 146). - center_x = self.content_rect.x + WIDGET_ANCHOR_OFFSET + # Keep wide cards inside the content rect without moving compact widgets. current_y = self.content_rect.y + 45 for widget in active_widgets: w, h = widget.get_size() + center_x = self.content_rect.x + max(float(WIDGET_ANCHOR_OFFSET), w / 2 + 30) widget.set_rect(rl.Rectangle(center_x - w / 2, current_y, w, h)) current_y += h + self.spacing diff --git a/selfdrive/ui/onroad/starpilot/widgets/__init__.py b/selfdrive/ui/onroad/starpilot/widgets/__init__.py index 3d77fbaba8..298178c81a 100644 --- a/selfdrive/ui/onroad/starpilot/widgets/__init__.py +++ b/selfdrive/ui/onroad/starpilot/widgets/__init__.py @@ -1,6 +1,5 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget -from openpilot.selfdrive.ui.onroad.starpilot.widgets.set_speed import SetSpeedWidget -from openpilot.selfdrive.ui.onroad.starpilot.widgets.speed_limit import SpeedLimitWidget +from openpilot.selfdrive.ui.onroad.starpilot.widgets.unified_speed import UnifiedSpeedWidget from openpilot.selfdrive.ui.onroad.starpilot.widgets.pedal_icons import PedalIconsWidget from openpilot.selfdrive.ui.onroad.starpilot.widgets.aethergauge import AetherGaugeWidget from openpilot.selfdrive.ui.onroad.starpilot.widgets.personality_button import PersonalityButtonWidget @@ -11,8 +10,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.widgets.model_source import ModelSo __all__ = [ "LayoutWidget", - "SetSpeedWidget", - "SpeedLimitWidget", + "UnifiedSpeedWidget", "PedalIconsWidget", "AetherGaugeWidget", "PersonalityButtonWidget", diff --git a/selfdrive/ui/onroad/starpilot/widgets/set_speed.py b/selfdrive/ui/onroad/starpilot/widgets/set_speed.py deleted file mode 100644 index 1a66fed870..0000000000 --- a/selfdrive/ui/onroad/starpilot/widgets/set_speed.py +++ /dev/null @@ -1,72 +0,0 @@ -import pyray as rl -from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.system.ui.lib.application import gui_app, FontWeight -from openpilot.system.ui.lib.multilang import tr -from openpilot.system.ui.lib.text_measure import measure_text_cached -from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget -from openpilot.selfdrive.ui.onroad.hud_renderer import ( - UI_CONFIG, FONT_SIZES, COLORS, CRUISE_DISABLED_CHAR -) -from openpilot.selfdrive.ui.onroad.starpilot.widget_style import draw_control_card - -class SetSpeedWidget(LayoutWidget): - def __init__(self, hud_renderer): - super().__init__("set_speed", priority=1) - self.hud_renderer = hud_renderer - self._font_semi_bold = gui_app.font(FontWeight.SEMI_BOLD) - self._font_bold = gui_app.font(FontWeight.BOLD) - - @property - def is_visible(self) -> bool: - return ( - self.hud_renderer.is_cruise_available - and not ui_state.starpilot_toggles.get("hide_max_speed", False) - ) - - def get_size(self) -> tuple[float, float]: - set_speed_width = ( - UI_CONFIG.set_speed_width_metric - if ui_state.is_metric - else UI_CONFIG.set_speed_width_imperial - ) - return float(set_speed_width), float(UI_CONFIG.set_speed_height) - - def _render(self, rect: rl.Rectangle) -> None: - draw_control_card(rect) - - max_color = COLORS.GREY - set_speed_color = COLORS.DARK_GREY - if self.hud_renderer.is_cruise_set: - set_speed_color = COLORS.WHITE - if ui_state.status == UIStatus.ENGAGED: - max_color = COLORS.ENGAGED - elif ui_state.status == UIStatus.DISENGAGED: - max_color = COLORS.DISENGAGED - elif ui_state.status == UIStatus.OVERRIDE: - max_color = COLORS.OVERRIDE - - max_text = tr("MAX") - max_text_width = measure_text_cached(self._font_semi_bold, max_text, FONT_SIZES.max_speed).x - rl.draw_text_ex( - self._font_semi_bold, - max_text, - rl.Vector2(rect.x + (rect.width - max_text_width) / 2, rect.y + 27), - FONT_SIZES.max_speed, - 0, - max_color, - ) - - set_speed_text = ( - CRUISE_DISABLED_CHAR - if not self.hud_renderer.is_cruise_set - else str(round(self.hud_renderer.set_speed)) - ) - speed_text_width = measure_text_cached(self._font_bold, set_speed_text, FONT_SIZES.set_speed).x - rl.draw_text_ex( - self._font_bold, - set_speed_text, - rl.Vector2(rect.x + (rect.width - speed_text_width) / 2, rect.y + 77), - FONT_SIZES.set_speed, - 0, - set_speed_color, - ) diff --git a/selfdrive/ui/onroad/starpilot/widgets/speed_limit.py b/selfdrive/ui/onroad/starpilot/widgets/speed_limit.py deleted file mode 100644 index 5d29f9270d..0000000000 --- a/selfdrive/ui/onroad/starpilot/widgets/speed_limit.py +++ /dev/null @@ -1,67 +0,0 @@ -import pyray as rl -from typing import Optional -from openpilot.common.params import Params -from openpilot.selfdrive.ui.ui_state import ui_state -from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget -from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( - _get_slc_state, render_speed_limit_at, EU_SIGN_SIZE, -) -from openpilot.selfdrive.ui.onroad.starpilot.widget_style import CONTROL_WIDTH, SLC_HEIGHT - - -class SpeedLimitWidget(LayoutWidget): - TOUCH_SLOP = 20 - - def __init__(self): - super().__init__("speed_limit", priority=2) - self._slc_state: dict | None = None - self._sign_rect: Optional[rl.Rectangle] = None - - @property - def _hit_rect(self) -> rl.Rectangle: - rect = self._sign_rect or self.rect - slop = self.TOUCH_SLOP - return rl.Rectangle( - rect.x - slop, - rect.y - slop, - rect.width + 2 * slop, - rect.height + 2 * slop, - ) - - @property - def is_visible(self) -> bool: - self._slc_state = _get_slc_state() - if self._slc_state is None: - self._sign_rect = None - return False - return True - - def get_size(self) -> tuple[float, float]: - if self._slc_state is None: - return 0.0, 0.0 - - use_vienna = self._slc_state['use_vienna'] - w = float(EU_SIGN_SIZE if use_vienna else CONTROL_WIDTH) - h = float(EU_SIGN_SIZE if use_vienna else SLC_HEIGHT) - - return w, h - - def _render(self, rect: rl.Rectangle) -> None: - if self._slc_state is None: - return - params = ui_state.ui_params - expanded = params.get_bool("SpeedLimitSources") - self._sign_rect = render_speed_limit_at(self._slc_state, rect, expanded) - - def _handle_mouse_press(self, mouse_pos) -> None: - state = self._slc_state - if state is None or not rl.check_collision_point_rec(mouse_pos, self._hit_rect): - return - - if state['speed_limit_changed'] and state['unconfirmed_valid']: - Params(memory=True).put_bool("SpeedLimitAccepted", True) - return - - params = ui_state.ui_params - current = params.get_bool("SpeedLimitSources") - params.put_bool("SpeedLimitSources", not current) diff --git a/selfdrive/ui/onroad/starpilot/widgets/unified_speed.py b/selfdrive/ui/onroad/starpilot/widgets/unified_speed.py new file mode 100644 index 0000000000..d130460b39 --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/widgets/unified_speed.py @@ -0,0 +1,324 @@ +"""One Big UI card for Max Set and the accepted speed limit.""" + +from __future__ import annotations + +import math + +import pyray as rl +from openpilot.common.params import Params +from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS +from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( + _draw_source_icon, _draw_sources_bubble, _get_slc_state, _is_slc_enabled, _speed_limit_pulse_color, source_icon_key, +) +from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import ( + UnifiedSpeedPresentation, resolve_unified_speed, +) +from openpilot.selfdrive.ui.onroad.starpilot.widget_style import ( + CONTROL_BG, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, draw_control_card, roundness_for, +) +from openpilot.selfdrive.ui.onroad.starpilot.widgets.base import LayoutWidget +from openpilot.system.ui.lib.application import gui_app, FontWeight +from openpilot.system.ui.lib.multilang import tr +from openpilot.system.ui.lib.text_measure import measure_text_cached + + +UNIFIED_WIDTH = 520 +UNIFIED_HEIGHT = 250 +SINGLE_WIDTH = 250 +MERGED_SEPARATOR_Y = 76 +HEADER_ICON_SIZE = 34 +HEADER_FONT_SIZE = 28 +VALUE_FONT_SIZE = 96 +UNIT_FONT_SIZE = 28 +PAUSE_ICON_WIDTH = 12 +PAUSE_ICON_HEIGHT = 14 +PAUSE_ICON_GAP = 8 +OFFSET_FONT_SIZE = 22 +OFFSET_PILL_HEIGHT = 30 +CONFIRMATION_COLOR = rl.Color(188, 132, 255, 255) +UNIFIED_ACCENT = rl.Color(160, 96, 230, 230) +OFFSET_COLOR = rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 255) + + +def _draw_header_icon(icon_key: str, x: float, y: float) -> None: + scale = max(1.0, gui_app._scale * max(gui_app._pixel_scale_x, gui_app._pixel_scale_y)) + # Supersample for smooth edges. + texture_size = math.ceil(2 * HEADER_ICON_SIZE * scale) + + def render() -> None: + rl.rl_push_matrix() + try: + texture_scale = texture_size / HEADER_ICON_SIZE + rl.rl_scalef(texture_scale, texture_scale, 1.0) + _draw_source_icon(icon_key, 0, 0, HEADER_ICON_SIZE, rl.WHITE) + finally: + rl.rl_pop_matrix() + + texture = gui_app.cached_render_texture( + f"unified-speed-header:{icon_key}:{texture_size}", texture_size, texture_size, render, + ) + if texture is None: + _draw_source_icon(icon_key, x, y, HEADER_ICON_SIZE, rl.WHITE) + return + + rl.begin_blend_mode(rl.BlendMode.BLEND_ALPHA_PREMULTIPLY) + try: + rl.draw_texture_pro( + texture, rl.Rectangle(0, 0, texture_size, -texture_size), + rl.Rectangle(x, y, HEADER_ICON_SIZE, HEADER_ICON_SIZE), rl.Vector2(0, 0), 0.0, rl.WHITE, + ) + finally: + rl.end_blend_mode() + + +class UnifiedSpeedWidget(LayoutWidget): + TOUCH_SLOP = 20 + + def __init__(self, hud_renderer): + super().__init__("unified_speed", priority=1) + self.hud_renderer = hud_renderer + self._font_semi_bold = gui_app.font(FontWeight.SEMI_BOLD) + self._font_bold = gui_app.font(FontWeight.BOLD) + self._slc_state: dict | None = None + self._slc_enabled = False + self._presentation: UnifiedSpeedPresentation | None = None + self._show_max = False + self._pedal_override = False + self._snapshot_frame: int | None = None + + def _refresh_snapshot(self) -> None: + frame = getattr(ui_state.sm, "frame", None) + if frame is not None and frame == self._snapshot_frame: + return + self._snapshot_frame = frame + self._slc_enabled = _is_slc_enabled() + self._slc_state = _get_slc_state() + self._show_max = ( + self.hud_renderer.is_cruise_available and + not ui_state.starpilot_toggles.get("hide_max_speed", False) + ) + self._pedal_override = ( + self.hud_renderer.is_cruise_set and ui_state.engaged and + ui_state.sm.valid.get("carState", False) and ui_state.sm.alive.get("carState", False) and + ui_state.sm.recv_frame["carState"] >= ui_state.started_frame and ui_state.sm["carState"].gasPressed + ) + self._presentation = resolve_unified_speed( + self._show_max, self.hud_renderer.is_cruise_set, self.hud_renderer.set_speed, + self._slc_state, self._slc_enabled, ui_state.is_metric, + ) + + @property + def is_visible(self) -> bool: + self._refresh_snapshot() + return self._show_max or self._presentation.mode != "max_only" + + def get_size(self) -> tuple[float, float]: + self._refresh_snapshot() + width = UNIFIED_WIDTH if self._presentation.mode in ("split", "merged") else SINGLE_WIDTH + return float(width), float(UNIFIED_HEIGHT) + + @property + def _hit_rect(self) -> rl.Rectangle: + rect = self.rect + return rl.Rectangle( + rect.x, rect.y - self.TOUCH_SLOP, + rect.width + self.TOUCH_SLOP, rect.height + 2 * self.TOUCH_SLOP, + ) + + def _speed_limit_bounds(self, rect: rl.Rectangle) -> rl.Rectangle | None: + mode = self._presentation.mode + if mode in ("split", "merged"): + return rl.Rectangle(rect.x + rect.width / 2, rect.y, rect.width / 2, rect.height) + if mode == "limit_only": + return rect + return None + + def _draw_centered_text(self, text: str, bounds: rl.Rectangle, y: float, + font_size: int, color: rl.Color, *, bold: bool = False) -> None: + font = self._font_bold if bold else self._font_semi_bold + text_size = measure_text_cached(font, text, font_size) + text_x = bounds.x + (bounds.width - text_size.x) / 2 + rl.draw_text_ex(font, text, rl.Vector2(text_x, y), font_size, 0, color) + + def _draw_header(self, bounds: rl.Rectangle, text: str, icon_key: str | None, label_color: rl.Color) -> None: + text = tr(text) + font_size = HEADER_FONT_SIZE + icon_width = HEADER_ICON_SIZE + 9 if icon_key else 0 + while font_size > 16 and measure_text_cached(self._font_semi_bold, text, font_size).x + icon_width > bounds.width - 24: + font_size -= 1 + text_size = measure_text_cached(self._font_semi_bold, text, font_size) + group_width = icon_width + text_size.x + group_x = bounds.x + (bounds.width - group_width) / 2 + icon_y = bounds.y + 20 + if icon_key: + _draw_header_icon(icon_key, group_x, icon_y) + rl.draw_text_ex( + self._font_semi_bold, text, + rl.Vector2(group_x + icon_width, icon_y + (HEADER_ICON_SIZE - text_size.y) / 2), + font_size, 0, label_color, + ) + + def _draw_offset_pill(self, bounds: rl.Rectangle, text: str, y: float) -> None: + text_size = measure_text_cached(self._font_semi_bold, text, OFFSET_FONT_SIZE) + width = max(56.0, text_size.x + 20.0) + pill = rl.Rectangle(bounds.x + (bounds.width - width) / 2, y, width, OFFSET_PILL_HEIGHT) + rl.draw_rectangle_rounded(pill, roundness_for(pill, 17), 8, rl.Color(32, 20, 45, 255)) + rl.draw_rectangle_rounded_lines_ex(pill, roundness_for(pill, 17), 8, 2, OFFSET_COLOR) + self._draw_centered_text(text, pill, y + (pill.height - text_size.y) / 2, OFFSET_FONT_SIZE, OFFSET_COLOR) + + def _draw_unit(self, bounds: rl.Rectangle, y: float) -> None: + text = tr(self._presentation.unit_text) + color = COLORS.WHITE_TRANSLUCENT + if self._pedal_override: + text_size = measure_text_cached(self._font_semi_bold, text, UNIT_FONT_SIZE) + text_shift = (PAUSE_ICON_WIDTH + PAUSE_ICON_GAP) / 2 + icon_x = bounds.x + (bounds.width - text_size.x) / 2 - text_shift + icon_y = y + (text_size.y - PAUSE_ICON_HEIGHT) / 2 + bar_width = PAUSE_ICON_WIDTH / 3 + for x in (icon_x, icon_x + 2 * bar_width): + rl.draw_rectangle_rec(rl.Rectangle(x, icon_y, bar_width, PAUSE_ICON_HEIGHT), OFFSET_COLOR) + bounds = rl.Rectangle(bounds.x + text_shift, bounds.y, bounds.width, bounds.height) + color = COLORS.DISENGAGED + self._draw_centered_text(text, bounds, y, UNIT_FONT_SIZE, color) + + def _max_header_color(self, active_side: str, cruise_set: bool) -> rl.Color: + if self._pedal_override: + return COLORS.DISENGAGED + if cruise_set and ui_state.status == UIStatus.ENGAGED and active_side in ("max", "shared"): + return COLORS.ENGAGED + if cruise_set and ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE): + return COLORS.DISENGAGED + return COLORS.GREY + + def _limit_header_color(self, active_side: str, overridden: bool) -> rl.Color: + if self._pedal_override or overridden or ui_state.status in (UIStatus.DISENGAGED, UIStatus.OVERRIDE): + return COLORS.DISENGAGED + if ui_state.status == UIStatus.ENGAGED and active_side in ("slc", "shared"): + return COLORS.ENGAGED + return COLORS.GREY + + def _draw_active_emphasis(self, rect: rl.Rectangle) -> None: + presentation = self._presentation + if self._pedal_override or presentation.mode == "merged" or ui_state.status != UIStatus.ENGAGED or presentation.active_side == "none": + return + if presentation.mode in ("max_only", "limit_only"): + bounds = rect + elif presentation.active_side == "slc": + bounds = self._speed_limit_bounds(rect) + elif presentation.active_side == "max": + bounds = rl.Rectangle(rect.x, rect.y, rect.width / 2, rect.height) + else: + bounds = rect + rl.draw_line_ex( + rl.Vector2(bounds.x + 18, rect.y + 65), + rl.Vector2(bounds.x + bounds.width - 18, rect.y + 65), + 3, UNIFIED_ACCENT, + ) + + def _draw_merged_separator(self, rect: rl.Rectangle) -> None: + center = rect.x + rect.width / 2 + shelf_y = rect.y + MERGED_SEPARATOR_Y + valley_y = shelf_y + 12 + color = rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 170) + rl.draw_line_ex(rl.Vector2(rect.x + 18, shelf_y), rl.Vector2(center - 34, shelf_y), 2, color) + rl.draw_spline_segment_bezier_cubic( + rl.Vector2(center - 34, shelf_y), rl.Vector2(center - 19, shelf_y), + rl.Vector2(center - 23, valley_y), rl.Vector2(center - 7, valley_y), 2, color, + ) + rl.draw_line_ex(rl.Vector2(center - 7, valley_y), rl.Vector2(center + 7, valley_y), 2, color) + rl.draw_spline_segment_bezier_cubic( + rl.Vector2(center + 7, valley_y), rl.Vector2(center + 23, valley_y), + rl.Vector2(center + 19, shelf_y), rl.Vector2(center + 34, shelf_y), 2, color, + ) + rl.draw_line_ex(rl.Vector2(center + 34, shelf_y), rl.Vector2(rect.x + rect.width - 18, shelf_y), 2, color) + + def _draw_speed_limit_border(self, rect: rl.Rectangle, right: rl.Rectangle, color: rl.Color) -> None: + # Clip the shared rounded outline so only the Speed Limit side changes. + rl.begin_scissor_mode(int(right.x), int(rect.y), int(right.width + 1), int(rect.height + 1)) + try: + rl.draw_rectangle_rounded_lines_ex(rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 3, color) + finally: + rl.end_scissor_mode() + if self._presentation.mode == "split": + rl.draw_line_ex(rl.Vector2(right.x, rect.y + 8), rl.Vector2(right.x, rect.y + rect.height - 8), 3, color) + + def _render(self, rect: rl.Rectangle) -> None: + presentation = self._presentation + state = self._slc_state + speed_color = COLORS.DISENGAGED if self._pedal_override else COLORS.WHITE + rl.draw_rectangle_rounded_lines_ex( + rect, CONTROL_ROUNDNESS, CONTROL_SEGMENTS, 7, + rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 55), + ) + draw_control_card(rect, fill=CONTROL_BG, border=UNIFIED_ACCENT, border_width=2) + if presentation.mode == "split": + divider_x = rect.x + rect.width / 2 + rl.draw_line_ex( + rl.Vector2(divider_x, rect.y + 8), rl.Vector2(divider_x, rect.y + rect.height - 8), + 2, rl.Color(UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b, 110), + ) + elif presentation.mode == "merged": + self._draw_merged_separator(rect) + + self._draw_active_emphasis(rect) + max_bounds = rl.Rectangle(rect.x, rect.y, rect.width / 2, rect.height) if presentation.mode in ("split", "merged") else rect + limit_bounds = self._speed_limit_bounds(rect) + if self._show_max or presentation.confirmation_pending: + max_color = COLORS.DARK_GREY if not self.hud_renderer.is_cruise_set else speed_color + max_label_color = self._max_header_color(presentation.active_side, self.hud_renderer.is_cruise_set) + self._draw_header(max_bounds, "MAX SET", "speedometer", max_label_color) + if presentation.mode != "merged": + self._draw_centered_text(presentation.max_speed_text, max_bounds, rect.y + 75, VALUE_FONT_SIZE, max_color, bold=True) + self._draw_unit(max_bounds, rect.y + 204) + + if limit_bounds is not None: + icon_key = source_icon_key(presentation.source) + overridden = bool(state and state['slc_overridden_speed']) + label_color = self._limit_header_color(presentation.active_side, overridden) + self._draw_header(limit_bounds, "SPEED LIMIT", icon_key, label_color) + if presentation.mode != "merged": + self._draw_centered_text(presentation.posted_speed_text, limit_bounds, rect.y + 75, VALUE_FONT_SIZE, speed_color, bold=True) + if presentation.confirmation_pending: + self._draw_centered_text(tr("PENDING"), limit_bounds, rect.y + 175, 25, CONFIRMATION_COLOR) + elif presentation.offset_text is not None: + self._draw_offset_pill(limit_bounds, presentation.offset_text, rect.y + 175) + self._draw_unit(limit_bounds, rect.y + 204) + + if presentation.mode == "merged": + self._draw_centered_text(presentation.effective_speed_text, rect, rect.y + 98, VALUE_FONT_SIZE, speed_color, bold=True) + self._draw_unit(rect, rect.y + 204) + if presentation.offset_text is not None: + self._draw_offset_pill( + limit_bounds, presentation.offset_text, rect.y + MERGED_SEPARATOR_Y - OFFSET_PILL_HEIGHT / 2, + ) + + if presentation.confirmation_pending and limit_bounds is not None: + intensity = (1.0 + math.sin(2.0 * math.pi * rl.get_time())) / 2.0 + alpha = round(100 + 155 * intensity) + pulse = rl.Color(CONFIRMATION_COLOR.r, CONFIRMATION_COLOR.g, CONFIRMATION_COLOR.b, alpha) + self._draw_speed_limit_border(rect, limit_bounds, pulse) + else: + if limit_bounds is not None and state is not None: + vision_color = _speed_limit_pulse_color(UNIFIED_ACCENT, UNIFIED_ACCENT.a) + if (vision_color.r, vision_color.g, vision_color.b) != (UNIFIED_ACCENT.r, UNIFIED_ACCENT.g, UNIFIED_ACCENT.b): + self._draw_speed_limit_border(rect, limit_bounds, vision_color) + if state is not None and ui_state.ui_params.get_bool("SpeedLimitSources"): + _draw_sources_bubble(state, rect) + + def _handle_mouse_press(self, mouse_pos) -> None: + right = self._speed_limit_bounds(self.rect) + if right is None and self._slc_state is not None: + # The detailed source panel remains dismissible when no limit is valid. + right = self.rect + if right is None: + return + target = rl.Rectangle(right.x, right.y - self.TOUCH_SLOP, + right.width + self.TOUCH_SLOP, right.height + 2 * self.TOUCH_SLOP) + if not rl.check_collision_point_rec(mouse_pos, target): + return + if self._presentation.confirmation_pending: + Params(memory=True).put_bool("SpeedLimitAccepted", True) + return + params = ui_state.ui_params + params.put_bool("SpeedLimitSources", not params.get_bool("SpeedLimitSources")) diff --git a/selfdrive/ui/tests/test_onroad_render_layers.py b/selfdrive/ui/tests/test_onroad_render_layers.py index 1b9900b641..01e215cd21 100644 --- a/selfdrive/ui/tests/test_onroad_render_layers.py +++ b/selfdrive/ui/tests/test_onroad_render_layers.py @@ -69,8 +69,7 @@ def _load_starpilot_onroad_view(monkeypatch): stub_module("openpilot.selfdrive.ui.onroad.starpilot.widget_layout_manager", WidgetLayoutManager=dummy_widget) stub_module( "openpilot.selfdrive.ui.onroad.starpilot.widgets", - SetSpeedWidget=dummy_widget, - SpeedLimitWidget=dummy_widget, + UnifiedSpeedWidget=dummy_widget, PedalIconsWidget=dummy_widget, AetherGaugeWidget=dummy_widget, PersonalityButtonWidget=dummy_widget, diff --git a/selfdrive/ui/tests/test_slc_sources_bubble.py b/selfdrive/ui/tests/test_slc_sources_bubble.py index fc129760bf..1b0def9c79 100644 --- a/selfdrive/ui/tests/test_slc_sources_bubble.py +++ b/selfdrive/ui/tests/test_slc_sources_bubble.py @@ -80,39 +80,19 @@ def test_visible_source_rows_honor_active_only_and_source_order(): ] # When no sources have a valid speed reading (> 0), returns empty list (triggers empty state) assert visible_source_rows( - source_defs, {key: 0.0 for key in values}, "Map Data", ("Map Data",), + source_defs, dict.fromkeys(values, 0.0), "Map Data", ("Map Data",), ) == [] -def test_source_label_color_override_and_engagement_states(): - from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import _source_label_color - from openpilot.selfdrive.ui.onroad.hud_renderer import COLORS - from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus +def test_header_reuses_diagnostic_source_icons_without_unknown_fallback(): + from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import source_icon_key - # Engaged and not overridden -> Active green - ui_state.status = UIStatus.ENGAGED - color = _source_label_color(255, is_overridden=False) - assert (color.r, color.g, color.b, color.a) == (COLORS.ENGAGED.r, COLORS.ENGAGED.g, COLORS.ENGAGED.b, 255) - - # Engaged but overridden -> Disengaged/override gray - color_overridden = _source_label_color(255, is_overridden=True) - assert (color_overridden.r, color_overridden.g, color_overridden.b, color_overridden.a) == ( - COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255 - ) - - # Disengaged -> Disengaged/override gray - ui_state.status = UIStatus.DISENGAGED - color_disengaged = _source_label_color(255, is_overridden=False) - assert (color_disengaged.r, color_disengaged.g, color_disengaged.b, color_disengaged.a) == ( - COLORS.DISENGAGED.r, COLORS.DISENGAGED.g, COLORS.DISENGAGED.b, 255 - ) - - # Override UI status -> Disengaged/override gray - ui_state.status = UIStatus.OVERRIDE - color_ui_override = _source_label_color(255, is_overridden=False) - assert (color_ui_override.r, color_ui_override.g, color_ui_override.b, color_ui_override.a) == ( - COLORS.OVERRIDE.r, COLORS.OVERRIDE.g, COLORS.OVERRIDE.b, 255 - ) + assert source_icon_key("Vision") == "camera" + assert source_icon_key("Dashboard") == "dashboard" + assert source_icon_key("Map Data") == "map" + assert source_icon_key("Mapbox") == "map" + assert source_icon_key("None") is None + assert source_icon_key("Unexpected") is None def test_vision_pulse_ignores_same_limit_source_flapping(monkeypatch): diff --git a/selfdrive/ui/tests/test_unified_speed_presentation.py b/selfdrive/ui/tests/test_unified_speed_presentation.py new file mode 100644 index 0000000000..b3c21ce462 --- /dev/null +++ b/selfdrive/ui/tests/test_unified_speed_presentation.py @@ -0,0 +1,186 @@ +import pytest + +from openpilot.common.constants import CV +from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import resolve_unified_speed + + +def slc_state(posted_mph=65, offset_mph=0, source="Map Data", *, pending_mph=0, + enabled=True, limiting=False, overridden=False, metric=False): + conversion = CV.MS_TO_KPH if metric else CV.MS_TO_MPH + return { + "accepted_speed_limit_ms": posted_mph / conversion, + "effective_target_ms": max(0, posted_mph + offset_mph) / conversion, + "offset_ms": offset_mph / conversion, + "speed_conversion": conversion, + "unconfirmed_speed_limit": pending_mph, + "unconfirmed_valid": pending_mph > 0, + "speed_limit_changed": pending_mph > 0, + "presented_source": source, + "slc_enabled": enabled, + "slc_is_limiting_max_set": limiting, + "slc_overridden_speed": 1.0 if overridden else 0.0, + } + + +@pytest.mark.parametrize("max_speed,posted,offset,expected_mode", [ + (80, 65, 5, "split"), + (70, 70, 0, "merged"), + (70, 65, 5, "merged"), + (70, 65, 4, "split"), + (65, 70, -5, "merged"), +]) +def test_split_and_merge_use_effective_accepted_limit(max_speed, posted, offset, expected_mode): + result = resolve_unified_speed(True, True, max_speed, slc_state(posted, offset), True, False) + assert result.mode == expected_mode + assert result.posted_speed_text == str(posted) + assert result.effective_speed_text == str(posted + offset) + assert result.offset_text == (f"{offset:+d}" if offset else None) + + +def test_pending_candidate_forces_split_without_replacing_accepted_target(): + state = slc_state(65, 5, source="Vision", pending_mph=75) + result = resolve_unified_speed(True, True, 70, state, True, False) + assert result.mode == "split" + assert result.confirmation_pending + assert result.posted_speed_text == "75" + assert result.effective_speed_text == "70" + assert result.source == "Vision" + + +def test_resolution_after_confirmation_uses_same_equality_rule(): + state = slc_state(65, 5, pending_mph=75) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split" + state["speed_limit_changed"] = state["unconfirmed_valid"] = False + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + assert resolve_unified_speed(True, True, 80, state, True, False).mode == "split" + + +def test_source_change_and_override_do_not_change_layout(): + state = slc_state(65, 5) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + state["presented_source"] = "Vision" + result = resolve_unified_speed(True, True, 70, state, True, False) + assert result.mode == "merged" + assert result.source == "Vision" + state["presented_source"] = "Dashboard" + state["slc_overridden_speed"] = 40.0 + result = resolve_unified_speed(True, True, 70, state, True, False) + assert result.mode == "merged" + assert result.source == "Dashboard" + + +def test_source_target_change_recomputes_layout_independently(): + state = slc_state(65, 5, source="Map Data") + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + state.update(slc_state(55, 5, source="Vision")) + result = resolve_unified_speed(True, True, 70, state, True, False) + assert result.mode == "split" + assert result.source == "Vision" + + +def test_active_side_uses_published_control_semantic(): + state = slc_state(65, 5, limiting=True) + assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "slc" + state["slc_is_limiting_max_set"] = False + assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "max" + state["slc_overridden_speed"] = 40.0 + assert resolve_unified_speed(True, True, 80, state, True, False).active_side == "none" + + +def test_display_only_speed_limit_stays_split(): + result = resolve_unified_speed(True, True, 70, slc_state(70, enabled=False), False, False) + assert result.mode == "split" + + +def test_disabled_confirmation_does_not_force_split(): + state = slc_state(65, 5, pending_mph=75) + state["speed_limit_changed"] = False + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + + +def test_missing_limit_never_renders_zero_or_a_stale_source(): + result = resolve_unified_speed(True, True, 70, slc_state(0, source="None"), True, False) + assert result.mode == "split" + assert result.posted_speed_text == "–" + assert result.effective_speed_text == "–" + assert result.source == "None" + + +def test_missing_limit_with_slc_disabled_allows_max_only(): + result = resolve_unified_speed(True, True, 70, slc_state(0, source="None", enabled=False), False, False) + assert result.mode == "max_only" + + +@pytest.mark.parametrize("show_max,expected_mode", [(True, "split"), (False, "limit_only")]) +def test_stale_slc_data_keeps_speed_limit_region(show_max, expected_mode): + result = resolve_unified_speed(show_max, True, 70, None, True, False) + assert result.mode == expected_mode + assert result.posted_speed_text == "–" + assert result.effective_speed_text == "–" + assert result.source == "None" + + +def test_missing_data_with_slc_disabled_uses_max_only(): + assert resolve_unified_speed(True, True, 70, None, False, False).mode == "max_only" + + +def test_persisted_previous_limit_without_source_remains_visible(): + result = resolve_unified_speed(True, True, 70, slc_state(45, source="Previous Limit"), True, False) + assert result.mode == "split" + assert result.posted_speed_text == "45" + assert result.source == "Previous Limit" + + +def test_low_limit_with_large_negative_offset_preserves_configured_offset(): + result = resolve_unified_speed(True, True, 70, slc_state(5, -99), True, False) + assert result.mode == "split" + assert result.posted_speed_text == "5" + assert result.effective_speed_text == "–" + assert result.offset_text == "-99" + + +def test_metric_and_rounding_follow_the_displayed_value(): + state = slc_state(65.4, 4.4, metric=True) + result = resolve_unified_speed(True, True, 70, state, True, True) + assert result.mode == "merged" + assert result.posted_speed_text == "65" + assert result.offset_text == "+4" + assert result.unit_text == "km/h" + + +def test_invisible_fraction_does_not_keep_card_split(): + state = slc_state(65.1, 5.2) + result = resolve_unified_speed(True, True, 70.4, state, True, False) + assert result.mode == "merged" + + +def test_hidden_max_still_shows_posted_limit(): + result = resolve_unified_speed(False, True, 70, slc_state(65), True, False) + assert result.mode == "limit_only" + result = resolve_unified_speed(False, True, 70, slc_state(65, enabled=False), False, False) + assert result.mode == "limit_only" + + +def test_confirmation_forces_split_even_when_max_is_hidden(): + result = resolve_unified_speed(False, True, 70, slc_state(65, pending_mph=75), True, False) + assert result.mode == "split" + assert result.max_speed_text == "70" + assert result.confirmation_pending + + +def test_offset_max_pending_and_source_transitions_recompute_mode(): + state = slc_state(65) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split" + state.update(slc_state(65, 5)) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + assert resolve_unified_speed(True, True, 75, state, True, False).mode == "split" + state.update(slc_state(65, 5, pending_mph=75)) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "split" + state.update(slc_state(65, 5)) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" + state.update(slc_state(0, source="None")) + missing = resolve_unified_speed(True, True, 70, state, True, False) + assert missing.mode == "split" + assert missing.posted_speed_text == "–" + state.update(slc_state(65, 5)) + assert resolve_unified_speed(True, True, 70, state, True, False).mode == "merged" diff --git a/selfdrive/ui/tests/test_unified_speed_widget.py b/selfdrive/ui/tests/test_unified_speed_widget.py new file mode 100644 index 0000000000..2140d710ae --- /dev/null +++ b/selfdrive/ui/tests/test_unified_speed_widget.py @@ -0,0 +1,495 @@ +from types import SimpleNamespace +from dataclasses import replace + +import pyray as rl +import pytest + +from cereal import custom +from openpilot.common.constants import CV +from openpilot.selfdrive.ui.onroad.starpilot import slc_speed_limit +from openpilot.selfdrive.ui.onroad.starpilot.unified_speed_presentation import UnifiedSpeedPresentation, resolve_unified_speed +from openpilot.selfdrive.ui.onroad.starpilot.widgets import unified_speed + + +def make_widget(mode="split", pending=False): + widget = object.__new__(unified_speed.UnifiedSpeedWidget) + widget._rect = rl.Rectangle(30, 75, 520, 250) + widget._presentation = UnifiedSpeedPresentation(mode, "70", "65", "70", "+5", "mph", "Map Data", pending, "slc") + widget._show_max = True + widget._slc_state = None + widget._pedal_override = False + widget.hud_renderer = SimpleNamespace(is_cruise_set=True) + return widget + + +@pytest.fixture +def header_icon_cache(monkeypatch): + app = object.__new__(type(unified_speed.gui_app)) + app._scale = app._pixel_scale_x = app._pixel_scale_y = 1.0 + app._cached_render_textures = {} + app._pending_render_textures = {} + geometry, draws, allocations, scales = [], [], [], [] + monkeypatch.setattr(unified_speed, "gui_app", app) + monkeypatch.setattr(unified_speed, "_draw_source_icon", lambda *args: geometry.append(args)) + monkeypatch.setattr(unified_speed, "measure_text_cached", lambda *args: rl.Vector2(100, 28)) + monkeypatch.setattr(rl, "draw_text_ex", lambda *args: None) + monkeypatch.setattr(rl, "draw_texture_pro", lambda *args: draws.append(args)) + monkeypatch.setattr(rl, "rl_scalef", lambda *args: scales.append(args)) + for name in ("rl_push_matrix", "rl_pop_matrix", "begin_texture_mode", "end_texture_mode", "clear_background", + "rl_set_blend_factors_separate", "begin_blend_mode", "end_blend_mode", "set_texture_filter", "set_texture_wrap"): + monkeypatch.setattr(rl, name, lambda *args: None) + + def allocate(width, height): + allocations.append((width, height)) + return SimpleNamespace(texture=SimpleNamespace(width=width, height=height)) + + monkeypatch.setattr(rl, "load_render_texture", allocate) + return app, geometry, draws, allocations, scales + + +def test_header_glyph_cache_is_shared_and_skips_geometry_after_first_frame(header_icon_cache): + app, geometry, draws, allocations, _scales = header_icon_cache + widgets = [make_widget(), make_widget()] + for widget in widgets: + widget._font_semi_bold = None + widget = widgets[0] + for label, icon in (("MAX SET", "speedometer"), ("SPEED LIMIT", "map")): + widget._draw_header(widget.rect, label, icon, rl.WHITE) + assert len(geometry) == 2 + assert allocations == [] + app._populate_render_texture_cache() + assert len(geometry) == 4 + + for frame in range(60): + widget = widgets[frame % 2] + bounds = rl.Rectangle(frame, frame, 260, 250) + widget._draw_header(bounds, "MAX SET", "speedometer", rl.WHITE) + widget._draw_header(bounds, f"LIMIT {frame}", "map", rl.GRAY) + assert len(geometry) == 4 + assert len(draws) == 120 + assert len(allocations) == len(app._cached_render_textures) == 2 + assert app._pending_render_textures == {} + + +@pytest.mark.parametrize("scale,dpi,texture_size", [(0.5, 1.0, 68), (1.0, 2.0, 136), (1.25, 1.5, 128)]) +def test_header_cache_resolution_preserves_logical_geometry(header_icon_cache, scale, dpi, texture_size): + app, geometry, draws, allocations, scales = header_icon_cache + app._scale, app._pixel_scale_x = scale, dpi + for icon in ("speedometer", "map", "camera", "dashboard", "next"): + unified_speed._draw_header_icon(icon, 10, 20) + app._populate_render_texture_cache() + assert len(app._cached_render_textures) == 5 + assert allocations == [(texture_size, texture_size)] * 5 + assert all(args[1:4] == (0, 0, 34) for args in geometry[5:]) + assert scales == [(texture_size / 34, texture_size / 34, 1.0)] * 5 + + unified_speed._draw_header_icon("map", 200, 300) + assert len(geometry) == 10 + source, destination = draws[-1][1:3] + assert (source.width, source.height) == (texture_size, -texture_size) + assert (destination.x, destination.y, destination.width, destination.height) == (200, 300, 34, 34) + + +def test_speed_limit_hit_target_is_right_half_in_both_layouts(): + for mode in ("split", "merged"): + right = make_widget(mode)._speed_limit_bounds(rl.Rectangle(30, 75, 520, 250)) + assert (right.x, right.width) == (290, 260) + + +def test_confirmation_touch_only_accepts_on_speed_limit_side(monkeypatch): + widget = make_widget(pending=True) + writes = [] + monkeypatch.setattr(unified_speed, "Params", lambda memory: SimpleNamespace(put_bool=lambda key, value: writes.append((key, value)))) + widget._handle_mouse_press(rl.Vector2(100, 150)) + assert writes == [] + widget._handle_mouse_press(rl.Vector2(400, 150)) + assert writes == [("SpeedLimitAccepted", True)] + + +def test_merged_speed_limit_side_toggles_sources(monkeypatch): + widget = make_widget("merged") + writes = [] + params = SimpleNamespace(get_bool=lambda _key: False, put_bool=lambda key, value: writes.append((key, value))) + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(ui_params=params)) + widget._handle_mouse_press(rl.Vector2(100, 150)) + assert writes == [] + widget._handle_mouse_press(rl.Vector2(400, 150)) + assert writes == [("SpeedLimitSources", True)] + + +def test_diagnostic_sources_can_be_dismissed_from_max_only_card(monkeypatch): + widget = make_widget("max_only") + widget._slc_state = {} + params = SimpleNamespace(get_bool=lambda _key: True, put_bool=lambda key, value: writes.append((key, value))) + writes = [] + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(ui_params=params)) + widget._handle_mouse_press(rl.Vector2(100, 150)) + assert writes == [("SpeedLimitSources", False)] + + +def test_right_border_overlay_is_clipped_to_speed_limit_side(monkeypatch): + events = [] + monkeypatch.setattr(unified_speed.rl, "begin_scissor_mode", lambda *args: events.append(("begin", args))) + monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: events.append(("outline", args))) + monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: events.append(("divider", args))) + monkeypatch.setattr(unified_speed.rl, "end_scissor_mode", lambda: events.append(("end",))) + for mode, expected in (("split", ["begin", "outline", "end", "divider"]), + ("merged", ["begin", "outline", "end"])): + events.clear() + widget = make_widget(mode) + rect = widget.rect + right = widget._speed_limit_bounds(rect) + widget._draw_speed_limit_border(rect, right, rl.Color(188, 132, 255, 200)) + assert events[0] == ("begin", (290, 75, 261, 251)) + assert [event[0] for event in events] == expected + + +def test_split_and_merged_draw_one_card_with_both_headers(monkeypatch): + cards = [] + lines = [] + monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: cards.append(args[0])) + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.DISENGAGED, + ui_params=SimpleNamespace(get_bool=lambda _key: False))) + monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args)) + monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None) + for mode in ("split", "merged"): + lines.clear() + widget = make_widget(mode) + headers = [] + values = [] + separators = [] + offsets = [] + monkeypatch.setattr(widget, "_draw_header", lambda _bounds, text, icon, _color, rows=headers: rows.append((text, icon))) + monkeypatch.setattr(widget, "_draw_centered_text", lambda text, *args, rows=values, **kwargs: rows.append(text)) + monkeypatch.setattr(widget, "_draw_offset_pill", lambda bounds, text, y, rows=offsets: rows.append((bounds, text, y))) + monkeypatch.setattr(widget, "_draw_merged_separator", lambda _rect, rows=separators: rows.append(True)) + monkeypatch.setattr(widget, "_draw_active_emphasis", lambda *args: None) + widget._render(widget.rect) + assert headers == [("MAX SET", "speedometer"), ("SPEED LIMIT", "map")] + assert separators == ([True] if mode == "merged" else []) + assert sum(line[0].x == line[1].x == 290 for line in lines) == (1 if mode == "split" else 0) + assert values == (["70", "mph"] if mode == "merged" else ["70", "mph", "65", "mph"]) + assert offsets[0][0].x == 290 + assert offsets[0][2] == (136 if mode == "merged" else 250) + assert len(cards) == 2 + + +def test_merged_draws_effective_speed_once_and_skips_active_line(monkeypatch): + widget = make_widget("merged") + widget._presentation = replace(widget._presentation, max_speed_text="71", effective_speed_text="70", active_side="shared") + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED, + ui_params=SimpleNamespace(get_bool=lambda _key: False))) + monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: None) + monkeypatch.setattr(unified_speed.rl, "draw_rectangle_rounded_lines_ex", lambda *args: None) + lines = [] + monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args)) + monkeypatch.setattr(widget, "_draw_merged_separator", lambda _rect: None) + monkeypatch.setattr(widget, "_draw_header", lambda *args: None) + monkeypatch.setattr(widget, "_draw_offset_pill", lambda *args: None) + values = [] + monkeypatch.setattr(widget, "_draw_centered_text", lambda text, *args, **kwargs: values.append(text)) + widget._render(widget.rect) + assert values == ["70", "mph"] + assert lines == [] + + +def test_merged_separator_has_shallow_center_dip(monkeypatch): + widget = make_widget("merged") + segments = [] + monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: segments.append(("line", args))) + monkeypatch.setattr(unified_speed.rl, "draw_spline_segment_bezier_cubic", lambda *args: segments.append(("curve", args))) + widget._draw_merged_separator(widget.rect) + assert [segment[0] for segment in segments] == ["line", "curve", "line", "curve", "line"] + assert segments[0][1][0].y == widget.rect.y + 76 + assert segments[2][1][0].y == widget.rect.y + 88 + + +def test_enabled_slc_stays_full_width_when_plan_is_stale(monkeypatch): + widget = make_widget("split") + widget._snapshot_frame = None + widget.hud_renderer = SimpleNamespace(is_cruise_available=True, is_cruise_set=True, set_speed=70) + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace( + sm=SimpleNamespace(frame=1), starpilot_toggles={}, is_metric=False, engaged=False, + )) + monkeypatch.setattr(unified_speed, "_is_slc_enabled", lambda: True) + monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: None) + assert widget.get_size() == (520.0, 250.0) + assert widget.is_visible + assert widget._presentation.posted_speed_text == "–" + + +@pytest.fixture +def pedal_snapshot(monkeypatch): + class SubMaster(dict): + pass + + sm = SubMaster(carState=SimpleNamespace(gasPressed=True)) + sm.frame = 20 + sm.valid = {"carState": True} + sm.alive = {"carState": True} + sm.recv_frame = {"carState": 20} + ui = SimpleNamespace(sm=sm, started_frame=10, engaged=True, starpilot_toggles={}, is_metric=False) + widget = make_widget() + widget._snapshot_frame = None + widget.hud_renderer = SimpleNamespace(is_cruise_available=True, is_cruise_set=True, set_speed=70) + monkeypatch.setattr(unified_speed, "ui_state", ui) + monkeypatch.setattr(unified_speed, "_is_slc_enabled", lambda: True) + monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: None) + return widget, ui + + +@pytest.mark.parametrize("gas,engaged,cruise_set,valid,alive,received,expected", [ + (True, True, True, True, True, 20, True), + (False, True, True, True, True, 20, False), + (True, False, True, True, True, 20, False), + (True, True, False, True, True, 20, False), + (True, True, True, False, True, 20, False), + (True, True, True, True, False, 20, False), + (True, True, True, True, True, 9, False), +]) +def test_pedal_override_requires_fresh_gas_and_engaged_cruise(pedal_snapshot, gas, engaged, cruise_set, valid, alive, received, expected): + widget, ui = pedal_snapshot + ui.sm["carState"].gasPressed = gas + ui.engaged = engaged + widget.hud_renderer.is_cruise_set = cruise_set + ui.sm.valid["carState"] = valid + ui.sm.alive["carState"] = alive + ui.sm.recv_frame["carState"] = received + widget._refresh_snapshot() + assert widget._pedal_override == expected + + +def test_pedal_cue_clears_on_release_with_a_persistent_slc_override(pedal_snapshot, monkeypatch): + widget, ui = pedal_snapshot + sm = ui.sm + state = { + "speed_conversion": CV.MS_TO_MPH, "accepted_speed_limit_ms": 65 * CV.MPH_TO_MS, + "effective_target_ms": 70 * CV.MPH_TO_MS, "offset_ms": 5 * CV.MPH_TO_MS, + "speed_limit_changed": False, "unconfirmed_valid": False, "presented_source": "Map Data", + "slc_is_limiting_max_set": False, "slc_overridden_speed": 80 * CV.MPH_TO_MS, + } + monkeypatch.setattr(unified_speed, "_get_slc_state", lambda: state) + widget._refresh_snapshot() + assert widget._pedal_override + presentation = widget._presentation + + sm["carState"].gasPressed = False + widget._refresh_snapshot() + assert widget._pedal_override + sm.frame += 1 + sm.recv_frame["carState"] = sm.frame + widget._refresh_snapshot() + assert not widget._pedal_override + assert widget._presentation == presentation + assert widget._slc_state["slc_overridden_speed"] > 0 + + +@pytest.mark.parametrize("mode", ["split", "merged", "max_only", "limit_only"]) +@pytest.mark.parametrize("unit", ["mph", "km/h"]) +def test_pedal_cue_mutes_targets_and_preserves_units_offsets_and_layout(monkeypatch, mode, unit): + widget = make_widget(mode) + widget._pedal_override = True + widget._font_semi_bold = None + widget._show_max = mode != "limit_only" + widget._presentation = replace(widget._presentation, unit_text=unit) + if mode in ("max_only", "limit_only"): + widget._rect.width = 250 + values, headers, pauses, offsets, lines = [], [], [], [], [] + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED)) + monkeypatch.setattr(unified_speed, "draw_control_card", lambda *args, **kwargs: None) + monkeypatch.setattr(unified_speed, "measure_text_cached", lambda *args: rl.Vector2(60, 28)) + monkeypatch.setattr(rl, "draw_rectangle_rounded_lines_ex", lambda *args: None) + monkeypatch.setattr(rl, "draw_rectangle_rec", lambda *args: pauses.append(args)) + monkeypatch.setattr(rl, "draw_line_ex", lambda *args: lines.append(args)) + monkeypatch.setattr(widget, "_draw_merged_separator", lambda *args: None) + monkeypatch.setattr(widget, "_draw_header", lambda bounds, text, icon, color: headers.append(color)) + monkeypatch.setattr(widget, "_draw_centered_text", lambda text, bounds, y, size, color, **kwargs: values.append((text, bounds, size, color))) + monkeypatch.setattr(widget, "_draw_offset_pill", lambda bounds, text, y: offsets.append(text)) + + widget._render(widget.rect) + speed_values = [value for value in values if value[2] == unified_speed.VALUE_FONT_SIZE] + unit_values = [value for value in values if value[2] == unified_speed.UNIT_FONT_SIZE] + expected_speeds = {"split": ["70", "65"], "merged": ["70"], "max_only": ["70"], "limit_only": ["65"]} + assert [value[0] for value in speed_values] == expected_speeds[mode] + assert all(value[3] == unified_speed.COLORS.DISENGAGED for value in speed_values + unit_values) + assert all(color == unified_speed.COLORS.DISENGAGED for color in headers) + assert [value[0] for value in unit_values] == [unit] * (2 if mode == "split" else 1) + assert len(pauses) == 2 * len(unit_values) + assert all(color == unified_speed.OFFSET_COLOR for _bounds, color in pauses) + assert offsets == ([] if mode == "max_only" else ["+5"]) + assert not any(line[2] == 3 for line in lines) + for index, value in enumerate(unit_values): + pause = pauses[index * 2][0] + assert pause.x == pytest.approx(value[1].x + (value[1].width - 60) / 2 - 20) + + values.clear() + pauses.clear() + widget._pedal_override = False + widget._render(widget.rect) + assert not pauses + assert all(value[3] == unified_speed.COLORS.WHITE for value in values if value[2] == unified_speed.VALUE_FONT_SIZE) + assert all(value[3] == unified_speed.COLORS.WHITE_TRANSLUCENT for value in values if value[2] == unified_speed.UNIT_FONT_SIZE) + + +def test_split_merged_transitions_keep_the_same_footprint(monkeypatch): + widget = make_widget("split") + monkeypatch.setattr(widget, "_refresh_snapshot", lambda: None) + sizes = [] + for mode in ("split", "merged", "split", "merged"): + widget._presentation = replace(widget._presentation, mode=mode) + sizes.append(widget.get_size()) + assert sizes == [(520.0, 250.0)] * 4 + + +@pytest.fixture +def slc_ui(monkeypatch): + class Params(dict): + def get_bool(self, key): + return bool(self.get(key)) + + def get(self, key, encoding=None): + return super().get(key) + + class SubMaster(dict): + recv_frame = {"starpilotPlan": 10} + valid = {"starpilotCarState": True} + + plan = custom.StarPilotPlan.new_message( + slcSpeedLimit=30 * CV.MPH_TO_MS, slcSpeedLimitOffset=0.0, slcSpeedLimitSource="Map Data", + slcOverriddenSpeed=0.0, slcMapSpeedLimit=30 * CV.MPH_TO_MS, slcMapboxSpeedLimit=0.0, + slcNextSpeedLimit=0.0, unconfirmedSlcSpeedLimit=0.0, speedLimitChanged=False, + ) + sm = SubMaster(starpilotPlan=plan, starpilotCarState=SimpleNamespace(dashboardSpeedLimit=0.0)) + sm.recv_frame = sm.recv_frame.copy() + params = Params(SpeedLimitController=True, ShowSpeedLimits=False) + ui = SimpleNamespace( + sm=sm, started_frame=10, is_metric=False, ui_params=params, starpilot_toggles={}, + params_memory=SimpleNamespace(get_float=lambda _key: 0.0), + ) + monkeypatch.setattr(slc_speed_limit, "ui_state", ui) + monkeypatch.setattr(slc_speed_limit, "starpilot_state", SimpleNamespace(car_state=SimpleNamespace(hasDashSpeedLimits=True))) + monkeypatch.setattr(slc_speed_limit, "_tick_pulse", lambda *args: None) + return ui + + +def test_slc_state_extraction_respects_feature_and_display_toggles(slc_ui): + assert slc_speed_limit._is_slc_enabled() + assert slc_speed_limit._get_slc_state()["slc_enabled"] + slc_ui.starpilot_toggles["speed_limit_controller"] = False + assert not slc_speed_limit._is_slc_enabled() + assert slc_speed_limit._get_slc_state() is None + slc_ui.ui_params["ShowSpeedLimits"] = True + assert not slc_speed_limit._get_slc_state()["slc_enabled"] + slc_ui.starpilot_toggles["speed_limit_controller"] = True + slc_ui.sm.recv_frame["starpilotPlan"] = 9 + assert slc_speed_limit._get_slc_state() is None + + +@pytest.mark.parametrize("presented_source,expected_source,expected_speed", [ + ("", "Map Data", "30"), + ("Map Data", "Map Data", "30"), + ("None", "None", "–"), + ("Previous Limit", "Previous Limit", "30"), + ("Vision", "Vision", "30"), +]) +def test_serialized_plan_source_defaults_and_explicit_values(slc_ui, presented_source, expected_source, expected_speed): + message = slc_ui.sm["starpilotPlan"] + if presented_source: + message.slcPresentedSpeedLimitSource = presented_source + # Replay decodes older plans with a present but empty Text attribute. + with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan: + slc_ui.sm["starpilotPlan"] = plan + state = slc_speed_limit._get_slc_state() + result = resolve_unified_speed(True, True, 35, state, True, False) + assert result.source == expected_source + assert result.posted_speed_text == expected_speed + assert result.mode == "split" + + +def test_legacy_replay_limit_and_offset_merge_with_max_set(slc_ui): + message = slc_ui.sm["starpilotPlan"] + message.slcSpeedLimitOffset = 5 * CV.MPH_TO_MS + with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan: + slc_ui.sm["starpilotPlan"] = plan + result = resolve_unified_speed(True, True, 35, slc_speed_limit._get_slc_state(), True, False) + assert (result.source, result.posted_speed_text, result.effective_speed_text) == ("Map Data", "30", "35") + assert (result.mode, result.offset_text) == ("merged", "+5") + + +@pytest.mark.parametrize("presented_source,limiting,max_speed,enabled,overridden,expected_side,line_x", [ + ("", False, 40, True, False, "slc", 308), + ("", False, 34, True, False, "max", 48), + ("", False, 35, True, False, "shared", None), + ("", False, 40, False, False, "max", 48), + ("", False, 40, True, True, "none", None), + ("Map Data", False, 40, True, False, "max", 48), + ("Map Data", True, 40, True, False, "slc", 308), +]) +def test_active_underline_with_legacy_and_current_plans(slc_ui, monkeypatch, presented_source, limiting, + max_speed, enabled, overridden, expected_side, line_x): + message = slc_ui.sm["starpilotPlan"] + message.slcSpeedLimitOffset = 5 * CV.MPH_TO_MS + message.slcPresentedSpeedLimitSource = presented_source + message.slcIsLimitingMaxSet = limiting + message.slcOverriddenSpeed = 40 * CV.MPH_TO_MS if overridden else 0.0 + slc_ui.starpilot_toggles["speed_limit_controller"] = enabled + slc_ui.ui_params["ShowSpeedLimits"] = True + with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan: + slc_ui.sm["starpilotPlan"] = plan + presentation = resolve_unified_speed(True, True, max_speed, slc_speed_limit._get_slc_state(), enabled, False) + assert presentation.active_side == expected_side + + widget = make_widget(presentation.mode) + widget._presentation = presentation + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED)) + lines = [] + monkeypatch.setattr(unified_speed.rl, "draw_line_ex", lambda *args: lines.append(args)) + widget._draw_active_emphasis(widget.rect) + if line_x is None: + assert lines == [] + else: + assert len(lines) == 1 + assert (lines[0][0].x, lines[0][0].y, lines[0][1].x) == (line_x, 140, line_x + 224) + assert lines[0][3] == unified_speed.UNIFIED_ACCENT + + +def test_legacy_plan_without_active_source_does_not_use_diagnostic_map_limit(slc_ui): + message = slc_ui.sm["starpilotPlan"] + message.slcSpeedLimitSource = "None" + with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan: + slc_ui.sm["starpilotPlan"] = plan + state = slc_speed_limit._get_slc_state() + assert round(state["map_sl"]) == 30 + result = resolve_unified_speed(True, True, 35, state, True, False) + assert (result.source, result.posted_speed_text, result.mode) == ("None", "–", "split") + + +def test_legacy_pending_candidate_remains_visible_without_active_source(slc_ui): + message = slc_ui.sm["starpilotPlan"] + message.slcSpeedLimitSource = "None" + message.unconfirmedSlcSpeedLimit = 45 * CV.MPH_TO_MS + message.speedLimitChanged = True + with custom.StarPilotPlan.from_bytes(message.to_bytes()) as plan: + slc_ui.sm["starpilotPlan"] = plan + result = resolve_unified_speed(True, True, 35, slc_speed_limit._get_slc_state(), True, False) + assert (result.posted_speed_text, result.mode, result.confirmation_pending) == ("45", "split", True) + + +def test_header_colors_preserve_engaged_disengaged_and_override_semantics(monkeypatch): + widget = make_widget() + colors = unified_speed.COLORS + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED)) + assert widget._max_header_color("max", True) == colors.ENGAGED + assert widget._max_header_color("slc", True) == colors.GREY + assert widget._limit_header_color("slc", False) == colors.ENGAGED + + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.DISENGAGED)) + assert widget._max_header_color("max", True) == colors.DISENGAGED + assert widget._limit_header_color("slc", False) == colors.DISENGAGED + + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.OVERRIDE)) + assert widget._max_header_color("max", True) == colors.DISENGAGED + assert widget._limit_header_color("slc", False) == colors.DISENGAGED + + monkeypatch.setattr(unified_speed, "ui_state", SimpleNamespace(status=unified_speed.UIStatus.ENGAGED)) + assert widget._limit_header_color("none", True) == colors.DISENGAGED diff --git a/selfdrive/ui/tests/test_widget_layout_manager.py b/selfdrive/ui/tests/test_widget_layout_manager.py index b9e6a17402..f9e0ad677a 100644 --- a/selfdrive/ui/tests/test_widget_layout_manager.py +++ b/selfdrive/ui/tests/test_widget_layout_manager.py @@ -137,6 +137,19 @@ class TestWidgetLayoutManager(unittest.TestCase): # w3: should stack directly below w1: y = 75 + 100 + 15 = 190 self.assertEqual(w3.rect.y, 190) + def test_wide_unified_card_stays_inside_the_left_edge(self): + card = DummyLayoutWidget("unified_speed", priority=1, width=520, height=250) + gauge = DummyLayoutWidget("aethergauge", priority=3, width=176, height=260) + self.layout_manager.register_widget("left", card) + self.layout_manager.register_widget("left", gauge) + + self.layout_manager.update_layout(self.content_rect) + + self.assertEqual(card.rect.x, self.content_rect.x + 30) + self.assertEqual(card.rect.y, self.content_rect.y + 45) + self.assertEqual(gauge.rect.x + gauge.rect.width / 2, self.content_rect.x + 146) + self.assertEqual(gauge.rect.y, card.rect.y + card.rect.height + self.layout_manager.spacing) + def test_dynamic_repositioning_on_rect_change(self): # Register a widget w1 = DummyLayoutWidget("w1", priority=1, width=100, height=100) diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 8d67a60e4b..be25ed2d19 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -2370,8 +2370,8 @@ { "key": "ShowSLCOffset", "label": "Show Speed Limit Offset", - "description": "Show the current offset from the posted limit on the driving screen.", - "picker_description": "Shows the current offset from the posted limit.", + "description": "Show the current offset on the compact driving display. The unified Max Set / Speed Limit card always shows nonzero offsets.", + "picker_description": "Shows the offset on the compact display; the unified card always shows nonzero offsets.", "data_type": "bool", "ui_type": "toggle", "parent_key": "SpeedLimitController", @@ -2990,8 +2990,8 @@ { "key": "UseVienna", "label": "Use Vienna-Style Speed Signs", - "description": "Show Vienna-style (EU) speed-limit signs instead of MUTCD (US).", - "picker_description": "Uses Vienna-style speed-limit signs.", + "description": "Use Vienna-style (EU) speed-limit signs on the compact driving display. The unified Max Set / Speed Limit card uses its own layout.", + "picker_description": "Uses Vienna-style signs on the compact display.", "data_type": "bool", "ui_type": "toggle", "parent_key": "NavigationUI", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 23d3cd3334..6c9e9d83b1 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -1461,6 +1461,8 @@ class StarPilotVariables: speed_limit_confirmation = self.get_value("SLCConfirmation", condition=toggle.speed_limit_controller) toggle.speed_limit_confirmation_higher = self.get_value("SLCConfirmationHigher", condition=speed_limit_confirmation) toggle.speed_limit_confirmation_lower = self.get_value("SLCConfirmationLower", condition=speed_limit_confirmation) + # Legacy setting is hidden in the current UI. SLC's pedal and +/- overrides + # remain available regardless of its saved value; keep loading it for compatibility. slc_override_method = self.get_value("SLCOverride", cast=float, condition=toggle.speed_limit_controller) toggle.speed_limit_controller_override_manual = slc_override_method == 1 toggle.speed_limit_controller_override_set_speed = slc_override_method == 2 diff --git a/starpilot/controls/lib/mapbox_speed_limit.py b/starpilot/controls/lib/mapbox_speed_limit.py new file mode 100644 index 0000000000..2e9d586030 --- /dev/null +++ b/starpilot/controls/lib/mapbox_speed_limit.py @@ -0,0 +1,130 @@ +import calendar +import json +from concurrent.futures import ThreadPoolExecutor + +import requests + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL +from openpilot.starpilot.common.starpilot_utilities import calculate_bearing_offset, is_url_pingable + + +FREE_MAPBOX_REQUESTS = 100_000 + + +class MapboxSpeedLimit: + def __init__(self, params): + self.params = params + try: + self.requests = json.loads(params.get("MapBoxRequests", encoding="utf-8") or "{}") + except (TypeError, ValueError): + self.requests = {} + self.requests.setdefault("total_requests", 0) + self.requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - 28 * 100) + + self.host = "https://api.mapbox.com" + self.token = params.get("MapboxSecretKey", encoding="utf-8") + self.limit = 0.0 + self.segment_distance = 0.0 + self.future = None + self.executor = ThreadPoolExecutor(max_workers=1) + self.session = requests.Session() + self.session.headers.update({"Accept-Language": "en"}) + self.session.headers.update({"User-Agent": "starpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/StarPilot)"}) + + def reset(self): + # A discarded future may still finish, but only update() can publish its result. + if self.future is not None: + self.future.cancel() + self.future = None + self.limit = 0.0 + self.segment_distance = 0.0 + + def shutdown(self): + self.reset() + self.executor.shutdown(wait=False, cancel_futures=True) + self.session.close() + + def _request(self, position, v_ego): + if not is_url_pingable(self.host): + return 0.0, v_ego + + self.requests["total_requests"] += 1 + self.params.put_nonblocking("MapBoxRequests", json.dumps(self.requests)) + + bearing = position.get("bearing") + latitude = position.get("latitude") + longitude = position.get("longitude") + future_latitude, future_longitude = calculate_bearing_offset(latitude, longitude, bearing, v_ego) + url = f"{self.host}/matching/v5/mapbox/driving/{longitude},{latitude};{future_longitude},{future_latitude}.json" + params = { + "access_token": self.token, + "annotations": "maxspeed,distance", + "geometries": "polyline6", + "overview": "full", + "steps": "false", + "radiuses": "10;10", + "tidy": "true", + } + response = self.session.get(url, params=params, timeout=10) + response.raise_for_status() + matchings = response.json().get("matchings") or [] + if not matchings: + return 0.0, v_ego + legs = (matchings[0] or {}).get("legs") or [] + if not legs: + return 0.0, v_ego + + annotation = legs[0].get("annotation") or {} + distances = annotation.get("distance") or [v_ego] + speeds = annotation.get("maxspeed") or [] + if not speeds: + return 0.0, v_ego + first = speeds[0] + try: + speed = float(first.get("speed")) if first.get("speed") != "none" else 0.0 + except (TypeError, ValueError): + speed = 0.0 + if speed <= 0: + return 0.0, v_ego + conversion = CV.MPH_TO_MS if first.get("unit", "km/h") == "mph" else CV.KPH_TO_MS + return speed * conversion, distances[0] + + def update(self, now, time_validated, v_ego, gps_valid, position, steering_angle, angle_offset): + if not gps_valid or not self.token or abs(steering_angle - angle_offset) >= 45: + self.reset() + return + + if time_validated and now.month != self.requests.get("month"): + self.requests.update({ + "month": now.month, + "total_requests": 0, + "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, now.month)[1] * 100, + }) + if self.requests["total_requests"] >= self.requests["max_requests"]: + self.reset() + return + + if self.future is not None: + if not self.future.done(): + return + future = self.future + self.future = None + try: + self.limit, self.segment_distance = future.result() + except Exception as exception: + print(f"Unexpected error in Mapbox request: {exception}") + self.limit, self.segment_distance = 0.0, v_ego + return + + if v_ego < 1: + return + if self.segment_distance > 0: + self.segment_distance -= v_ego * DT_MDL + return + + try: + self.future = self.executor.submit(self._request, dict(position), v_ego) + except RuntimeError: + self.segment_distance = v_ego + return diff --git a/starpilot/controls/lib/speed_limit_controller.py b/starpilot/controls/lib/speed_limit_controller.py index 312fb3bff8..5d8b333957 100644 --- a/starpilot/controls/lib/speed_limit_controller.py +++ b/starpilot/controls/lib/speed_limit_controller.py @@ -1,19 +1,20 @@ #!/usr/bin/env python3 # PFEIFER - SLC - Modified by FrogAi -import calendar -import json -import requests - -from concurrent.futures import ThreadPoolExecutor - from openpilot.common.constants import CV from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from cereal import custom -from openpilot.starpilot.common.starpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable +from openpilot.starpilot.controls.lib.mapbox_speed_limit import MapboxSpeedLimit -FREE_MAPBOX_REQUESTS = 100_000 + +SOURCE_NONE = "None" +SOURCE_DASHBOARD = "Dashboard" +SOURCE_MAP = "Map Data" +SOURCE_VISION = "Vision" +SOURCE_MAPBOX = "Mapbox" +SOURCE_PREVIOUS_LIMIT = "Previous Limit" +REAL_SOURCES = (SOURCE_DASHBOARD, SOURCE_MAP, SOURCE_VISION, SOURCE_MAPBOX) OFFSET_MAP_IMPERIAL = [ (0, 11.2, "speed_limit_offset1"), # 0–24 mph @@ -36,83 +37,90 @@ 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 +SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND = 0.1 +SAME_LIMIT_TOLERANCE = 1.0 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 + class SpeedLimitController: def __init__(self, StarPilotVCruise): self.starpilot_planner = StarPilotVCruise.starpilot_planner self.starpilot_toggles = None + self.mapbox = MapboxSpeedLimit(self.starpilot_planner.params) - self.calling_mapbox = False - self.override_slc = False - self.override_disable_timer = 0.0 - self._prev_v_cruise = None - self._persistent_override_speed = 0.0 - self._set_speed_override_input_consumed = False + self.source = SOURCE_NONE + self.target = 0.0 + self.map_speed_limit = 0.0 + self.next_speed_limit = 0.0 + self.vision_limit = 0.0 + self.overridden_speed = 0.0 - self.denied_target = 0 - self.map_speed_limit = 0 - self.mapbox_limit = 0 - self.next_speed_limit = 0 - self.overridden_speed = 0 - self.segment_distance = 0 - self.speed_limit_changed_timer = 0 - self.target = 0 - self.unconfirmed_speed_limit = 0 - self.vision_limit = 0 - - self.previous_source = "None" - self.source = "None" + self.last_valid_limit = max(self.starpilot_planner.params.get_float("PreviousSpeedLimit"), 0.0) + self.last_valid_source = SOURCE_NONE # The persisted number has no known live source. + self.pending_limit = 0.0 + self.pending_source = SOURCE_NONE + self.confirmation_time = 0.0 + self.denied_limit = 0.0 self.previous_road_name = "" - self._slc_adopt_counter = 0 - - mapbox_requests_raw = self.starpilot_planner.params.get("MapBoxRequests", encoding="utf-8") - try: - self.mapbox_requests = json.loads(mapbox_requests_raw or "{}") - except (TypeError, ValueError): - self.mapbox_requests = {} - self.mapbox_requests.setdefault("total_requests", 0) - self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100)) - - self.mapbox_host = "https://api.mapbox.com" - self.mapbox_token = self.starpilot_planner.params.get("MapboxSecretKey", encoding="utf-8") - - self.previous_target = self.starpilot_planner.params.get_float("PreviousSpeedLimit") - self.last_valid_limit = self.previous_target if self.previous_target > 0 else 0 - - self.executor = ThreadPoolExecutor(max_workers=1) - self.mapbox_future = None - - self.session = requests.Session() - self.session.headers.update({"Accept-Language": "en"}) - self.session.headers.update({"User-Agent": "starpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/StarPilot)"}) + self.set_speed_override = 0.0 + self.pedal_override = 0.0 + self.previous_set_speed = None + self.consume_set_speed_change = False + self.override_disable_time = 0.0 + self.limit_change_started = False + self.confirmation_button_consumed = False + self._active_control = False + self._using_experimental_fallback = False + self._using_previous_limit_fallback = False + self._mode = "off" def shutdown(self): - self.executor.shutdown(wait=False, cancel_futures=True) - self.session.close() + self.mapbox.shutdown() + + @property + def mapbox_limit(self): + return self.mapbox.limit + + @property + def confirmation_pending(self): + return self.pending_limit >= 1 + + @property + def unconfirmed_speed_limit(self): + return self.pending_limit + + @property + def presented_source(self): + if self.confirmation_pending: + return self.pending_source + if self.source in REAL_SOURCES: + return self.source + if self._using_previous_limit_fallback and self.target >= 1: + return self.last_valid_source if self.last_valid_source in REAL_SOURCES else SOURCE_PREVIOUS_LIMIT + if (self.denied_limit > 0 and self.last_valid_limit > 0 and self.target >= 1 and + abs(self.target - self.last_valid_limit) < SAME_LIMIT_TOLERANCE): + return self.last_valid_source if self.last_valid_source in REAL_SOURCES else SOURCE_PREVIOUS_LIMIT + return SOURCE_NONE @property def experimental_mode(self): - return self.target == 0 and bool(getattr(self.starpilot_toggles, "slc_fallback_experimental_mode", False)) + return self._active_control and self._using_experimental_fallback @property def target_to_use(self): - if self.source == "None" and self.target > 0 and self.last_valid_limit > 0: - if self.target >= self.last_valid_limit: - return self.last_valid_limit + # Keep Set Speed fallback from arming an override against a higher fake limit. + if self.source == SOURCE_NONE and self.target > 0 and self.last_valid_limit > 0: + return min(self.target, self.last_valid_limit) return self.target - def get_offset(self, target_speed): + def get_offset(self, limit): if self.starpilot_toggles is None: - return 0 + return 0.0 offset_map = OFFSET_MAP_METRIC if self.starpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL - return next((getattr(self.starpilot_toggles, offset) for low, high, offset in offset_map if low <= target_speed < high), 0) + return next((getattr(self.starpilot_toggles, name) for low, high, name in offset_map if low <= limit < high), 0.0) @property def offset(self): @@ -124,458 +132,303 @@ class SpeedLimitController: 0 < limit <= max(getattr(self.starpilot_toggles, "vision_speed_limit_low_limit_threshold", 0), 0) ) - def _confirmation_required(self, desired_source, desired_target): - return desired_source != "None" and ( - (desired_target < self.target and self.starpilot_toggles.speed_limit_confirmation_lower) or - (desired_target > self.target and self.starpilot_toggles.speed_limit_confirmation_higher) - ) + def reset_control_state(self): + self._clear_pending() + self.clear_override() + self.previous_set_speed = None + self.consume_set_speed_change = False + self.override_disable_time = 0.0 + self.limit_change_started = False + self.confirmation_button_consumed = False + self._active_control = False + self._using_experimental_fallback = False + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit") - def clear_override(self): - self.override_slc = False - self.overridden_speed = 0 - self._persistent_override_speed = 0.0 + def _get_vision_limit(self, v_ego, sm, display_only): + enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False) + self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if enabled else 0.0 + limit = self.vision_limit + if not display_only and self.low_vision_limit_filtered(limit): + return 0.0 - def clear_persistent_override(self): - self._persistent_override_speed = 0.0 - - def clear_persistent_override_for_limit_change(self, previous_limit, new_limit): - if self._persistent_override_speed <= 0: - return - if previous_limit <= 0 or new_limit <= 0 or abs(new_limit - previous_limit) < 0.1: - return - - new_target_with_offset = new_limit + self.get_offset(new_limit) - if new_limit < previous_limit or self._persistent_override_speed <= new_target_with_offset: - self.clear_persistent_override() - - 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 abs(sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45: - self.mapbox_limit = 0 - self.segment_distance = 0 - return - - if v_ego < 1: - return - - if self.segment_distance > 0: - self.segment_distance -= v_ego * DT_MDL - return - - if self.calling_mapbox: - self.segment_distance = v_ego - return - - def make_request(): - successful = False - response_data = None - try: - if not is_url_pingable(self.mapbox_host): - self.segment_distance = 1000 - successful = True - return None - - if time_validated: - current_month = now.month - if current_month != self.mapbox_requests.get("month"): - self.mapbox_requests.update({ - "month": current_month, - "total_requests": 0, - "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100, - }) - - self.mapbox_requests["total_requests"] += 1 - self.starpilot_planner.params.put_nonblocking("MapBoxRequests", json.dumps(self.mapbox_requests)) - - current_bearing = self.starpilot_planner.gps_position.get("bearing") - current_latitude = self.starpilot_planner.gps_position.get("latitude") - current_longitude = self.starpilot_planner.gps_position.get("longitude") - - future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego) - - url = ( - f"{self.mapbox_host}/matching/v5/mapbox/driving/" - f"{current_longitude},{current_latitude};" - f"{future_longitude},{future_latitude}.json" - ) - - mapbox_params = { - "access_token": self.mapbox_token, - "annotations": "maxspeed,distance", - "geometries": "polyline6", - "overview": "full", - "steps": "false", - "radiuses": "10;10", - "tidy": "true", - } - - response = self.session.get(url, params=mapbox_params, timeout=10) - response.raise_for_status() - - successful = True - response_data = response.json() - except Exception as exception: - print(f"Unexpected error in Mapbox request: {exception}") - finally: - self.calling_mapbox = False - - if not successful: - self.mapbox_limit = 0 - self.segment_distance = v_ego - return response_data - - def complete_request(future): - try: - data = future.result() - if data: - matchings = data.get("matchings") or [] - if not matchings: - self.mapbox_limit = 0 - self.segment_distance = v_ego - return - - legs = (matchings[0] or {}).get("legs") or [] - if not legs: - self.mapbox_limit = 0 - self.segment_distance = v_ego - return - - annotation = legs[0].get("annotation") or {} - - distances = annotation.get("distance") or [v_ego] - segment_distance = distances[0] - - speed_data = annotation.get("maxspeed", []) - if speed_data: - first_segment_speed = speed_data[0] - try: - raw_speed = float(first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0.0) - except (ValueError, TypeError): - raw_speed = 0.0 - unit = first_segment_speed.get("unit", "km/h") - if raw_speed > 0: - if unit == "mph": - self.mapbox_limit = raw_speed * CV.MPH_TO_MS - else: - self.mapbox_limit = raw_speed * CV.KPH_TO_MS - self.segment_distance = segment_distance - return - - self.mapbox_limit = 0 - self.segment_distance = v_ego - - except Exception as exception: - print(f"Mapbox Callback Error: {exception}") - self.mapbox_limit = 0 - self.segment_distance = v_ego - finally: - self.mapbox_future = None - - self.calling_mapbox = True - try: - future = self.executor.submit(make_request) - except RuntimeError: - self.calling_mapbox = False - self.segment_distance = v_ego - return - - self.mapbox_future = future - future.add_done_callback(complete_request) - - def handle_limit_change(self, desired_source, desired_target, current_road_name, v_ego, sm): - self.speed_limit_changed_timer += DT_MDL - previous_limit = self.last_valid_limit if self.last_valid_limit > 0 else self.target - - long_active = sm["carControl"].longActive - accepted_by_accel_button = sm["starpilotCarState"].accelPressed and long_active - confirmation_required = self._confirmation_required(desired_source, desired_target) - higher_confirmation = confirmation_required and desired_target > self.target - speed_limit_accepted = confirmation_required and accepted_by_accel_button - if confirmation_required and not speed_limit_accepted and self._slc_adopt_counter % 4 == 0: - speed_limit_accepted = self.starpilot_planner.params_memory.get_bool("SpeedLimitAccepted") - if not confirmation_required: - self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") - self.unconfirmed_speed_limit = 0 - speed_limit_denied = confirmation_required and ( - sm["starpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30 and long_active) - ) - - if not long_active and not sm["selfdriveState"].enabled: - speed_limit_accepted = True - - if speed_limit_accepted: - self.source = desired_source - self.target = desired_target - self.clear_persistent_override_for_limit_change(previous_limit, desired_target) - set_speed_kph = float(sm["carState"].vCruise) - target_with_offset = self.target + self.offset - if ( - higher_confirmation - and long_active - and 0 < set_speed_kph < V_CRUISE_UNSET - and set_speed_kph * CV.KPH_TO_MS < target_with_offset - ): - self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", target_with_offset) - if accepted_by_accel_button and confirmation_required: - self._set_speed_override_input_consumed = True - - self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") - - elif speed_limit_denied: - self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") - self.denied_target = desired_target - - self.previous_source = desired_source - self.previous_target = desired_target - self.previous_road_name = current_road_name - - elif desired_target != self.target and not confirmation_required: - self.source = desired_source - self.target = desired_target - self.clear_persistent_override_for_limit_change(previous_limit, desired_target) - - elif desired_target == self.target: - self.source = desired_source - self.target = desired_target - - else: - self.source = "None" - self.unconfirmed_speed_limit = desired_target - - if (self.target != self.previous_target or self.previous_road_name != current_road_name) and self.target > 0 and not speed_limit_denied: - self.denied_target = 0 - - self.previous_source = self.source - self.previous_target = self.target - self.previous_road_name = current_road_name - - self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target)) - - def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm, display_only=False): - self.update_map_speed_limit(v_ego, sm) - vision_enabled = getattr(self.starpilot_toggles, "vision_speed_limit_detection", False) - self.vision_limit = self.starpilot_planner.params_memory.get_float("VisionSpeedLimit") if vision_enabled else 0 - usable_vision_limit = self.vision_limit - if not display_only and self.low_vision_limit_filtered(usable_vision_limit): - usable_vision_limit = 0 - # The planner clamps V_CRUISE_UNSET to V_CRUISE_MAX, so plausibility must use the raw selected speed. raw_set_speed_kph = float(sm["carState"].vCruise) - selected_set_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0 - reference_speed = selected_set_speed if selected_set_speed > 0 else max(float(v_ego), 0) - # vEgo jitters around zero at standstill; do not let that switch the active source. - if ( - usable_vision_limit > 0 and reference_speed > 0 and not sm["carState"].standstill and - abs(usable_vision_limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA - ): - support_count = self.starpilot_planner.params_memory.get_int("VisionSpeedLimitSupportCount") - support_speed = self.starpilot_planner.params_memory.get_float("VisionSpeedLimitSupportSpeed") - if support_count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - usable_vision_limit) > VISION_SUPPORT_SPEED_TOLERANCE: - usable_vision_limit = 0 + selected_speed = raw_set_speed_kph * CV.KPH_TO_MS if 0 < raw_set_speed_kph < V_CRUISE_UNSET else 0.0 + reference_speed = selected_speed if selected_speed > 0 else max(float(v_ego), 0.0) + if (limit > 0 and reference_speed > 0 and not sm["carState"].standstill and + abs(limit - reference_speed) >= VISION_LARGE_REFERENCE_SPEED_DELTA): + memory = self.starpilot_planner.params_memory + count = memory.get_int("VisionSpeedLimitSupportCount") + support_speed = memory.get_float("VisionSpeedLimitSupportSpeed") + if count < VISION_LARGE_SET_SPEED_MIN_SUPPORT or abs(support_speed - limit) > VISION_SUPPORT_SPEED_TOLERANCE: + return 0.0 + return limit - configured_priorities = { - self.starpilot_toggles.speed_limit_priority1, - self.starpilot_toggles.speed_limit_priority2, - } - limits = { - "Dashboard": dashboard_speed_limit, - "Map Data": self.map_speed_limit, - } - if "Vision" in configured_priorities: - limits["Vision"] = usable_vision_limit - filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1} - - if self.starpilot_toggles.speed_limit_priority_highest: - desired_source = max(filtered_limits, key=filtered_limits.get, default="None") - desired_target = filtered_limits.get(desired_source, 0) - - elif self.starpilot_toggles.speed_limit_priority_lowest: - desired_source = min(filtered_limits, key=filtered_limits.get, default="None") - desired_target = filtered_limits.get(desired_source, 0) - - elif filtered_limits: - for priority in [ - self.starpilot_toggles.speed_limit_priority1, - self.starpilot_toggles.speed_limit_priority2 - ]: - if priority in filtered_limits: - desired_source = priority - desired_target = filtered_limits[desired_source] - break - else: - desired_source = "None" - desired_target = 0 - - else: - desired_source = "None" - desired_target = 0 - - if desired_target == 0: - if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.starpilot_toggles.slc_mapbox_filler: - self.get_mapbox_speed_limit(now, time_validated, v_ego, sm) - - if self.mapbox_limit >= 1: - desired_source = "Mapbox" - desired_target = self.mapbox_limit - - if not display_only and desired_target == 0: - previous_vision_limit_filtered = self.previous_source == "Vision" and self.low_vision_limit_filtered(self.previous_target) - if self.previous_target > 0 and self.starpilot_toggles.slc_fallback_previous_speed_limit and not previous_vision_limit_filtered: - desired_source = self.previous_source - desired_target = self.previous_target - - self.target = desired_target - - elif sm["selfdriveState"].enabled and self.starpilot_toggles.slc_fallback_set_speed: - desired_source = "None" - desired_target = v_cruise - else: - self.mapbox_limit = 0 - self.segment_distance = 0 - - if display_only: - self.speed_limit_changed_timer = 0 - self.unconfirmed_speed_limit = 0 - self.clear_override() - - if desired_target >= 1: - self.source = desired_source - self.target = desired_target - else: - self.source = "None" - self.target = 0 - - return - - current_road_name = sm["mapdOut"].roadName if desired_source == "Map Data" else "" - current_speed = self.target if (self.source != "None" and self.target > 0) else self.last_valid_limit - - # Do not trigger alerts when shifting to fallback or when re-obtaining the same speed limit - is_fallback = desired_source == "None" or desired_target == 0 - same_speed = desired_target > 0 and current_speed > 0 and abs(desired_target - current_speed) < 1 - confirmation_required = self._confirmation_required(desired_source, desired_target) - denied_same_limit = ( - confirmation_required and self.denied_target > 0 and - abs(desired_target - self.denied_target) < 1 - ) - - if not denied_same_limit: - self.denied_target = 0 - - if denied_same_limit: - self.speed_limit_changed_timer = 0 - self.unconfirmed_speed_limit = 0 - elif not is_fallback and not same_speed and (abs(desired_target - self.previous_target) >= 1 or current_speed == 0): - self.handle_limit_change(desired_source, desired_target, current_road_name, v_ego, sm) - else: - self.speed_limit_changed_timer = 0 - self.unconfirmed_speed_limit = 0 - if desired_source != self.source or desired_target != self.target: - if not is_fallback: - self.clear_persistent_override_for_limit_change(current_speed, desired_target) - self.source = desired_source - self.target = desired_target - if desired_source != "None" and desired_target > 0: - self.previous_source = desired_source - self.previous_target = desired_target - if current_road_name != self.previous_road_name and current_road_name != "": - self.previous_road_name = current_road_name - self.denied_target = 0 - - if self.source != "None" and self.target > 0: - self.last_valid_limit = self.target - - self._slc_adopt_counter += 1 - if self._slc_adopt_counter % 4 == 0 and self.starpilot_planner.params_memory.get_bool("SLCAdoptSpeedLimit"): - self.starpilot_planner.params_memory.remove("SLCAdoptSpeedLimit") - if desired_target > 0: - self.clear_override() - self.denied_target = 0 - self.source = desired_source - self.target = desired_target - self.previous_source = desired_source - self.previous_target = desired_target - self.speed_limit_changed_timer = 0 - self.unconfirmed_speed_limit = 0 - self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(self.target)) - self.starpilot_planner.params_memory.put_float("SLCForceCruiseSpeed", self.target + self.offset) - - def update_map_speed_limit(self, v_ego, sm): - next_speed_limit_distance = sm["mapdOut"].nextSpeedLimitDistance - - way_sel = sm["mapdOut"].waySelectionType - if way_sel in (custom.WaySelectionType.current, - custom.WaySelectionType.extended): - self.map_speed_limit = sm["mapdOut"].speedLimit - self.next_speed_limit = sm["mapdOut"].nextSpeedLimit - elif way_sel in (custom.WaySelectionType.predicted, - custom.WaySelectionType.possible): - speed = sm["mapdOut"].speedLimit + def _update_map_speed_limit(self, v_ego, sm): + map_data = sm["mapdOut"] + way_sel = map_data.waySelectionType + if way_sel in (custom.WaySelectionType.current, custom.WaySelectionType.extended): + self.map_speed_limit = map_data.speedLimit + self.next_speed_limit = map_data.nextSpeedLimit + elif way_sel in (custom.WaySelectionType.predicted, custom.WaySelectionType.possible): + speed = map_data.speedLimit if speed > 0 and (self.map_speed_limit == 0 or speed < self.map_speed_limit): self.map_speed_limit = speed - self.next_speed_limit = 0 + self.next_speed_limit = 0.0 else: - self.next_speed_limit = 0 + # Explicit selection failure means the old current limit is no longer live. + self.map_speed_limit = 0.0 + self.next_speed_limit = 0.0 if self.next_speed_limit > 0: if self.map_speed_limit < self.next_speed_limit: - max_lookahead = self.starpilot_toggles.map_speed_lookahead_higher * v_ego + lookahead = self.starpilot_toggles.map_speed_lookahead_higher * v_ego elif self.map_speed_limit > self.next_speed_limit: - max_lookahead = self.starpilot_toggles.map_speed_lookahead_lower * v_ego + lookahead = self.starpilot_toggles.map_speed_lookahead_lower * v_ego else: - max_lookahead = 0 - - if next_speed_limit_distance < max_lookahead: + lookahead = 0.0 + if map_data.nextSpeedLimitDistance < lookahead: self.map_speed_limit = self.next_speed_limit - def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm): - # Detect +/- changes on the raw set speed (button-driven, no cluster jitter). A fresh edge - # keeps a cleared override from re-arming while the selected speed stays high. - prev_v_cruise = self._prev_v_cruise - self._prev_v_cruise = v_cruise - set_speed_changed = prev_v_cruise is not None and abs(v_cruise - prev_v_cruise) > SET_SPEED_RAISE_EPS - set_speed_raised = prev_v_cruise is not None and v_cruise > prev_v_cruise + SET_SPEED_RAISE_EPS - set_speed_input_consumed = self._set_speed_override_input_consumed - # The button and its vCruise update can arrive in adjacent frames. Clear a consumed - # confirmation only after this frame has seen the speed change or button release. - if set_speed_input_consumed and (set_speed_changed or not sm["starpilotCarState"].accelPressed): - self._set_speed_override_input_consumed = False + def _select_limit(self, dashboard, map_limit, vision_limit): + priorities = (self.starpilot_toggles.speed_limit_priority1, self.starpilot_toggles.speed_limit_priority2) + limits = {SOURCE_DASHBOARD: dashboard, SOURCE_MAP: map_limit} + if SOURCE_VISION in priorities: + limits[SOURCE_VISION] = vision_limit + valid = {source: limit for source, limit in limits.items() if limit >= 1} + if not valid: + return SOURCE_NONE, 0.0 + if self.starpilot_toggles.speed_limit_priority_highest: + source = max(valid, key=valid.get) + elif self.starpilot_toggles.speed_limit_priority_lowest: + source = min(valid, key=valid.get) + else: + source = next((name for name in priorities if name in valid), SOURCE_NONE) + return source, valid.get(source, 0.0) - if not sm["selfdriveState"].enabled: - self.override_disable_timer += DT_MDL - if self.override_disable_timer >= SLC_OVERRIDE_DISABLE_CLEAR_TIME: - self.clear_override() + def _apply_mapbox_filler(self, source, limit, now, time_validated, v_ego, sm): + if source != SOURCE_NONE or not self.starpilot_toggles.slc_mapbox_filler: + self.mapbox.reset() + return source, limit + self.mapbox.update( + now, time_validated, v_ego, self.starpilot_planner.gps_valid, self.starpilot_planner.gps_position, + sm["carState"].steeringAngleDeg, sm["liveParameters"].angleOffsetDeg, + ) + if self.mapbox.limit >= 1: + return SOURCE_MAPBOX, self.mapbox.limit + return source, limit + + def _apply_fallback(self, v_cruise, enabled): + self._using_experimental_fallback = False + self._using_previous_limit_fallback = False + previous_vision_filtered = self.last_valid_source == SOURCE_VISION and self.low_vision_limit_filtered(self.last_valid_limit) + if self.starpilot_toggles.slc_fallback_previous_speed_limit and self.last_valid_limit > 0 and not previous_vision_filtered: + self.source = self.last_valid_source + self.target = self.last_valid_limit + self._using_previous_limit_fallback = True + elif enabled and self.starpilot_toggles.slc_fallback_set_speed: + self.source = SOURCE_NONE + self.target = v_cruise + else: + self.source = SOURCE_NONE + self.target = 0.0 + self._using_experimental_fallback = bool(self.starpilot_toggles.slc_fallback_experimental_mode) + + def _confirmation_required(self, limit): + current = self.last_valid_limit + return ((limit < current and self.starpilot_toggles.speed_limit_confirmation_lower) or + (limit > current and self.starpilot_toggles.speed_limit_confirmation_higher)) + + def _clear_pending(self): + self.pending_limit = 0.0 + self.pending_source = SOURCE_NONE + self.confirmation_time = 0.0 + + def _reconcile_set_speed_override(self, old_limit, new_limit): + if self.set_speed_override <= 0 or old_limit <= 0 or new_limit <= 0 or abs(new_limit - old_limit) < 0.1: + return + if (new_limit < old_limit or + self.set_speed_override <= new_limit + self.get_offset(new_limit) + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND): + self.set_speed_override = 0.0 + self.overridden_speed = self.pedal_override + + def _accept_limit(self, source, limit, *, persist=True): + assert source in REAL_SOURCES and limit >= 1 + old_limit = self.last_valid_limit + self._reconcile_set_speed_override(old_limit, limit) + self.source = source + self.target = limit + self.last_valid_limit = limit + self.last_valid_source = source + self.denied_limit = 0.0 + self._clear_pending() + if persist and abs(limit - old_limit) >= 0.1: + self.starpilot_planner.params.put_nonblocking("PreviousSpeedLimit", float(limit)) + + def _reject_limit(self, limit): + self.denied_limit = limit + self.source = SOURCE_NONE + self._clear_pending() + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + + def _update_limit(self, source, limit, sm): + road_name = sm["mapdOut"].roadName if source == SOURCE_MAP else "" + if road_name and road_name != self.previous_road_name: + self.denied_limit = 0.0 + self.previous_road_name = road_name + + if source == SOURCE_NONE: + self._clear_pending() + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") return - self.override_disable_timer = 0.0 + current = self.last_valid_limit + if current > 0 and abs(limit - current) < SAME_LIMIT_TOLERANCE: + self._accept_limit(source, limit, persist=False) + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + return - target_to_use = self.target_to_use - target_with_offset = target_to_use + self.get_offset(target_to_use) + confirmation_required = self._confirmation_required(limit) + if self.denied_limit > 0 and abs(limit - self.denied_limit) < SAME_LIMIT_TOLERANCE: + if confirmation_required: + self._clear_pending() + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + self.source = SOURCE_NONE + return + # Turning confirmation off applies the already-announced candidate. + self._accept_limit(source, limit) + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + return + self.denied_limit = 0.0 - set_speed = v_cruise + v_cruise_diff - bidirectional_set_speed = getattr(self.starpilot_toggles, "redneck_cruise", False) - - if self._persistent_override_speed > 0: - if bidirectional_set_speed: - if set_speed <= 0: - self.clear_persistent_override() - else: - self._persistent_override_speed = set_speed - elif set_speed <= 0 or (target_with_offset > 0 and set_speed <= target_with_offset and (self.source != "None" or set_speed_changed)): - self.clear_persistent_override() - else: - self._persistent_override_speed = set_speed - elif ( - target_with_offset > 0 - and set_speed > 0 - and not set_speed_input_consumed - and ((bidirectional_set_speed and set_speed_changed) or (not bidirectional_set_speed and set_speed_raised and set_speed > target_with_offset)) - ): - self._persistent_override_speed = set_speed - - if sm["carState"].gasPressed and v_ego > target_with_offset > 0: - self.override_slc = True - self.overridden_speed = v_ego + v_ego_diff - elif self._persistent_override_speed > 0: - self.override_slc = True - self.overridden_speed = self._persistent_override_speed + if self.pending_limit == 0 or abs(limit - self.pending_limit) >= SAME_LIMIT_TOLERANCE: + self.pending_limit = limit + self.pending_source = source + self.confirmation_time = 0.0 + self.limit_change_started = True + new_pending = True else: - self.clear_override() + self.pending_source = source + new_pending = False + + if not confirmation_required: + self._accept_limit(source, limit) + self.starpilot_planner.params_memory.remove("SpeedLimitAccepted") + return + + self.source = SOURCE_NONE + self.confirmation_time += DT_MDL + long_active = sm["carControl"].longActive + accel_accept = bool(sm["starpilotCarState"].accelPressed and long_active) + if new_pending and accel_accept: + # This press cannot confirm a candidate that was replaced on this frame. + self.confirmation_button_consumed = True + self.consume_set_speed_change = True + memory = self.starpilot_planner.params_memory + ui_accept = memory.get_bool("SpeedLimitAccepted") + if ui_accept: + memory.remove("SpeedLimitAccepted") + + fully_disengaged = not long_active and not sm["selfdriveState"].enabled + if ((accel_accept or ui_accept) and not new_pending) or fully_disengaged: + pending_limit, pending_source = self.pending_limit, self.pending_source + higher = pending_limit > current + self._accept_limit(pending_source, pending_limit) + if accel_accept: + self.consume_set_speed_change = True + self.confirmation_button_consumed = True + set_speed_kph = float(sm["carState"].vCruise) + target_with_offset = self.target + self.offset + if (higher and long_active and 0 < set_speed_kph < V_CRUISE_UNSET and + set_speed_kph * CV.KPH_TO_MS < target_with_offset): + memory.put_float("SLCForceCruiseSpeed", target_with_offset) + elif sm["starpilotCarState"].decelPressed or (self.confirmation_time >= 30 and long_active): + self._reject_limit(self.pending_limit) + + def _process_adopt_request(self, source, limit): + memory = self.starpilot_planner.params_memory + if not memory.get_bool("SLCAdoptSpeedLimit"): + return + memory.remove("SLCAdoptSpeedLimit") + if source not in REAL_SOURCES or limit < 1: + return + self.clear_override() + self.consume_set_speed_change = True + self._accept_limit(source, limit) + memory.put_float("SLCForceCruiseSpeed", self.target + self.offset) + + def clear_override(self): + self.set_speed_override = 0.0 + self.pedal_override = 0.0 + self.overridden_speed = 0.0 + + def _update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm): + previous = self.previous_set_speed + self.previous_set_speed = v_cruise + changed = previous is not None and abs(v_cruise - previous) > SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND + raised = previous is not None and v_cruise > previous + SET_SPEED_CHANGE_TOLERANCE_METERS_PER_SECOND + consumed = self.consume_set_speed_change + if consumed and (changed or not sm["starpilotCarState"].accelPressed): + self.consume_set_speed_change = False + + if not sm["selfdriveState"].enabled: + self.override_disable_time += DT_MDL + if self.override_disable_time >= SLC_OVERRIDE_DISABLE_CLEAR_TIME: + self.clear_override() + return + self.override_disable_time = 0.0 + + target = self.target_to_use + target_with_offset = target + self.get_offset(target) + set_speed = v_cruise + v_cruise_diff + bidirectional = getattr(self.starpilot_toggles, "redneck_cruise", False) + if self.set_speed_override > 0: + if bidirectional: + self.set_speed_override = max(set_speed, 0.0) + elif set_speed <= 0 or (target_with_offset > 0 and set_speed <= target_with_offset and + (self.source != SOURCE_NONE or changed)): + self.set_speed_override = 0.0 + else: + self.set_speed_override = set_speed + elif (target_with_offset > 0 and set_speed > 0 and not consumed and + ((bidirectional and changed) or (not bidirectional and raised and set_speed > target_with_offset))): + self.set_speed_override = set_speed + + self.pedal_override = v_ego + v_ego_diff if sm["carState"].gasPressed and v_ego > target_with_offset > 0 else 0.0 + self.overridden_speed = self.pedal_override or self.set_speed_override + + def update(self, dashboard_speed_limit, now, time_validated, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, + *, active=True, display_only=False): + self.limit_change_started = False + self.confirmation_button_consumed = False + self._using_experimental_fallback = False + self._using_previous_limit_fallback = False + mode = "display" if display_only else "active" if active else "off" + if mode != self._mode: + self.mapbox.reset() + self._mode = mode + if not active and not display_only: + self.reset_control_state() + self.mapbox.reset() + self.source, self.target = SOURCE_NONE, 0.0 + self.map_speed_limit = self.next_speed_limit = self.vision_limit = 0.0 + return + + self._update_map_speed_limit(v_ego, sm) + vision_limit = self._get_vision_limit(v_ego, sm, display_only) + source, limit = self._select_limit(dashboard_speed_limit, self.map_speed_limit, vision_limit) + source, limit = self._apply_mapbox_filler(source, limit, now, time_validated, v_ego, sm) + + if display_only: + self.reset_control_state() + self.source, self.target = (source, limit) if limit >= 1 else (SOURCE_NONE, 0.0) + return + + self._active_control = True + if source == SOURCE_NONE: + self._update_limit(source, limit, sm) + self._apply_fallback(v_cruise, sm["selfdriveState"].enabled) + else: + self._update_limit(source, limit, sm) + self._process_adopt_request(source, limit) + self._update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm) diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index 6e0e963eb4..99549a7c65 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -216,7 +216,6 @@ class StarPilotAcceleration: effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), - getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), @@ -271,7 +270,6 @@ class StarPilotAcceleration: v_ego_diff = v_ego_cluster - v_ego effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), - getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), @@ -312,7 +310,6 @@ class StarPilotAcceleration: v_ego_cluster = v_ego effective_slc_target = get_active_slc_control_target( getattr(starpilot_toggles, "speed_limit_controller", False), - getattr(starpilot_toggles, "set_speed_limit", False), getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0), getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0), getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0), diff --git a/starpilot/controls/lib/starpilot_events.py b/starpilot/controls/lib/starpilot_events.py index 37a826250a..b0ffe6a7f8 100644 --- a/starpilot/controls/lib/starpilot_events.py +++ b/starpilot/controls/lib/starpilot_events.py @@ -189,7 +189,7 @@ class StarPilotEvents: else: self.events.add(StarPilotEventName.openpilotCrashed) - if self.starpilot_planner.starpilot_vcruise.slc.speed_limit_changed_timer == DT_MDL and starpilot_toggles.speed_limit_changed_alert: + if self.starpilot_planner.starpilot_vcruise.slc.limit_change_started and starpilot_toggles.speed_limit_changed_alert: self.events.add(StarPilotEventName.speedLimitChanged) self.startup_seen |= sm["starpilotSelfdriveState"].alertText1 == starpilot_toggles.startup_alert_top and sm["starpilotSelfdriveState"].alertText2 == starpilot_toggles.startup_alert_bottom diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 7b3a39d52e..1db29302d7 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -105,10 +105,9 @@ def get_lead_veto_distance(car_params): return LEAD_VETO_M_OVERRIDES.get(fingerprint, LEAD_VETO_M) -def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, +def get_active_slc_control_target(speed_limit_controller, slc_target, slc_offset, overridden_speed, v_ego_diff, allow_lower_override=False): - # `SetSpeedLimit` only controls engage-time set-speed initialization. Ongoing - # SLC speed matching must remain active whenever Speed Limit Controller is on. + # SetSpeedLimit controls engage-time initialization; SLC limits ongoing cruise. if not speed_limit_controller: return 0.0 @@ -209,6 +208,7 @@ class StarPilotVCruise: self._nav_instruction_state_raw = None self._nav_instruction_state = {} self._applied_slc_control_target = 0.0 + self.slc_is_limiting_max_set = False self.csc_controlling_speed = False self.csc_glow_release_timer = 0.0 self.csc_override = False @@ -343,6 +343,7 @@ class StarPilotVCruise: # ===== Main update ===== def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles): + self.slc_is_limiting_max_set = False if not controls_enabled or not getattr(starpilot_toggles, "speed_limit_controller", False): self._applied_slc_control_target = 0.0 @@ -568,6 +569,18 @@ class StarPilotVCruise: v_ego_cluster = max(sm["carState"].vEgoCluster, v_ego) v_ego_diff = v_ego_cluster - v_ego + # Resolve this frame's SLC confirmation before CSC can consume accel/+. + self.slc.starpilot_toggles = starpilot_toggles + slc_active = starpilot_toggles.speed_limit_controller + slc_display_only = not slc_active and starpilot_toggles.show_speed_limits + self.slc.update( + sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated, + v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm, + active=slc_active, display_only=slc_display_only, + ) + self.slc_offset = self.slc.offset if slc_active else 0 + self.slc_target = self.slc.target if (slc_active or slc_display_only) else 0 + # Curve Speed Controller following_lead = bool(getattr(self.starpilot_planner.starpilot_following, "following_lead", False)) manual_speed_control = is_manual_speed_control(sm) @@ -586,8 +599,9 @@ class StarPilotVCruise: not self.starpilot_planner.driving_in_curve) csc_was_controlling = self.csc_controlling_speed - slc_confirmation_pending = self.slc.speed_limit_changed_timer > DT_MDL and self.slc.unconfirmed_speed_limit >= 1 - csc_accel_button = bool(sm["starpilotCarState"].accelPressed) and not slc_confirmation_pending + csc_accel_button = (bool(sm["starpilotCarState"].accelPressed) and + not self.slc.confirmation_pending and + not self.slc.confirmation_button_consumed) @@ -637,24 +651,6 @@ class StarPilotVCruise: self.csc.handle_override(v_ego, csc_was_controlling, sm, accel_button=csc_accel_button) self.csc.log_data(v_ego, sm) - # Pfeiferj's Speed Limit Controller - self.slc.starpilot_toggles = starpilot_toggles - - if starpilot_toggles.speed_limit_controller: - self.slc.update_limits(sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm) - self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm) - - self.slc_offset = self.slc.offset - self.slc_target = self.slc.target - elif starpilot_toggles.show_speed_limits: - self.slc.update_limits(sm["starpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm, display_only=True) - - self.slc_offset = 0 - self.slc_target = self.slc.target - else: - self.slc_offset = 0 - self.slc_target = 0 - self.nav_turn_target = self._get_nav_turn_control_target(v_cruise, sm, starpilot_toggles) # Single tuning knob (signed feet -> meters). Defense clamp on top of UI bounds. @@ -750,7 +746,6 @@ class StarPilotVCruise: targets.append(self.csc_target) slc_control_target = get_active_slc_control_target( starpilot_toggles.speed_limit_controller, - getattr(starpilot_toggles, "set_speed_limit", False), self.slc_target, self.slc_offset, self.slc.overridden_speed, @@ -766,6 +761,8 @@ class StarPilotVCruise: self.slc.overridden_speed > 0.0, getattr(self.slc, "source", "None"), ) + # Publish the semantic used by the UI after the lead-drop adjustment. + self.slc_is_limiting_max_set = bool(controls_enabled and 0 < slc_control_target < v_cruise) self._applied_slc_control_target = slc_control_target if slc_control_target > 0.0 else 0.0 if slc_control_target > 0.0: targets.append(slc_control_target) diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 8a3d921a53..a05490f675 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -4,7 +4,7 @@ from opendbc.car.chrysler.values import pacifica_hybrid_aol_requires_set_press from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, HyundaiFlags from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.common.params import Params -from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType +from openpilot.selfdrive.car.cruise import CRUISE_LONG_PRESS, ButtonType, is_speed_limit_confirmation_pending from openpilot.selfdrive.selfdrived.events import ET from openpilot.starpilot.common.experimental_state import ( @@ -57,6 +57,8 @@ class StarPilotCard: self.params_memory = Params(memory=True) self.accel_pressed = False + self.confirmation_button_suppressed = set() + self.pressed_accel_buttons = set() self.always_on_lateral_allowed = False self.controller_aol_override = None self.pacifica_aol_set_seen = False @@ -271,6 +273,23 @@ class StarPilotCard: ] button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents] + accel_button_types = (int(ButtonType.accelCruise), int(ButtonType.resumeCruise)) + confirmation_pending = is_speed_limit_confirmation_pending(sm["starpilotPlan"]) + if confirmation_pending: + self.confirmation_button_suppressed.update(self.pressed_accel_buttons) + suppressed_releases = set() + for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False): + if be_type not in accel_button_types: + continue + if be.pressed: + self.pressed_accel_buttons.add(be_type) + if confirmation_pending: + self.confirmation_button_suppressed.add(be_type) + else: + self.pressed_accel_buttons.discard(be_type) + if be_type in self.confirmation_button_suppressed: + self.confirmation_button_suppressed.remove(be_type) + suppressed_releases.add(be_type) button_aol_supported = self.always_on_lateral_supported and ( self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol ) @@ -392,8 +411,11 @@ class StarPilotCard: if not self.always_on_lateral_supported: self.always_on_lateral_allowed = False - if sm.updated["starpilotPlan"] or any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types): - self.accel_pressed = any(be_type in (ButtonType.accelCruise, ButtonType.resumeCruise) for be_type in button_event_types) + if sm.updated["starpilotPlan"] or any(be_type in accel_button_types for be_type in button_event_types): + self.accel_pressed = any( + be_type in accel_button_types and (be.pressed or be_type not in suppressed_releases) + for be, be_type in zip(carState.buttonEvents, button_event_types, strict=False) + ) if sm.updated["starpilotPlan"] or any(be_type == ButtonType.decelCruise for be_type in button_event_types): self.decel_pressed = any(be_type == ButtonType.decelCruise for be_type in button_event_types) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 7eda736be1..8680e729e5 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -382,7 +382,9 @@ class StarPilotPlanner: starpilotPlan.slcSpeedLimit = self.starpilot_vcruise.slc_target starpilotPlan.slcSpeedLimitOffset = self.starpilot_vcruise.slc_offset starpilotPlan.slcSpeedLimitSource = self.starpilot_vcruise.slc.source - starpilotPlan.speedLimitChanged = self.starpilot_vcruise.slc.speed_limit_changed_timer > DT_MDL + starpilotPlan.slcPresentedSpeedLimitSource = self.starpilot_vcruise.slc.presented_source + starpilotPlan.slcIsLimitingMaxSet = self.starpilot_vcruise.slc_is_limiting_max_set + starpilotPlan.speedLimitChanged = self.starpilot_vcruise.slc.confirmation_pending starpilotPlan.unconfirmedSlcSpeedLimit = self.starpilot_vcruise.slc.unconfirmed_speed_limit starpilotPlan.themeUpdated = theme_updated diff --git a/starpilot/controls/tests/test_personality_longitudinal_profiles.py b/starpilot/controls/tests/test_personality_longitudinal_profiles.py index 80ae7fcdc3..95f5816020 100644 --- a/starpilot/controls/tests/test_personality_longitudinal_profiles.py +++ b/starpilot/controls/tests/test_personality_longitudinal_profiles.py @@ -39,8 +39,8 @@ sys.modules["openpilot.selfdrive.controls.lib.longitudinal_planner"] = _module( ) sys.modules["openpilot.starpilot.controls.lib.starpilot_vcruise"] = _module( "openpilot.starpilot.controls.lib.starpilot_vcruise", - get_active_slc_control_target=lambda enabled, set_speed_limit, target, offset, overridden_speed, *_args, **_kwargs: ( - float(overridden_speed or target) + float(offset) if enabled and set_speed_limit else 0.0 + get_active_slc_control_target=lambda enabled, target, offset, overridden_speed, *_args, **_kwargs: ( + float(overridden_speed or target) + float(offset) if enabled else 0.0 ), ) diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 70f579a0f4..25ee4db5d0 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -59,7 +59,7 @@ def make_sm(): "carControl": SimpleNamespace(longActive=False), "selfdriveState": SimpleNamespace(active=False, alertType=[], experimentalMode=False), "starpilotSelfdriveState": SimpleNamespace(alertType=[]), - "starpilotPlan": SimpleNamespace(lateralCheck=True), + "starpilotPlan": SimpleNamespace(lateralCheck=True, speedLimitChanged=False, unconfirmedSlcSpeedLimit=0.0), "liveCalibration": SimpleNamespace(calPerc=100), }, updated={"starpilotPlan": False}) @@ -295,6 +295,37 @@ def make_wrapped_button_event(button_type, pressed): return SimpleNamespace(type=SimpleNamespace(raw=int(button_type)), pressed=pressed) +@pytest.mark.parametrize("pending_before_press", [False, True]) +def test_slc_confirmation_release_does_not_republish_accel(monkeypatch, tmp_path, pending_before_press): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + card = spc.StarPilotCard(SimpleNamespace(brand="gm"), SimpleNamespace(alternativeExperience=0)) + sm = make_sm() + toggles = make_toggles(speed_limit_controller=True) + starpilot_car_state = SimpleNamespace(distancePressed=False) + button_type = spc.ButtonType.accelCruise + + if pending_before_press: + sm["starpilotPlan"].speedLimitChanged = True + sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 20.0 + pressed = make_car_state(button_events=[make_wrapped_button_event(button_type, True)]) + assert card.update(pressed, starpilot_car_state, sm, toggles).accelPressed + + if not pending_before_press: + sm["starpilotPlan"].speedLimitChanged = True + sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 20.0 + card.update(make_car_state(), starpilot_car_state, sm, toggles) + + sm["starpilotPlan"].speedLimitChanged = False + sm["starpilotPlan"].unconfirmedSlcSpeedLimit = 0.0 + released = make_car_state(button_events=[make_wrapped_button_event(button_type, False)]) + assert not card.update(released, starpilot_car_state, sm, toggles).accelPressed + assert not card.confirmation_button_suppressed + + assert card.update(pressed, starpilot_car_state, sm, toggles).accelPressed + assert card.update(released, starpilot_car_state, sm, toggles).accelPressed + + @pytest.mark.parametrize( ("car_fingerprint", "expect_normalized_release"), (