diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py index 9b31b07d24..8781782f67 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/__init__.py @@ -6,10 +6,6 @@ DEBUG = True PARAMS_UPDATE_PERIOD = 3. # secs. Time between parameter updates. PRE_ACTIVE_GUARD_PERIOD = 5. # secs. Time to wait after activation before considering temp deactivation signal. -# Lookup table for speed limit percent offset depending on speed. -LIMIT_PERC_OFFSET_V = [0.1, 0.05, 0.038] # 55, 105, 135 km/h -LIMIT_PERC_OFFSET_BP = [13.9, 27.8, 36.1] # 50, 100, 130 km/h - # Constants for Limit controllers. LIMIT_ADAPT_ACC = -1. # m/s^2 Ideal acceleration for the adapting (braking) phase when approaching speed limits. LIMIT_MIN_ACC = -1.5 # m/s^2 Maximum deceleration allowed for limit controllers to provide. diff --git a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py index 4a460b3335..414981a80b 100644 --- a/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py +++ b/sunnypilot/selfdrive/controls/lib/speed_limit_controller/common.py @@ -20,6 +20,6 @@ class Engage(IntEnum): class OffsetType(IntEnum): - default = 0 + off = 0 fixed = 1 percentage = 2 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 86a3355e51..862dc71d2e 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 @@ -11,9 +11,8 @@ 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.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_MAX_SET_SPEED, \ - CRUISE_SPEED_TOLERANCE +from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import 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 from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.helpers import description_for_state, debug @@ -127,8 +126,8 @@ class SpeedLimitController: return V_CRUISE_UNSET def get_offset(self, offset_type: OffsetType, offset_value: int) -> float: - if offset_type == OffsetType.default: - return float(np.interp(self._speed_limit, LIMIT_PERC_OFFSET_BP, LIMIT_PERC_OFFSET_V) * self._speed_limit) + if offset_type == OffsetType.off: + return 0 elif offset_type == OffsetType.fixed: return offset_value * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) elif offset_type == OffsetType.percentage: 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 9d1dd07f56..543c02cd59 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 @@ -18,6 +18,13 @@ from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller import S from openpilot.sunnypilot.selfdrive.controls.lib.speed_limit_controller.speed_limit_controller import SpeedLimitController, ACTIVE_STATES from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP +SPEED_LIMITS = { + 'residential': 25 * CV.MPH_TO_MS, # 25 mph + 'city': 35 * CV.MPH_TO_MS, # 35 mph + 'highway': 65 * CV.MPH_TO_MS, # 65 mph + 'freeway': 80 * CV.MPH_TO_MS, # 80 mph +} + class TestSpeedLimitController: @@ -30,10 +37,23 @@ class TestSpeedLimitController: sunnypilot_interfaces.setup_interfaces(CI, self.params) return CI + def teardown_method(self, method): + self.reset_state() def reset_state(self): + self.reset_custom_params() self.slc.state = SpeedLimitControlState.disabled self.slc.frame = -1 + self.slc.last_op_engaged_frame = 0.0 + self.slc.op_engaged = False + self.slc.op_engaged_prev = False + self.slc.initial_max_set = False + self.slc._speed_limit = 0. + self.slc.speed_limit_prev = 0. + self.slc.last_valid_speed_limit_offsetted = 0. + self.slc._distance = 0. + self.slc._source = Source.none + self.events_sp.clear() def setup_method(self): self.params = Params() @@ -48,13 +68,30 @@ class TestSpeedLimitController: self.params.put("SpeedLimitOffsetType", 0) self.params.put("SpeedLimitValueOffset", 0) + def test_initial_state(self): + assert self.slc.state == SpeedLimitControlState.disabled + assert not self.slc.is_enabled + assert not self.slc.is_active + assert self.slc.final_cruise_speed == V_CRUISE_UNSET + def test_disabled(self): self.params.put_bool("SpeedLimitControl", False) - 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, 50 * CV.MPH_TO_MS, 0, Source.none, self.events_sp) - assert v_cruise_slc == V_CRUISE_UNSET - assert self.slc.state == SpeedLimitControlState.disabled + for _ in range(int(10. / DT_MDL)): + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + assert self.slc.state == SpeedLimitControlState.disabled + + def test_transition_disabled_to_preactive(self): + for _ in range(int(3. / DT_MDL)): + _ = self.slc.update(True, SPEED_LIMITS['city'], 0, SPEED_LIMITS['highway'], SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + assert self.slc.state == SpeedLimitControlState.preActive + assert self.slc.is_enabled and not self.slc.is_active + + def test_preactive_to_active_with_max_speed_confirmation(self): + self.slc.state = SpeedLimitControlState.preActive + v_cruise_slc = self.slc.update(True, SPEED_LIMITS['city'], 0, REQUIRED_INITIAL_MAX_SET_SPEED, SPEED_LIMITS['city'], 0, Source.car_state, self.events_sp) + assert self.slc.state == SpeedLimitControlState.active + assert self.slc.is_enabled and self.slc.is_active + assert v_cruise_slc == SPEED_LIMITS['city'] def test_no_speed_limit(self): for v_ego in np.linspace(0, 100, 101):