offset default > off

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.

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

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:

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):