mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-24 09:52:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

split speed limit resolver out of slc

Browse Source
This commit is contained in:
Jason Wen
2025-08-30 23:44:20 -04:00
parent 84abd66bba
commit a6ea4e31b4
4 changed files with 30 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/longitudinal_planner.py
+19 -1
View File
@@ -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')
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_controller.py
+6 -8
View File
@@ -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)
sunnypilot/selfdrive/controls/lib/speed_limit_controller/speed_limit_resolver.py
+1 -3
View File
@@ -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()
sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_resolver.py
+4 -4
View File
@@ -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