diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 2fcc97411a..50d1876d53 100644 --- a/selfdrive/car/interfaces.py +++ b/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") diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 61edf7aade..db63c05cde 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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