VW MQB: Cleanup stock ACC button handling (#25168)

* VW MQB: Cleanup stock ACC button handling

* use controls resume output as trigger

* these can wait until taco bell

* pass through of previously fixed value

* retry CI

* checks already done in carcontroller

* don't need these anymore

* reduce diff for now

* remove parenthesis

* update refs

* fix packer exception

* update refs

Co-authored-by: Shane Smiskol <shane@smiskol.com>
old-commit-hash: 0574b8504f85132bfbe5d8a41913e37ad030401e

selfdrive/car/volkswagen/carcontroller.py

@@ -2,7 +2,7 @@ from cereal import car
 from opendbc.can.packer import CANPacker
 from selfdrive.car import apply_std_steer_torque_limits
 from selfdrive.car.volkswagen import volkswagencan
-from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P
+from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, CarControllerParams as P
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -17,10 +17,6 @@ class CarController:
 
     self.hcaSameTorqueCount = 0
     self.hcaEnabledFrameCount = 0
-    self.graButtonStatesToSend = None
-    self.graMsgSentCount = 0
-    self.graMsgStartFramePrev = 0
-    self.graMsgBusCounterPrev = 0
 
   def update(self, CC, CS, ext_bus):
     actuators = CC.actuators
@@ -83,30 +79,12 @@ class CarController:
 
     # **** ACC Button Controls ********************************************** #
 
-    # FIXME: this entire section is in desperate need of refactoring
-
-    if self.CP.pcmCruise:
-      if self.frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
-        if CC.cruiseControl.cancel:
-          # Cancel ACC if it's engaged with OP disengaged.
-          self.graButtonStatesToSend = BUTTON_STATES.copy()
-          self.graButtonStatesToSend["cancel"] = True
-        elif CC.cruiseControl.resume:
-          # Send Resume button when planner wants car to move
-          self.graButtonStatesToSend = BUTTON_STATES.copy()
-          self.graButtonStatesToSend["resumeCruise"] = True
-
-      if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
-        self.graMsgBusCounterPrev = CS.graMsgBusCounter
-        if self.graButtonStatesToSend is not None:
-          if self.graMsgSentCount == 0:
-            self.graMsgStartFramePrev = self.frame
-          idx = (CS.graMsgBusCounter + 1) % 16
-          can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx))
-          self.graMsgSentCount += 1
-          if self.graMsgSentCount >= P.GRA_VBP_COUNT:
-            self.graButtonStatesToSend = None
-            self.graMsgSentCount = 0
+    if self.CP.pcmCruise and self.frame % P.GRA_ACC_STEP == 0:
+      idx = (CS.gra_stock_values["COUNTER"] + 1) % 16
+      if CC.cruiseControl.cancel:
+        can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, cancel=True))
+      elif CC.cruiseControl.resume:
+        can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, CS.gra_stock_values, idx, resume=True))
 
     new_actuators = actuators.copy()
     new_actuators.steer = self.apply_steer_last / P.STEER_MAX

selfdrive/car/volkswagen/carstate.py

@@ -4,18 +4,19 @@ from common.conversions import Conversions as CV
 from selfdrive.car.interfaces import CarStateBase
 from opendbc.can.parser import CANParser
 from opendbc.can.can_define import CANDefine
-from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams
+from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, \
+                                            CarControllerParams, MQB_BUTTONS
 
 class CarState(CarStateBase):
   def __init__(self, CP):
     super().__init__(CP)
+    self.button_states = {button.event_type: False for button in MQB_BUTTONS}
     can_define = CANDefine(DBC_FILES.mqb)
     if CP.transmissionType == TransmissionType.automatic:
       self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
     elif CP.transmissionType == TransmissionType.direct:
       self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"]
     self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"]
-    self.buttonStates = BUTTON_STATES.copy()
 
   def update(self, pt_cp, cam_cp, ext_cp, trans_type):
     ret = car.CarState.new_message()
@@ -114,26 +115,20 @@ class CarState(CarStateBase):
       if ret.cruiseState.speed > 90:
         ret.cruiseState.speed = 0
 
-    # Update control button states for turn signals and ACC controls.
-    self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Hoch"])
-    self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Runter"])
-    self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Abbrechen"])
-    self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Setzen"])
-    self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Wiederaufnahme"])
-    self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"])
+    # Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
     ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"])
     ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
-
-    # Read ACC hardware button type configuration info that has to pass thru
-    # to the radar. Ends up being different for steering wheel buttons vs
-    # third stalk type controls.
-    self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Hauptschalter"]
-    self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]["GRA_Typ_Hauptschalter"]
-    self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]["GRA_ButtonTypeInfo"]
-    self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]["GRA_Tip_Stufe_2"]
-    # Pick up the GRA_ACC_01 CAN message counter so we can sync to it for
-    # later cruise-control button spamming.
-    self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]["COUNTER"]
+    self.gra_stock_values = pt_cp.vl["GRA_ACC_01"]
+    buttonEvents = []
+    for button in MQB_BUTTONS:
+      state = pt_cp.vl[button.can_addr][button.can_msg] in button.values
+      if self.button_states[button.event_type] != state:
+        event = car.CarState.ButtonEvent.new_message()
+        event.type = button.event_type
+        event.pressed = state
+        buttonEvents.append(event)
+      self.button_states[button.event_type] = state
+    ret.buttonEvents = buttonEvents
 
     # Additional safety checks performed in CarInterface.
     ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
