diff --git a/sunnypilot/mapd/live_map_data/debug.py b/sunnypilot/mapd/live_map_data/debug.py index e8a52cda08..a79e51bcf5 100644 --- a/sunnypilot/mapd/live_map_data/debug.py +++ b/sunnypilot/mapd/live_map_data/debug.py @@ -41,8 +41,8 @@ def live_map_data_sp_thread_debug(gps_location_service): _sub_master.update() v_ego = _sub_master['carState'].vEgo - _policy = Policy.car_state_priority - _resolver = SpeedLimitResolver(_policy) + _resolver = SpeedLimitResolver() + _resolver.policy = Policy.car_state_priority _resolver.update(v_ego, _sub_master) print(_resolver.speed_limit, _resolver.distance, _resolver.source) diff --git a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py index 8368268977..e6ede838ea 100644 --- a/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/sunnypilot/selfdrive/controls/lib/longitudinal_planner.py @@ -8,9 +8,7 @@ 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 @@ -23,10 +21,8 @@ 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.resolver = SpeedLimitResolver() self.dec = DynamicExperimentalController(CP, mpc) self.generation = int(model_bundle.generation) if (model_bundle := get_active_bundle()) else None @@ -37,11 +33,6 @@ 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 @@ -63,8 +54,6 @@ 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_resolver.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py index 941b601715..d92df09f7f 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 @@ -5,7 +5,8 @@ import cereal.messaging as messaging from cereal import custom from openpilot.common.gps import get_gps_location_service from openpilot.common.params import Params -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC +from openpilot.common.realtime import DT_MDL +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_MAX_MAP_DATA_AGE, LIMIT_ADAPT_ACC, PARAMS_UPDATE_PERIOD from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Policy SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimitSource @@ -21,12 +22,15 @@ class SpeedLimitResolver: distance: float source: SpeedLimitSource - def __init__(self, policy: Policy): - self._gps_location_service = get_gps_location_service(Params()) + def __init__(self): + self.params = Params() + self.frame = -1 + + self._gps_location_service = get_gps_location_service(self.params) self._limit_solutions = {} self._distance_solutions = {} - self._policy = policy + self.policy = self.params.get("SpeedLimitControlPolicy", return_default=True) self._policy_to_sources_map = { Policy.car_state_only: [SpeedLimitSource.car], Policy.car_state_priority: [SpeedLimitSource.car, SpeedLimitSource.map], @@ -37,8 +41,13 @@ class SpeedLimitResolver: for source in ALL_SOURCES: self._reset_limit_sources(source) + def update_params(self): + if self.frame % int(PARAMS_UPDATE_PERIOD / DT_MDL) == 0: + self.policy = Policy(self.params.get("SpeedLimitControlPolicy", return_default=True)) + self.change_policy(self.policy) + def change_policy(self, policy: Policy) -> None: - self._policy = policy + self.policy = policy def _reset_limit_sources(self, source: SpeedLimitSource) -> None: self._limit_solutions[source] = 0. @@ -97,9 +106,9 @@ class SpeedLimitResolver: return speed_limit, distance, source def _get_source_solution_according_to_policy(self) -> SpeedLimitSource: - sources_for_policy = self._policy_to_sources_map[self._policy] + sources_for_policy = self._policy_to_sources_map[self.policy] - if self._policy != Policy.combined: + if self.policy != Policy.combined: # They are ordered in the order of preference, so we pick the first that's non zero for source in sources_for_policy: if self._limit_solutions[source] > 0.: @@ -116,6 +125,9 @@ class SpeedLimitResolver: def update(self, v_ego: float, sm: messaging.SubMaster) -> None: self._v_ego = v_ego + self.update_params() self._resolve_limit_sources(sm) self.speed_limit, self.distance, self.source = self._consolidate() + + self.frame += 1 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 738c144066..e01a60164d 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 @@ -68,7 +68,8 @@ class TestSpeedLimitResolverValidation: @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) def test_initial_state(self, resolver_class, policy): - resolver = resolver_class(policy) + resolver = resolver_class() + resolver.policy = policy for source in ALL_SOURCES: if source in resolver._limit_solutions: assert resolver._limit_solutions[source] == 0. @@ -76,7 +77,8 @@ class TestSpeedLimitResolverValidation: @parametrized_policies def test_resolver(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class(policy) + resolver = resolver_class() + resolver.policy = policy sm_mock = setup_sm_mock(mocker) source_speed_limit = sm_mock[sm_key].speedLimit @@ -86,7 +88,8 @@ class TestSpeedLimitResolverValidation: assert resolver.source == ALL_SOURCES[function_key] def test_resolver_combined(self, resolver_class, mocker: MockerFixture): - resolver = resolver_class(Policy.combined) + resolver = resolver_class() + resolver.policy = Policy.combined sm_mock = setup_sm_mock(mocker) socket_to_source = {'carStateSP': SpeedLimitSource.car, 'liveMapDataSP': SpeedLimitSource.map} minimum_key, minimum_speed_limit = min( @@ -100,7 +103,8 @@ class TestSpeedLimitResolverValidation: @parametrized_policies def test_parser(self, resolver_class, policy, sm_key, function_key, mocker: MockerFixture): - resolver = resolver_class(policy) + resolver = resolver_class() + resolver.policy = policy sm_mock = setup_sm_mock(mocker) source_speed_limit = sm_mock[sm_key].speedLimit @@ -112,7 +116,8 @@ class TestSpeedLimitResolverValidation: @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) def test_resolve_interaction_in_update(self, resolver_class, policy, mocker: MockerFixture): v_ego = 50 - resolver = resolver_class(policy) + resolver = resolver_class() + resolver.policy = policy sm_mock = setup_sm_mock(mocker) resolver.update(v_ego, sm_mock) @@ -124,7 +129,8 @@ class TestSpeedLimitResolverValidation: @pytest.mark.parametrize("policy", list(Policy), ids=lambda policy: policy.name) def test_old_map_data_ignored(self, resolver_class, policy, mocker: MockerFixture): - resolver = resolver_class(policy) + resolver = resolver_class() + resolver.policy = policy sm_mock = mocker.MagicMock() sm_mock['gpsLocation'].unixTimestampMillis = (time.monotonic() - 2 * LIMIT_MAX_MAP_DATA_AGE) * 1e3 resolver._get_from_map_data(sm_mock)