From 0f2db833d5ee096f0650ca0fc2ccc7d1a0922277 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 31 Aug 2025 01:05:26 -0400 Subject: [PATCH] some tests --- .../lib/speed_limit_controller/__init__.py | 2 +- .../speed_limit_controller.py | 4 +-- .../tests/test_speed_limit_controller.py | 35 ++++++++++++++++--- 3 files changed, 34 insertions(+), 7 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py index ecde484372..9b31b07d24 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py @@ -19,6 +19,6 @@ LIMIT_SPEED_OFFSET_TH = -1. # m/s Maximum offset between speed limit and curren LIMIT_MAX_MAP_DATA_AGE = 10. # s Maximum time to hold to map data, then consider it invalid inside limits controllers. # Speed Limit Control Auto mode constants -REQUIRED_INITIAL_CRUISE_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params +REQUIRED_INITIAL_MAX_SET_SPEED = 35.7632 # m/s 80 MPH # TODO-SP: customizable with params CRUISE_SPEED_TOLERANCE = 0.44704 # m/s ±1 MPH tolerance # TODO-SP: metric vs imperial FALLBACK_CRUISE_SPEED = 255.0 # m/s fallback when no speed limit available 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 b419ac3f88..91d3da83df 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 @@ -12,7 +12,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V, \ - PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_CRUISE_SPEED, \ + PARAMS_UPDATE_PERIOD, LIMIT_SPEED_OFFSET_TH, SpeedLimitControlState, PRE_ACTIVE_GUARD_PERIOD, REQUIRED_INITIAL_MAX_SET_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, Engage, OffsetType @@ -178,7 +178,7 @@ class SpeedLimitController: return Engage.auto def initial_max_set_confirmed(self) -> bool: - return abs(self.v_cruise_setpoint - REQUIRED_INITIAL_CRUISE_SPEED) <= CRUISE_SPEED_TOLERANCE + return abs(self.v_cruise_setpoint - REQUIRED_INITIAL_MAX_SET_SPEED) <= CRUISE_SPEED_TOLERANCE def detect_manual_cruise_change(self) -> bool: if not self.is_active: diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py index 849c157183..cce3406802 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/tests/test_speed_limit_controller.py @@ -10,10 +10,12 @@ from opendbc.car.car_helpers import interfaces from opendbc.car.toyota.values import CAR as TOYOTA from openpilot.common.constants import CV from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.common import Source -from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import SpeedLimitControlState, REQUIRED_INITIAL_MAX_SET_SPEED +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP @@ -29,6 +31,9 @@ class TestSpeedLimitController: return CI + def reset_state(self): + self.slc.frame = -1 + def setup_method(self): self.params = Params() self.reset_custom_params() @@ -37,7 +42,7 @@ class TestSpeedLimitController: self.slc = SpeedLimitController(CI.CP) def reset_custom_params(self): - self.params.put_bool("SpeedLimitControl", False) + self.params.put_bool("SpeedLimitControl", True) self.params.put_bool("IsMetric", False) self.params.put("SpeedLimitOffsetType", 0) self.params.put("SpeedLimitValueOffset", 0) @@ -46,7 +51,29 @@ class TestSpeedLimitController: self.params.put("SpeedLimitWarningValueOffset", 0) def test_disabled(self): + self.params.put_bool("SpeedLimitControl", False) for v_ego in np.linspace(0, 100, 101): - for source in (Source.car_state, Source.map_data): - v_cruise_slc = self.slc.update(True, v_ego, 0, 50 * CV.MS_TO_MPH, 50 * CV.MS_TO_MPH, 0, source, self.events_sp) + for _ in range(int(10. / DT_MDL)): + v_cruise_slc = self.slc.update(True, v_ego, 0, 50 * CV.MPH_TO_MS, 50 * CV.MPH_TO_MS, 0, Source.none, self.events_sp) assert v_cruise_slc == V_CRUISE_UNSET + assert self.slc.state == SpeedLimitControlState.inactive + + def test_no_speed_limit(self): + for v_ego in np.linspace(0, 100, 101): + for _ in range(int(10. / DT_MDL)): + v_cruise_slc = self.slc.update(True, v_ego, 0, 50 * CV.MPH_TO_MS, 0, 0, Source.none, self.events_sp) + assert v_cruise_slc == V_CRUISE_UNSET + assert self.slc.state not in ACTIVE_STATES + + def test_speed_limit_at_initial_max_set_speed(self): + v_cruise_slc = V_CRUISE_UNSET + speed_limit = 50 * CV.MPH_TO_MS + offset = 0 + + for source in (Source.car_state, Source.map_data): + self.reset_state() + for _ in range(int(2. / DT_MDL)): + v_cruise_slc = self.slc.update(True, 40 * CV.MPH_TO_MS, 0, REQUIRED_INITIAL_MAX_SET_SPEED, speed_limit, 0, source, self.events_sp) + offset = self.slc.get_offset(self.slc.offset_type, self.slc.offset_value) + assert self.slc.state in ACTIVE_STATES + assert v_cruise_slc == speed_limit + offset