VW PQ: Add PLA for testing

selfdrive/car/volkswagen/carcontroller.py

@@ -4,7 +4,7 @@
 from openpilot.common.numpy_fast import clip
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.params import Params
-from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits
+from openpilot.selfdrive.car import DT_CTRL, apply_std_steer_angle_limits
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan
 from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags
@@ -23,11 +23,13 @@ def __init__(self, dbc_name, CP, VM):
     self.packer_pt = CANPacker(dbc_name)
     self.ext_bus = CANBUS.pt if CP.networkLocation == car.CarParams.NetworkLocation.fwdCamera else CANBUS.cam
 
-    self.apply_steer_last = 0
+    self.apply_angle_last = 0
     self.gra_acc_counter_last = None
-    self.eps_timer_soft_disable_alert = False
-    self.hca_frame_timer_running = 0
-    self.hca_frame_same_torque = 0
+    self.PLA_status = 0
+    self.PLA_ESP_status = 0
+    self.PLA_entryCounter = 0
+    self.CSsteeringAngleDegLast = 0
+    self.CSLH3_SignLast = 0
     self.last_button_frame = 0
 
     self.sm = messaging.SubMaster(['longitudinalPlanSP'])
@@ -95,46 +97,33 @@ def update(self, CC, CS, now_nanos):
     # **** Steering Controls ************************************************ #
 
     if self.frame % self.CCP.STEER_STEP == 0:
-      # Logic to avoid HCA state 4 "refused":
-      #   * Don't steer unless HCA is in state 3 "ready" or 5 "active"
-      #   * Don't steer at standstill
-      #   * Don't send > 3.00 Newton-meters torque
-      #   * Don't send the same torque for > 6 seconds
-      #   * Don't send uninterrupted steering for > 360 seconds
-      # MQB racks reset the uninterrupted steering timer after a single frame
-      # of HCA disabled; this is done whenever output happens to be zero.
+      # PLA_status definitions:
+      #  8 = standby
+      #  6 = active
+      #  4 = activatable, entry request signal
 
       if CC.latActive:
-        new_steer = int(round(actuators.steer * self.CCP.STEER_MAX))
-        apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP)
-        self.hca_frame_timer_running += self.CCP.STEER_STEP
-        if self.apply_steer_last == apply_steer:
-          self.hca_frame_same_torque += self.CCP.STEER_STEP
-          if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL:
-            apply_steer -= (1, -1)[apply_steer < 0]
-            self.hca_frame_same_torque = 0
-        else:
-          self.hca_frame_same_torque = 0
-        hca_enabled = abs(apply_steer) > 0
+        self.PLA_status = 6 if self.PLA_entryCounter >= 11 else 4
+        self.PLA_ESP_status = 6 if self.PLA_entryCounter >= 32 else 4
+        self.PLA_entryCounter += 1 if self.PLA_entryCounter <= 32 else self.PLA_entryCounter
+        if CS.LH2_steeringState != 64 and self.PLA_entryCounter >= 30:
+          self.PLA_entryCounter = 0
       else:
-        hca_enabled = False
-        apply_steer = 0
+        self.PLA_status = 8
+        self.PLA_ESP_status = 8
+        self.PLA_entryCounter = 0
 
-      if not hca_enabled:
-        self.hca_frame_timer_running = 0
+      apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo, CarControllerParams) \
+        if CC.latActive and self.PLA_status == 6 else self.CSsteeringAngleDegLast
 
-      self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
-      self.apply_steer_last = apply_steer
-      can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hca_enabled))
+      self.apply_angle_last = apply_angle
+      self.CSsteeringAngleDegLast = CS.out.steeringAngleDeg
+      can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.br, apply_angle, self.PLA_status, self.PLA_ESP_status, self.CSLH3_SignLast))
+      self.CSLH3_SignLast = CS.LH_3_Sign
 
