Refactor lateral speed calculation method

This commit abstracts the computation of minimal lateral speed into a function named 'get_min_lateral_speed'. It simplifies the calculation logic used for lane change speed minimum in 'selfdrive/car/interfaces.py'. The 'pause_lateral_speed' value has been changed from a string to an integer and directly used in the new function. This change improves code readability and modularity.

selfdrive/car/interfaces.py

@@ -18,8 +18,7 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
 from openpilot.selfdrive.car.values import PLATFORMS
-from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
-from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, get_friction
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, V_CRUISE_UNSET, get_friction, get_min_lateral_speed
 from openpilot.selfdrive.controls.lib.events import Events
 from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
 
@@ -222,7 +221,7 @@ class CarInterfaceBase(ABC):
     self.acc_mads_combo = self.param_s.get_bool("AccMadsCombo")
     self.is_metric = self.param_s.get_bool("IsMetric")
     self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause")
-    self.pause_lateral_speed = self.param_s.get("PauseLateralSpeed", encoding="utf8")
+    self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8"))
     self.prev_acc_mads_combo = False
     self.mads_event_lock = True
     self.gap_button_counter = 0
@@ -608,10 +607,9 @@ class CarInterfaceBase(ABC):
     if self.CP.openpilotLongitudinalControl:
       self.toggle_exp_mode(gap_button)
 
-    below_lateral_speed = LANE_CHANGE_SPEED_MIN if not self.pause_lateral_speed else \
-                          float(self.pause_lateral_speed) * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS)
+    lane_change_speed_min = get_min_lateral_speed(self.pause_lateral_speed, self.is_metric)
 
-    cs_out.belowLaneChangeSpeed = cs_out.vEgo < below_lateral_speed and self.below_speed_pause
+    cs_out.belowLaneChangeSpeed = cs_out.vEgo < lane_change_speed_min and self.below_speed_pause
 
     if cs_out.gearShifter in [GearShifter.park, GearShifter.reverse] or cs_out.doorOpen or \
       (cs_out.seatbeltUnlatched and cs_out.gearShifter != GearShifter.park):
@@ -716,7 +714,7 @@ class CarInterfaceBase(ABC):
     if self._frame % 100 == 0:
       self.is_metric = self.param_s.get_bool("IsMetric")
       self.below_speed_pause = self.param_s.get_bool("BelowSpeedPause")
-      self.pause_lateral_speed = self.param_s.get("PauseLateralSpeed", encoding="utf8")
+      self.pause_lateral_speed = int(self.param_s.get("PauseLateralSpeed", encoding="utf8"))
     if self._frame % 300 == 0:
       self._frame = 0
       self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam")

selfdrive/controls/lib/drive_helpers.py

@@ -5,6 +5,7 @@ from cereal import car, log, custom
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.common.realtime import DT_MDL, DT_CTRL
+from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
 # WARNING: this value was determined based on the model's training distribution,
@@ -341,3 +342,8 @@ def get_road_edge(carstate, model_v2, toggle):
     road_edge = False
 
   return road_edge
+
+
+def get_min_lateral_speed(value: int, is_metric: bool, default: float = LANE_CHANGE_SPEED_MIN):
+  speed: float = default if value == 0 else value * CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
+  return speed