diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 1ab9a2325e..a4ee1c153e 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -35,6 +35,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 self.prev_mads_enabled = self.mads_enabled @@ -157,6 +158,7 @@ class CarState(CarStateBase): ("ACCButtons", "ASCMSteeringButton"), ("LKAButton", "ASCMSteeringButton", 0), ("RollingCounter", "ASCMSteeringButton"), + ("DistanceButton", "ASCMSteeringButton"), ("SteeringWheelAngle", "PSCMSteeringAngle"), ("SteeringWheelRate", "PSCMSteeringAngle"), ("FLWheelSpd", "EBCMWheelSpdFront"), diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index dd832ff891..d28460f5b8 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -502,6 +502,7 @@ class CarStateBase(ABC): self.mads_enabled = False self.prev_mads_enabled = False self.control_initialized = False + self.gap_dist_button = 0 # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) # R = 0.3 diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 1c52dd3990..ddbd8c9299 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -150,6 +150,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. @@ -368,6 +372,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 @@ -413,6 +421,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 2fe55b04d1..db81304cb0 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -199,6 +199,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 47fc4680a0..a0014fcf18 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -142,6 +142,7 @@ class CarState(CarStateBase): ret.rightBlinker = ret.rightBlinkerOn = 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 @@ -251,6 +252,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"])