From a6ea4e31b49a01df785933d9e447ce4532c3f80f Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sat, 30 Aug 2025 23:44:20 -0400 Subject: [PATCH] split speed limit resolver out of slc --- .../controls/lib/longitudinal_planner.py | 20 ++++++++++++++++++- .../speed_limit_controller.py | 14 ++++++------- .../speed_limit_resolver.py | 4 +--- .../tests/test_speed_limit_resolver.py | 8 ++++---- 4 files changed, 30 insertions(+), 16 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 97dac727c1..56b52f6fef 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -7,8 +7,12 @@ See the LICENSE.md file in the root directory for more details. from cereal import messaging, custom from opendbc.car import structs +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL from openpilot.sunnypilot.selfdrive.controls.lib.dec.dec import DynamicExperimentalController +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.sunnypilot.models.helpers import get_active_bundle @@ -18,6 +22,11 @@ DecState = custom.LongitudinalPlanSP.DynamicExperimentalControl.DynamicExperimen class LongitudinalPlannerSP: def __init__(self, CP: structs.CarParams, mpc): self.events_sp = EventsSP() + self.params = Params() + self.frame = -1 + + self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True) + self.resolver = SpeedLimitResolver(self.policy) self.dec = DynamicExperimentalController(CP, mpc) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None @@ -28,6 +37,11 @@ class LongitudinalPlannerSP: # If we don't have a generation set, we assume it's default model. Which as of today are mlsim. return bool(self.generation is None or self.generation >= 11) + def update_params(self): + if self.frame % int(3. / DT_MDL) == 0: + self.policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True)) + self.resolver.change_policy(self.policy) + def get_mpc_mode(self) -> str | None: if not self.dec.active(): return None @@ -37,7 +51,9 @@ class LongitudinalPlannerSP: def update_v_cruise(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise: float) -> float: self.events_sp.clear() - v_cruise_slc = self.slc.update(sm, v_ego, a_ego, v_cruise, self.events_sp) + # Speed Limit Control + _speed_limit, _distance, _source = self.resolver.resolve(v_ego, sm) + v_cruise_slc = self.slc.update(sm, v_ego, a_ego, v_cruise, _speed_limit, _distance, _source, self.events_sp) v_cruise_final = min(v_cruise, v_cruise_slc) @@ -46,6 +62,8 @@ class LongitudinalPlannerSP: def update(self, sm: messaging.SubMaster) -> None: self.dec.update(sm) + self.frame += 1 + def publish_longitudinal_plan_sp(self, sm: messaging.SubMaster, pm: messaging.PubMaster) -> None: plan_sp_send = messaging.new_message('longitudinalPlanSP') diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py index 655fa36160..63c1df90c8 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py @@ -16,9 +16,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import L PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_CRUISE_SPEED, \ CRUISE_SPEED_TOLERANCE from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Policy, Engage, OffsetType +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source, Engage, OffsetType from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_resolver import SpeedLimitResolver from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP from openpilot.selfdrive.modeld.constants import ModelConstants @@ -39,8 +38,6 @@ class SpeedLimitController: def __init__(self, CP): self.params = Params() self.CP = CP - self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True) - self.resolver = SpeedLimitResolver(self.policy) self.frame = -1 self.last_op_engaged_time = 0.0 self.is_metric = self.params.get_bool("IsMetric") @@ -173,10 +170,8 @@ class SpeedLimitController: self.warning_type = self.params.get("SpeedLimitWarningType", return_default=True) self.warning_offset_type = OffsetType(self.params.get("SpeedLimitWarningOffsetType", return_default=True)) self.warning_offset_value = self.params.get("SpeedLimitWarningValueOffset", return_default=True) - self.policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True)) self.is_metric = self.params.get_bool("IsMetric") self.speed_factor = CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH - self.resolver.change_policy(self.policy) self.engage_type = self.read_engage_type_param() @staticmethod @@ -294,11 +289,14 @@ class SpeedLimitController: elif self.speed_limit_changed: events_sp.add(EventNameSP.speedLimitValueChange) - def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise_setpoint: float, events_sp: EventsSP) -> float: + def update(self, sm: messaging.SubMaster, v_ego: float, a_ego: float, v_cruise_setpoint: float, + speed_limit: float, distance: float, source: Source, events_sp: EventsSP) -> float: self.op_engaged = sm['carControl'].longActive self.current_time = time.monotonic() - self._speed_limit, self._distance, self._source = self.resolver.resolve(v_ego, self._speed_limit, sm) + self._speed_limit = speed_limit + self._distance = distance + self._source = source self.update_params() self.update_calculations(v_ego, a_ego, v_cruise_setpoint) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 43f78087b2..887f419bf4 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py @@ -13,7 +13,6 @@ class SpeedLimitResolver: _limit_solutions: dict[Source, float] # Store for speed limit solutions from different sources _distance_solutions: dict[Source, float] # Store for distance to current speed limit start for different sources _v_ego: float - _current_speed_limit: float def __init__(self, policy: Policy): self._gps_location_service = get_gps_location_service(Params()) @@ -38,9 +37,8 @@ class SpeedLimitResolver: self._limit_solutions[source] = 0. self._distance_solutions[source] = 0. - def resolve(self, v_ego: float, current_speed_limit: float, sm: messaging.SubMaster) -> tuple[float, float, Source]: + def resolve(self, v_ego: float, sm: messaging.SubMaster) -> tuple[float, float, Source]: self._v_ego = v_ego - self._current_speed_limit = current_speed_limit self._resolve_limit_sources(sm) return self._consolidate() diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py index 62a9329958..457439022a 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py @@ -79,7 +79,7 @@ class TestSpeedLimitResolverValidation: source_speed_limit = sm_mock[sm_key].speedLimit # Assert the resolver - speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock) + speed_limit, _, source = resolver.resolve(source_speed_limit, sm_mock) assert speed_limit == source_speed_limit assert source == Source[function_key] @@ -92,7 +92,7 @@ class TestSpeedLimitResolverValidation: socket_to_source.keys()), key=lambda x: x[1]) # Assert the resolver - speed_limit, _, source = resolver.resolve(minimum_speed_limit, 0, sm_mock) + speed_limit, _, source = resolver.resolve(minimum_speed_limit, sm_mock) assert speed_limit == minimum_speed_limit assert source == socket_to_source[minimum_key] @@ -103,7 +103,7 @@ class TestSpeedLimitResolverValidation: source_speed_limit = sm_mock[sm_key].speedLimit # Assert the parsing - speed_limit, _, source = resolver.resolve(source_speed_limit, 0, sm_mock) + speed_limit, _, source = resolver.resolve(source_speed_limit, sm_mock) assert resolver._limit_solutions[Source[function_key]] == source_speed_limit assert resolver._distance_solutions[Source[function_key]] == 0. @@ -113,7 +113,7 @@ class TestSpeedLimitResolverValidation: resolver = resolver_class(policy) sm_mock = setup_sm_mock(mocker) - _speed_limit, _distance, _source = resolver.resolve(v_ego, 0, sm_mock) + _speed_limit, _distance, _source = resolver.resolve(v_ego, sm_mock) # After resolution assert _speed_limit is not None