Merge branch 'brand/vag/avoid-eps-lockout' into pre-vag

common/params_keys.h

@@ -146,4 +146,5 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"dp_device_ip", CLEAR_ON_MANAGER_START},
     {"dp_vag_a0_sng", PERSISTENT},
     {"dp_vag_pq_steering_patch", PERSISTENT},
+    {"dp_vag_avoid_eps_lockout", PERSISTENT},
 };

opendbc_repo/opendbc/car/structs.py

@@ -24,4 +24,5 @@ class DPFlags:
   ExtRadar = 2
   VagA0SnG = 2 ** 2
   VAGPQSteeringPatch = 2 ** 3
+  VagAvoidEPSLockout = 2 ** 4
   pass

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
@@ -44,7 +44,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:

opendbc_repo/opendbc/car/volkswagen/interface.py

@@ -93,4 +93,7 @@ class CarInterface(CarInterfaceBase):
     if ret.flags & VolkswagenFlags.PQ and dp_params & structs.DPFlags.VAGPQSteeringPatch:
       ret.flags |= VolkswagenFlags.PQSteeringPatch.value
 
+    if dp_params & structs.DPFlags.VagAvoidEPSLockout:
+      ret.flags |= VolkswagenFlags.AVOID_EPS_LOCKOUT.value
+
     return ret

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
@@ -146,6 +148,8 @@ class VolkswagenFlags(IntFlag):
 
   A0SnG = 2 ** 2
   PQSteeringPatch = 2 ** 3
+  AVOID_EPS_LOCKOUT = 2 ** 4
+
 
 @dataclass
 class VolkswagenMQBPlatformConfig(PlatformConfig):

selfdrive/car/card.py

@@ -111,6 +111,9 @@ class Car:
       if self.params.get_bool("dp_vag_pq_steering_patch"):
         dp_params |= structs.DPFlags.VAGPQSteeringPatch
 
+      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

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

selfdrive/ui/qt/offroad/dp_panel.cc

@@ -50,6 +50,11 @@ void DPPanel::add_vag_toggles() {
       tr("PQ Steering Patch"),
       ""
     },
+    {
+      "dp_vag_avoid_eps_lockout",
+      tr("Avoid EPS Lockout"),
+      "",
+    },
   };
 
   QWidget *label = nullptr;

system/manager/manager.py

@@ -63,6 +63,7 @@ def manager_init() -> None:
     ("dp_ui_radar_tracks", "0"),
     ("dp_vag_a0_sng", "0"),
     ("dp_vag_pq_steering_patch", "0"),
+    ("dp_vag_avoid_eps_lockout", "0"),
   ]
 
   if params.get_bool("RecordFrontLock"):