VW: Prep for MQB longitudinal (#25777)

* VW: Prep for MQB longitudinal

* fine, be that way

* temporarily pacify the docs generator

selfdrive/car/volkswagen/carcontroller.py

@@ -7,6 +7,7 @@ from selfdrive.car.volkswagen import mqbcan, pqcan
 from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
 class CarController:
@@ -25,7 +26,6 @@ class CarController:
   def update(self, CC, CS, ext_bus):
     actuators = CC.actuators
     hud_control = CC.hudControl
-
     can_sends = []
 
     # **** Steering Controls ************************************************ #
@@ -71,9 +71,12 @@ class CarController:
     # **** Acceleration Controls ******************************************** #
 
     if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl:
-      tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
+      acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
       accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0
-      can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel))
+      stopping = actuators.longControlState == LongCtrlState.stopping
+      starting = actuators.longControlState == LongCtrlState.starting
+      can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, CC.longActive, accel,
+                                                         acc_control, stopping, starting, CS.out.cruiseState.standstill))
 
     # **** HUD Controls ***************************************************** #
 

selfdrive/car/volkswagen/carstate.py

@@ -215,6 +215,7 @@ class CarState(CarStateBase):
     ret.stockAeb = False
 
     # Update ACC radar status.
+    self.acc_type = 0  # TODO: this is ACC "basic" with nonzero min speed, support FtS (1) later
     ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
     ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"])
     if self.CP.pcmCruise:

selfdrive/car/volkswagen/interface.py

@@ -38,6 +38,7 @@ class CarInterface(CarInterfaceBase):
 
       if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)):  # Bremse_1, Lenkwinkel_1
         ret.networkLocation = NetworkLocation.gateway
+        ret.experimentalLongitudinalAvailable = True
       else:
         ret.networkLocation = NetworkLocation.fwdCamera
 
@@ -49,13 +50,6 @@ class CarInterface(CarInterfaceBase):
       # Panda ALLOW_DEBUG firmware required.
       ret.dashcamOnly = True
 
-      if experimental_long and ret.networkLocation == NetworkLocation.gateway:
-        # Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should
-        # be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required.
-        ret.experimentalLongitudinalAvailable = True
-        ret.openpilotLongitudinalControl = True
-        ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
-
     else:
       # Set global MQB parameters
       ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)]
@@ -87,6 +81,13 @@ class CarInterface(CarInterfaceBase):
 
     # Global longitudinal tuning defaults, can be overridden per-vehicle
 
+    if experimental_long and candidate in PQ_CARS:
+      # Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
+      ret.openpilotLongitudinalControl = True
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
+      if ret.transmissionType == TransmissionType.manual:
+        ret.minEnableSpeed = 4.5
+
     ret.pcmCruise = not ret.openpilotLongitudinalControl
     ret.longitudinalActuatorDelayUpperBound = 0.5  # s
     ret.longitudinalTuning.kpV = [0.1]

selfdrive/car/volkswagen/pqcan.py

@@ -35,15 +35,15 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, counter, cancel=Fa
   return packer.make_can_msg("GRA_Neu", bus, values)
 
 
-def tsk_status_value(main_switch_on, acc_faulted, long_active):
+def acc_control_value(main_switch_on, acc_faulted, long_active):
   if long_active:
-    tsk_status = 1
+    acc_control = 1
   elif main_switch_on:
-    tsk_status = 2
+    acc_control = 2
   else:
-    tsk_status = 0
+    acc_control = 0
 
-  return tsk_status
+  return acc_control
 
 
 def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
@@ -59,26 +59,32 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
   return hud_status
 
 
-def create_acc_accel_control(packer, bus, adr_status, accel):
+def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, stopping, starting, standstill):
+  commands = []
+
   values = {
-    "ACS_Sta_ADR": adr_status,
-    "ACS_StSt_Info": adr_status != 1,
-    "ACS_Typ_ACC": 0,  # TODO: this is ACC "basic", find a way to detect FtS support (1)
-    "ACS_Sollbeschl": accel if adr_status == 1 else 3.01,
-    "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27,
-    "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08,
+    "ACS_Sta_ADR": acc_control,
+    "ACS_StSt_Info": acc_control != 1,
+    "ACS_Typ_ACC": acc_type,
+    "ACS_Sollbeschl": accel if acc_control == 1 else 3.01,
+    "ACS_zul_Regelabw": 0.2 if acc_control == 1 else 1.27,
+    "ACS_max_AendGrad": 3.0 if acc_control == 1 else 5.08,
   }
 
-  return packer.make_can_msg("ACC_System", bus, values)
+  commands.append(packer.make_can_msg("ACC_System", bus, values))
+
+  return commands
 
 
-def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible):
+def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible):
   values = {
-    "ACA_StaACC": acc_status,
+    "ACA_StaACC": acc_hud_status,
     "ACA_Zeitluecke": 2,
     "ACA_V_Wunsch": set_speed,
     "ACA_gemZeitl": 8 if lead_visible else 0,
+    # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke
+    # display/display-prio handling probably needed to stop confusing the instrument cluster
+    # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint
   }
-  # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke
 
   return packer.make_can_msg("ACC_GRA_Anziege", bus, values)

selfdrive/car/volkswagen/values.py

@@ -20,7 +20,6 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 class CarControllerParams:
   HCA_STEP = 2                            # HCA_01/HCA_1 message frequency 50Hz
   ACC_CONTROL_STEP = 2                    # ACC_06/ACC_07/ACC_System frequency 50Hz
-  ACC_HUD_STEP = 4                        # ACC_GRA_Anziege frequency 25Hz
 
   ACCEL_MAX = 2.0                         # 2.0 m/s max acceleration
   ACCEL_MIN = -3.5                        # 3.5 m/s max deceleration
@@ -37,6 +36,7 @@ class CarControllerParams:
 
     if CP.carFingerprint in PQ_CARS:
       self.LDW_STEP = 5                   # LDW_1 message frequency 20Hz
+      self.ACC_HUD_STEP = 4               # ACC_GRA_Anziege 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))
       self.STEER_DELTA_DOWN = 10          # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
@@ -65,6 +65,7 @@ class CarControllerParams:
 
     else:
       self.LDW_STEP = 10                  # LDW_02 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))
       self.STEER_DELTA_DOWN = 10          # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))