-      if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
-        # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque
-        # to the greatest of actual driver input or 2x openpilot's output (1x openpilot output is not enough to
-        # consistently reset inactivity detection on straight level roads). See commaai/openpilot#23274 for background.
-        ea_simulated_torque = clip(apply_steer * 2, -self.CCP.STEER_MAX, self.CCP.STEER_MAX)
-        if abs(CS.out.steeringTorque) > abs(ea_simulated_torque):
-          ea_simulated_torque = CS.out.steeringTorque
-        can_sends.append(self.CCS.create_eps_update(self.packer_pt, CANBUS.cam, CS.eps_stock_values, ea_simulated_torque))
+    # **** Gate_Komf Spammer ************************************************ #
+    if self.PLA_entryCounter <= 30 and CC.latActive:
+      can_sends.append(self.CCS.create_gk_spam(self.packer_pt, CANBUS.br, CS.Gate_Komf_stock))
 
     # **** Acceleration Controls ******************************************** #
 
@@ -194,12 +183,15 @@ def update(self, CC, CS, now_nanos):
         self.last_cruise_button = self.cruise_button
 
     new_actuators = actuators.as_builder()
-    new_actuators.steer = self.apply_steer_last / self.CCP.STEER_MAX
-    new_actuators.steerOutputCan = self.apply_steer_last
+    new_actuators.steeringAngleDeg = self.apply_angle_last
+    self.eps_timer_soft_disable_alert = False
+
+    CS.Gate_Komf_stock = 0
 
     self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
     self.v_set_dis_prev = self.v_set_dis
     self.frame += 1
+    self.frameLast = self.frame
     return new_actuators, can_sends
 
   # multikyd methods, sunnyhaibin logic

selfdrive/car/volkswagen/carstate.py

@@ -17,6 +17,7 @@ def __init__(self, CP):
     self.esp_hold_confirmation = False
     self.upscale_lead_car_signal = False
     self.eps_stock_values = False
+    self.LH_3_Sign = False
 
   def create_button_events(self, pt_cp, buttons):
     button_events = []
@@ -187,13 +188,17 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
     ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE
     ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD
     hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"])
-    ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status)
+    ret.steerFaultTemporary = True if pt_cp.vl["Lenkhilfe_2"]["LH2_PLA_Abbr"] == 2 else False
+    self.LH_3_Sign = pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"]
+
+    self.Gate_Komf_stock = pt_cp.vl["Gate_Komf_1"]
+    self.LH2_steeringState = pt_cp.vl["Lenkhilfe_2"]["LH2_aktLenkeingriff"]
 
     # Update gas, brakes, and gearshift.
     ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0
     ret.gasPressed = ret.gas > 0
     ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0  # FIXME: this is pressure in Bar, not sure what OP expects
-    ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremslichtschalter"])
+    ret.brakePressed = False
     ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"])
     ret.brakeLightsDEPRECATED = bool(pt_cp.vl["Motor_2"]['Bremstestschalter'])
 
@@ -241,7 +246,7 @@ def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
     # Update ACC radar status.
     self.acc_type = ext_cp.vl["ACC_System"]["ACS_Typ_ACC"]
     ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
-    ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["GRA_Status"] in (1, 2)
+    ret.cruiseState.enabled = ret.cruiseState.available
     if self.CP.pcmCruise:
       ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7)
     else:

selfdrive/car/volkswagen/interface.py

