diff --git a/.importlinter b/.importlinter index 594d75ee4aa417..f77ea24b39751e 100644 --- a/.importlinter +++ b/.importlinter @@ -33,9 +33,6 @@ ignore_imports = openpilot.selfdrive.car.ecu_addrs -> openpilot.common.swaglog openpilot.selfdrive.car.car_helpers -> openpilot.common.swaglog openpilot.selfdrive.car.car_helpers -> openpilot.system.version - openpilot.selfdrive.car.ford.carcontroller -> openpilot.selfdrive.controls.lib.drive_helpers - openpilot.selfdrive.car.gm.interface -> openpilot.selfdrive.controls.lib.drive_helpers - openpilot.selfdrive.car.honda.carcontroller -> openpilot.selfdrive.controls.lib.drive_helpers openpilot.selfdrive.car.interfaces -> openpilot.selfdrive.controls.lib.drive_helpers openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.controls.lib.latcontrol_angle openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.controls.lib.longcontrol diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 6f3bde7f0c26c3..b685c6a4354b5d 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -166,6 +166,27 @@ def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_fra return above_limit_frames, request +def apply_center_deadzone(error, deadzone): + if (error > - deadzone) and (error < deadzone): + error = 0. + return error + + +def rate_limit(new_value, last_value, dw_step, up_step): + return clip(new_value, last_value + dw_step, last_value + up_step) + + +def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, + torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: + friction_interp = interp( + apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), + [-friction_threshold, friction_threshold], + [-torque_params.friction, torque_params.friction] + ) + friction = float(friction_interp) if friction_compensation else 0.0 + return friction + + def make_can_msg(addr, dat, bus): return [addr, 0, dat, bus] diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 7be3b2ebe9d0d1..36589f34e56c92 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -5,10 +5,10 @@ from openpilot.selfdrive.car.ford import fordcan from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState VisualAlert = car.CarControl.HUDControl.VisualAlert +V_CRUISE_MAX = 145 def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index d088050482a351..1d4a6066ccc3f9 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -6,11 +6,10 @@ from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import create_button_events, get_safety_config +from openpilot.selfdrive.car import create_button_events, get_safety_config, get_friction from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel -from openpilot.selfdrive.controls.lib.drive_helpers import get_friction ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 66bd50048581c5..dc42c41e63d09f 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -3,11 +3,10 @@ from cereal import car from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker -from openpilot.selfdrive.car import DT_CTRL +from openpilot.selfdrive.car import DT_CTRL, rate_limit from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 9f1b650158e1b7..d37e171da7a8d9 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -13,9 +13,9 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip -from openpilot.selfdrive.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG +from openpilot.selfdrive.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_friction, STD_CARGO_KG from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction +from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from openpilot.selfdrive.controls.lib.events import Events from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index bd74051374a384..4c111a4fd1b45a 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -2,7 +2,7 @@ from cereal import car, log from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import clip, interp +from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL # WARNING: this value was determined based on the model's training distribution, @@ -141,16 +141,6 @@ def initialize_v_cruise(self, CS, experimental_mode: bool) -> None: self.v_cruise_cluster_kph = self.v_cruise_kph -def apply_center_deadzone(error, deadzone): - if (error > - deadzone) and (error < deadzone): - error = 0. - return error - - -def rate_limit(new_value, last_value, dw_step, up_step): - return clip(new_value, last_value + dw_step, last_value + up_step) - - def clip_curvature(v_ego, prev_curvature, new_curvature): v_ego = max(MIN_SPEED, v_ego) max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755 @@ -161,17 +151,6 @@ def clip_curvature(v_ego, prev_curvature, new_curvature): return safe_desired_curvature -def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float, - torque_params: car.CarParams.LateralTorqueTuning, friction_compensation: bool) -> float: - friction_interp = interp( - apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone), - [-friction_threshold, friction_threshold], - [-torque_params.friction, torque_params.friction] - ) - friction = float(friction_interp) if friction_compensation else 0.0 - return friction - - def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float: # ToDo: Try relative error, and absolute speed if len(modelV2.temporalPose.trans):