From a451513fb35adb14d3e0aff0f7207016aff6f0f8 Mon Sep 17 00:00:00 2001 From: Rick Lan Date: Mon, 12 May 2025 15:13:25 +0800 Subject: [PATCH] VAG Avoid EPS Lockout - 2025-06-13 --- common/params_keys.h | 1 + opendbc_repo/opendbc/car/structs.py | 1 + opendbc_repo/opendbc/car/volkswagen/carcontroller.py | 10 ++++++++-- opendbc_repo/opendbc/car/volkswagen/interface.py | 3 +++ opendbc_repo/opendbc/car/volkswagen/values.py | 8 ++++++-- selfdrive/car/card.py | 3 +++ selfdrive/controls/lib/latcontrol.py | 3 +++ selfdrive/ui/qt/offroad/dp_panel.cc | 5 +++++ system/manager/manager.py | 1 + 9 files changed, 31 insertions(+), 4 deletions(-) diff --git a/common/params_keys.h b/common/params_keys.h index 2ddeff6fe..b1284803e 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -144,4 +144,5 @@ inline static std::unordered_map keys = { {"dp_device_auto_shutdown_in", PERSISTENT}, {"dp_ui_radar_tracks", PERSISTENT}, {"dp_device_ip", CLEAR_ON_MANAGER_START}, + {"dp_vag_avoid_eps_lockout", PERSISTENT}, }; diff --git a/opendbc_repo/opendbc/car/structs.py b/opendbc_repo/opendbc/car/structs.py index 7389cec8b..b91050efe 100644 --- a/opendbc_repo/opendbc/car/structs.py +++ b/opendbc_repo/opendbc/car/structs.py @@ -22,4 +22,5 @@ CarParamsT = capnp.lib.capnp._StructModule class DPFlags: LateralALKA = 1 ExtRadar = 2 + VagAvoidEPSLockout = 2 ** 2 pass diff --git a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py index e2a4fe86d..53e0dbbdd 100644 --- a/opendbc_repo/opendbc/car/volkswagen/carcontroller.py +++ b/opendbc_repo/opendbc/car/volkswagen/carcontroller.py @@ -13,7 +13,7 @@ LongCtrlState = structs.CarControl.Actuators.LongControlState class CarController(CarControllerBase): def __init__(self, dbc_names, CP): super().__init__(dbc_names, CP) - self.CCP = CarControllerParams(CP) + self.CCP = CarControllerParams(CP, CP.flags & VolkswagenFlags.AVOID_EPS_LOCKOUT) self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan self.packer_pt = CANPacker(dbc_names[Bus.pt]) self.ext_bus = CANBUS.pt if CP.networkLocation == structs.CarParams.NetworkLocation.fwdCamera else CANBUS.cam @@ -43,7 +43,13 @@ class CarController(CarControllerBase): # of HCA disabled; this is done whenever output happens to be zero. if CC.latActive: - new_torque = int(round(actuators.torque * self.CCP.STEER_MAX)) + if VolkswagenFlags.AVOID_EPS_LOCKOUT: + #根據速度縮放new_torque扭力上限 + torque_scale = np.interp(CS.out.vEgo, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0]) + scaled_steer_max = self.CCP.STEER_MAX * torque_scale + new_torque = int(round(actuators.torque * scaled_steer_max)) + else: + new_torque = int(round(actuators.torque * self.CCP.STEER_MAX)) apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP) self.hca_frame_timer_running += self.CCP.STEER_STEP if self.apply_torque_last == apply_torque: diff --git a/opendbc_repo/opendbc/car/volkswagen/interface.py b/opendbc_repo/opendbc/car/volkswagen/interface.py index 92e5a9baf..ad6f0f976 100644 --- a/opendbc_repo/opendbc/car/volkswagen/interface.py +++ b/opendbc_repo/opendbc/car/volkswagen/interface.py @@ -87,4 +87,7 @@ class CarInterface(CarInterfaceBase): ret.vEgoStopping = 0.5 ret.autoResumeSng = ret.minEnableSpeed == -1 + if dp_params & structs.DPFlags.VagAvoidEPSLockout: + ret.flags |= VolkswagenFlags.AVOID_EPS_LOCKOUT.value + return ret diff --git a/opendbc_repo/opendbc/car/volkswagen/values.py b/opendbc_repo/opendbc/car/volkswagen/values.py index 7b4e5a74c..08202c3ac 100644 --- a/opendbc_repo/opendbc/car/volkswagen/values.py +++ b/opendbc_repo/opendbc/car/volkswagen/values.py @@ -27,7 +27,8 @@ class CarControllerParams: # MQB vs PQ maximums are shared, but rate-of-change limited differently # based on safety requirements driven by lateral accel testing. - STEER_MAX = 300 # Max heading control assist torque 3.00 Nm + # rick - move to init so we can overwrite it with avoid eps lockout + # STEER_MAX = 300 # Max heading control assist torque 3.00 Nm STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily STEER_DRIVER_FACTOR = 1 # from dbc @@ -40,8 +41,9 @@ class CarControllerParams: ACCEL_MAX = 2.0 # 2.0 m/s max acceleration ACCEL_MIN = -3.5 # 3.5 m/s max deceleration - def __init__(self, CP): + def __init__(self, CP, avoid_eps_lockout = False): can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt]) + self.STEER_MAX = 300 if not avoid_eps_lockout else 288 if CP.flags & VolkswagenFlags.PQ: self.LDW_STEP = 5 # LDW_1 message frequency 20Hz @@ -144,6 +146,8 @@ class VolkswagenFlags(IntFlag): # Static flags PQ = 2 + AVOID_EPS_LOCKOUT = 2 ** 2 + @dataclass class VolkswagenMQBPlatformConfig(PlatformConfig): diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index fda553b24..761e7b179 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -105,6 +105,9 @@ class Car: if self.params.get_bool("dp_lat_alka"): dp_params |= structs.DPFlags.LateralALKA + if self.params.get_bool("dp_vag_avoid_eps_lockout"): + dp_params |= structs.DPFlags.VagAvoidEPSLockout + self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params) self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP) self.CP = self.CI.CP diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d67a4aa95..f47f5a4c5 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -2,6 +2,9 @@ import numpy as np from abc import abstractmethod, ABC from openpilot.common.realtime import DT_CTRL +from openpilot.common.params import Params + +MIN_LATERAL_CONTROL_SPEED = 2.5 if Params().get_bool("dp_vag_avoid_eps_lockout") else 0.3 # m/s class LatControl(ABC): diff --git a/selfdrive/ui/qt/offroad/dp_panel.cc b/selfdrive/ui/qt/offroad/dp_panel.cc index f9d2b8b72..ebff7ae44 100644 --- a/selfdrive/ui/qt/offroad/dp_panel.cc +++ b/selfdrive/ui/qt/offroad/dp_panel.cc @@ -40,6 +40,11 @@ void DPPanel::add_vag_toggles() { QString::fromUtf8("🐉 ") + tr("VW / Audi / Skoda"), "", }, + { + "dp_vag_avoid_eps_lockout", + tr("Avoid EPS Lockout"), + "", + }, }; QWidget *label = nullptr; diff --git a/system/manager/manager.py b/system/manager/manager.py index 2cf6debc6..d6da8ce6b 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -61,6 +61,7 @@ def manager_init() -> None: ("dp_lon_no_gas_gating", "0"), ("dp_device_auto_shutdown_in", "-5"), ("dp_ui_radar_tracks", "0"), + ("dp_vag_avoid_eps_lockout", "0"), ] if params.get_bool("RecordFrontLock"):