@@ -71,7 +71,7 @@ def _get_params(ret, candidate: CAR, fingerprint, car_fw, experimental_long, doc
     ret.steerLimitTimer = 0.4
     if ret.flags & VolkswagenFlags.PQ:
       ret.steerActuatorDelay = 0.2
-      CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+      ret.steerControlType = car.CarParams.SteerControlType.angle
     else:
       ret.steerActuatorDelay = 0.1
       ret.lateralTuning.pid.kpBP = [0.]

selfdrive/car/volkswagen/pqcan.py

@@ -1,12 +1,14 @@
-def create_steering_control(packer, bus, apply_steer, lkas_enabled):
+def create_steering_control(packer, bus, apply_angle, PLA_status, PLA_ESP_status, LH_3_Sign):
   values = {
-    "LM_Offset": abs(apply_steer),
-    "LM_OffSign": 1 if apply_steer < 0 else 0,
-    "HCA_Status": 7 if (lkas_enabled and apply_steer != 0) else 3,
-    "Vib_Freq": 16,
+    "PL1_Status_EPS": PLA_status,
+    "PL1_ArcAngleReq": abs(apply_angle),
+    "PL1_AngleReqSign": (1 if apply_angle < 0 else 0) if PLA_status == 6 else LH_3_Sign,
+    "PL1_Stat_PLA_ESP": PLA_ESP_status,
+    "PL1_Bremsmoment": 0,
+    "PL1_void": 0,
   }
 
-  return packer.make_can_msg("HCA_1", bus, values)
+  return packer.make_can_msg("PLA_1", bus, values)
 
 
 def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control):
@@ -56,6 +58,15 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0
   return packer.make_can_msg("GRA_Neu", bus, values)
 
 
+def create_gk_spam(packer, bus, gk_stock):
+  values = gk_stock
+  values.update({
+    "GK1_Rueckfahr": 1,
+  })
+
+  return packer.make_can_msg("Gate_Komf_1", bus, values)
+
+
 def acc_control_value(main_switch_on, acc_faulted, long_active, cruiseOverride):
   if long_active or cruiseOverride:
     acc_control = 1

selfdrive/car/volkswagen/values.py

@@ -6,7 +6,7 @@
 from panda.python import uds
 from opendbc.can.can_define import CANDefine
 from openpilot.common.conversions import Conversions as CV
-from openpilot.selfdrive.car import dbc_dict, CarSpecs, DbcDict, PlatformConfig, Platforms
+from openpilot.selfdrive.car import dbc_dict, AngleRateLimit, CarSpecs, DbcDict, PlatformConfig, Platforms
 from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
                                                      Device
 from openpilot.selfdrive.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16
@@ -39,11 +39,15 @@ class CarControllerParams:
   ACCEL_MAX = 2.0                          # 2.0 m/s max acceleration
   ACCEL_MIN = -3.5                         # 3.5 m/s max deceleration
 
+  ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 15.], angle_v=[10, 10.])
+  ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 15.], angle_v=[10, 10.])
+
   def __init__(self, CP):
     can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
 
     if CP.flags & VolkswagenFlags.PQ:
       self.LDW_STEP = 5                   # LDW_1 message frequency 20Hz
+      self.GK_STEP = 10                   # GK_1 message frequency 10Hz
       self.ACC_HUD_STEP = 4               # ACC_GRA_Anzeige frequency 25Hz
       self.STEER_DRIVER_ALLOWANCE = 80    # Driver intervention threshold 0.8 Nm
       self.STEER_DELTA_UP = 6             # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00))
@@ -74,6 +78,7 @@ def __init__(self, CP):
 
     else:
       self.LDW_STEP = 10                  # LDW_02 message frequency 10Hz
+      self.GK_STEP = 10                   # GK_1 message frequency 10Hz
       self.ACC_HUD_STEP = 6               # ACC_02 message frequency 16Hz
       self.STEER_DRIVER_ALLOWANCE = 80    # Driver intervention threshold 0.8 Nm
       self.STEER_DELTA_UP = 4             # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
@@ -110,6 +115,7 @@ def __init__(self, CP):
 
 class CANBUS:
   pt = 0
+  br = 1
   cam = 2
 
 

selfdrive/controls/lib/latcontrol.py

@@ -3,7 +3,7 @@
 from openpilot.common.numpy_fast import clip
 from openpilot.common.realtime import DT_CTRL
 
-MIN_LATERAL_CONTROL_SPEED = 0.3  # m/s
+MIN_LATERAL_CONTROL_SPEED = -0.1  # m/s
 
 
 class LatControl(ABC):