@@ -181,6 +176,7 @@ class CarState(CarStateBase):
       ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"),  # ACC button, resume
       ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"),  # ACC button, time gap adj
       ("GRA_Typ_Hauptschalter", "GRA_ACC_01"),   # ACC main button type
+      ("GRA_Codierung", "GRA_ACC_01"),           # ACC button configuration/coding
       ("GRA_Tip_Stufe_2", "GRA_ACC_01"),         # unknown related to stalk type
       ("GRA_ButtonTypeInfo", "GRA_ACC_01"),      # unknown related to stalk type
       ("COUNTER", "GRA_ACC_01"),                 # GRA_ACC_01 CAN message counter

selfdrive/car/volkswagen/interface.py

@@ -1,5 +1,5 @@
 from cereal import car
-from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter
+from selfdrive.car.volkswagen.values import CAR, CANBUS, NetworkLocation, TransmissionType, GearShifter
 from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -10,8 +10,6 @@ class CarInterface(CarInterfaceBase):
   def __init__(self, CP, CarController, CarState):
     super().__init__(CP, CarController, CarState)
 
-    self.buttonStatesPrev = BUTTON_STATES.copy()
-
     if CP.networkLocation == NetworkLocation.fwdCamera:
       self.ext_bus = CANBUS.pt
       self.cp_ext = self.cp
@@ -160,19 +158,8 @@ class CarInterface(CarInterfaceBase):
 
   # returns a car.CarState
   def _update(self, c):
-    buttonEvents = []
-
     ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType)
 
-    # Check for and process state-change events (button press or release) from
-    # the turn stalk switch or ACC steering wheel/control stalk buttons.
-    for button in self.CS.buttonStates:
-      if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
-        be = car.CarState.ButtonEvent.new_message()
-        be.type = button
-        be.pressed = self.CS.buttonStates[button]
-        buttonEvents.append(be)
-
     events = self.create_common_events(ret, extra_gears=[GearShifter.eco, GearShifter.sport, GearShifter.manumatic])
 
     # Low speed steer alert hysteresis logic
@@ -184,10 +171,6 @@ class CarInterface(CarInterfaceBase):
       events.add(EventName.belowSteerSpeed)
 
     ret.events = events.to_msg()
-    ret.buttonEvents = buttonEvents
-
-    # update previous car states
-    self.buttonStatesPrev = self.CS.buttonStates.copy()
 
     return ret
 

selfdrive/car/volkswagen/values.py

@@ -1,4 +1,4 @@
-from collections import defaultdict
+from collections import defaultdict, namedtuple
 from dataclasses import dataclass
 from enum import Enum
 from typing import Dict, List, Union
@@ -11,6 +11,7 @@ Ecu = car.CarParams.Ecu
 NetworkLocation = car.CarParams.NetworkLocation
 TransmissionType = car.CarParams.TransmissionType
 GearShifter = car.CarState.GearShifter
+Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
 
 
 class CarControllerParams:
@@ -18,9 +19,6 @@ class CarControllerParams:
   LDW_STEP = 10                  # LDW_02 message frequency 10Hz
   GRA_ACC_STEP = 3               # GRA_ACC_01 message frequency 33Hz
 
-  GRA_VBP_STEP = 100             # Send ACC virtual button presses once a second
-  GRA_VBP_COUNT = 16             # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16)
-
   # Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec.
   # Limiting rate-of-change based on real-world testing and Comma's safety
   # requirements for minimum time to lane departure.
@@ -43,14 +41,16 @@ class DBC_FILES:
 
 DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None))
 
-BUTTON_STATES = {
-  "accelCruise": False,
-  "decelCruise": False,
-  "cancel": False,
-  "setCruise": False,
-  "resumeCruise": False,
-  "gapAdjustCruise": False
-}
+
+MQB_BUTTONS = [
+  Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
+  Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
+  Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]),
+  Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]),
+  Button(car.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]),
+  Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [1]),
+]
+
 
 MQB_LDW_MESSAGES = {
   "none": 0,                            # Nothing to display

selfdrive/car/volkswagen/volkswagencan.py

@@ -32,19 +32,11 @@ def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, le
   })
   return packer.make_can_msg("LDW_02", bus, values)
 
-def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
-  values = {
-    "COUNTER": idx,
-    "GRA_Hauptschalter": CS.graHauptschalter,
-    "GRA_Abbrechen": buttonStatesToSend["cancel"],
-    "GRA_Tip_Setzen": buttonStatesToSend["setCruise"],
-    "GRA_Tip_Hoch": buttonStatesToSend["accelCruise"],
-    "GRA_Tip_Runter": buttonStatesToSend["decelCruise"],
-    "GRA_Tip_Wiederaufnahme": buttonStatesToSend["resumeCruise"],
-    "GRA_Verstellung_Zeitluecke": 3 if buttonStatesToSend["gapAdjustCruise"] else 0,
-    "GRA_Typ_Hauptschalter": CS.graTypHauptschalter,
-    "GRA_Codierung": 2,
-    "GRA_Tip_Stufe_2": CS.graTipStufe2,
-    "GRA_ButtonTypeInfo": CS.graButtonTypeInfo
-  }
+def create_mqb_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False):
+  values = gra_stock_values.copy()
+
+  values["COUNTER"] = idx
+  values["GRA_Abbrechen"] = cancel
+  values["GRA_Tip_Wiederaufnahme"] = resume
+
   return packer.make_can_msg("GRA_ACC_01", bus, values)

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-8f918a54cfefcaada91a7eca0c14ca4d4b6f11c8
+dfacf4d807e04b855832218803b5c025d06569b8