some tests

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

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:

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