From 7934454088025f34ce588fbe685074824a1514c8 Mon Sep 17 00:00:00 2001
From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com>
Date: Thu, 16 Feb 2023 20:57:07 -0500
Subject: [PATCH] Longitudinal: parse physical distance button (#113)

* Longitudinal: parse distance button from steering wheel

* missing gm signal
---
 selfdrive/car/gm/carstate.py         | 2 ++
 selfdrive/car/interfaces.py          | 2 ++
 selfdrive/car/toyota/carstate.py     | 9 +++++++++
 selfdrive/car/toyota/interface.py    | 2 ++
 selfdrive/car/toyota/values.py       | 2 ++
 selfdrive/car/volkswagen/carstate.py | 2 ++
 6 files changed, 19 insertions(+)

diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 3c7d35f2dc..a4bac36ea7 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -32,6 +32,7 @@ class CarState(CarStateBase):
     self.prev_cruise_buttons = self.cruise_buttons
     self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
     self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
+    self.gap_dist_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
     self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
     self.moving_backward = pt_cp.vl["EBCMWheelSpdRear"]["MovingBackward"] != 0
 
@@ -149,6 +150,7 @@ class CarState(CarStateBase):
       ("CruiseState", "AcceleratorPedal2"),
       ("ACCButtons", "ASCMSteeringButton"),
       ("RollingCounter", "ASCMSteeringButton"),
+      ("DistanceButton", "ASCMSteeringButton"),
       ("SteeringWheelAngle", "PSCMSteeringAngle"),
       ("SteeringWheelRate", "PSCMSteeringAngle"),
       ("FLWheelSpd", "EBCMWheelSpdFront"),
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index f1e2081d05..5c1df4c33e 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -334,6 +334,8 @@ class CarStateBase(ABC):
     self.cluster_speed_hyst_gap = 0.0
     self.cluster_min_speed = 0.0  # min speed before dropping to 0
 
+    self.gap_dist_button = 0
+
     # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]])
     # R = 0.3
     self.v_ego_kf = KF1D(x0=[[0.0], [0.0]],
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 0689df3845..b8213eef72 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -126,6 +126,10 @@ class CarState(CarStateBase):
     if self.CP.carFingerprint in (TSS2_CAR | RADAR_ACC_CAR):
       self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
       ret.stockFcw = bool(cp_acc.vl["ACC_HUD"]["FCW"])
+      self.gap_dist_button = cp_cam.vl["ACC_CONTROL"]["DISTANCE"]
+
+    if self.CP.flags & ToyotaFlags.SMART_DSU:
+      self.gap_dist_button = cp.vl["SDSU"]["FD_BUTTON"]
 
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
     # these cars are identified by an ACC_TYPE value of 2.
@@ -344,6 +348,10 @@ class CarState(CarStateBase):
         ("PRE_COLLISION", 33),
       ]
 
+    if CP.flags & ToyotaFlags.SMART_DSU:
+       signals.append(("FD_BUTTON", "SDSU", 0))
+       checks.append(("SDSU", 33))
+
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
 
   @staticmethod
@@ -387,6 +395,7 @@ class CarState(CarStateBase):
         ("FORCE", "PRE_COLLISION"),
         ("ACC_TYPE", "ACC_CONTROL"),
         ("FCW", "ACC_HUD"),
+        ("DISTANCE", 'ACC_CONTROL'),
       ]
       checks += [
         ("PRE_COLLISION", 33),
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 054dac6d9d..e75c78b2ae 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -196,6 +196,8 @@ class CarInterface(CarInterfaceBase):
     ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
     # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it
     smartDsu = 0x2FF in fingerprint[0]
+    if smartDsu:
+      ret.flags |= ToyotaFlags.SMART_DSU.value
     # In TSS2 cars the camera does long control
     found_ecus = [fw.ecu for fw in car_fw]
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) and not smartDsu
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index b6a556438d..f57afc03c3 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -34,6 +34,8 @@ class CarControllerParams:
 class ToyotaFlags(IntFlag):
   HYBRID = 1
 
+  SMART_DSU = 2
+
 
 class CAR:
   # Toyota
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index 0fb94909e4..dc0ce21960 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -139,6 +139,7 @@ class CarState(CarStateBase):
     ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"])
     ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
     self.gra_stock_values = pt_cp.vl["GRA_ACC_01"]
+    self.gap_dist_button = pt_cp.vl["GRA_ACC_01"]["GRA_Verstellung_Zeitluecke"]
 
     # Additional safety checks performed in CarInterface.
     ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0
@@ -245,6 +246,7 @@ class CarState(CarStateBase):
                                                                             pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])
     ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
     self.gra_stock_values = pt_cp.vl["GRA_Neu"]
+    self.gap_dist_button = pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]
 
     # Additional safety checks performed in CarInterface.
     ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"])