feat: Squash all pre-brand features into pre

common/params_keys.h

@@ -154,4 +154,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"dp_dev_delay_loggerd", {PERSISTENT, INT, "0"}},
     {"dp_dev_disable_connect", {PERSISTENT, BOOL, "0"}},
     {"dp_dev_tethering", {PERSISTENT, BOOL, "0"}},
+    {"dp_toyota_door_auto_lock_unlock", {PERSISTENT, BOOL, "0"}},
+    {"dp_toyota_tss1_sng", {PERSISTENT, BOOL, "0"}},
+    {"dp_toyota_stock_lon", {PERSISTENT, BOOL, "0"}},
+    {"dp_vag_a0_sng", {PERSISTENT, BOOL, "0"}},
+    {"dp_vag_pq_steering_patch", {PERSISTENT, BOOL, "0"}},
+    {"dp_vag_avoid_eps_lockout", {PERSISTENT, BOOL, "0"}},
 };

dragonpilot/selfdrive/ui/layouts/settings/dragonpilot.py

@@ -48,9 +48,51 @@ class DragonpilotLayout(Widget):
   def _toyota_toggles(self):
     self._toggles["title_toyota"] = simple_item(title=lambda: tr("### Toyota / Lexus ###"))
 
+    self._toggles["dp_toyota_door_auto_lock_unlock"] = toggle_item(
+      title=lambda: tr("Door Auto Lock/Unlock"),
+      description=lambda: tr("Enable openpilot to auto-lock doors above 20 km/h and auto-unlock when shifting to Park."),
+      initial_state=self._params.get_bool("dp_toyota_door_auto_lock_unlock"),
+      callback=lambda val: self._params.put_bool("dp_toyota_door_auto_lock_unlock", val),
+    )
+
+    self._toggles["dp_toyota_tss1_sng"] = toggle_item(
+      title=lambda: tr("Enable TSS1 SnG Mod"),
+      description=lambda: "",
+      initial_state=self._params.get_bool("dp_toyota_tss1_sng"),
+      callback=lambda val: self._params.put_bool("dp_toyota_tss1_sng", val),
+    )
+
+    self._toggles["dp_toyota_stock_lon"] = toggle_item(
+      title=lambda: tr("Use Stock Longitudinal Control"),
+      description=lambda: "",
+      initial_state=self._params.get_bool("dp_toyota_stock_lon"),
+      callback=lambda val: self._params.put_bool("dp_toyota_stock_lon", val),
+    )
+
   def _vag_toggles(self):
     self._toggles["title_vag"] = simple_item(title=lambda: tr("### VAG ###"))
 
+    self._toggles["dp_vag_a0_sng"] = toggle_item(
+      title=lambda: tr("MQB A0 SnG Mod"),
+      description=lambda: "",
+      initial_state=self._params.get_bool("dp_vag_a0_sng"),
+      callback=lambda val: self._params.put_bool("dp_vag_a0_sng", val),
+    )
+
+    self._toggles["dp_vag_pq_steering_patch"] = toggle_item(
+      title=lambda: tr("PQ Steering Patch"),
+      description=lambda: "",
+      initial_state=self._params.get_bool("dp_vag_pq_steering_patch"),
+      callback=lambda val: self._params.put_bool("dp_vag_pq_steering_patch", val),
+    )
+
+    self._toggles["dp_vag_avoid_eps_lockout"] = toggle_item(
+      title=lambda: tr("Avoid EPS Lockout"),
+      description=lambda: "",
+      initial_state=self._params.get_bool("dp_vag_avoid_eps_lockout"),
+      callback=lambda val: self._params.put_bool("dp_vag_avoid_eps_lockout", val),
+    )
+
   def _mazda_toggles(self):
     self._toggles["title_mazda"] = simple_item(title=lambda: tr("### Mazda ###"))
 

opendbc_repo/opendbc/car/__init__.py

@@ -91,6 +91,7 @@ class Bus(StrEnum):
   party = auto()
   ap_party = auto()
 
+  zss = auto()
 
 def rate_limit(new_value, last_value, dw_step, up_step):
   return float(np.clip(new_value, last_value + dw_step, last_value + up_step))

opendbc_repo/opendbc/car/honda/interface.py

@@ -91,11 +91,23 @@ class CarInterface(CarInterfaceBase):
       ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
 
     # Disable control if EPS mod detected
+    eps_modified = False
     for fw in car_fw:
       if fw.ecu == "eps" and b"," in fw.fwVersion:
-        ret.dashcamOnly = True
+        # ret.dashcamOnly = True
+        eps_modified = True
 
     if candidate == CAR.HONDA_CIVIC:
+      if eps_modified:
+        # stock request input values:     0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
+        # stock request output values:    0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
+        # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
+        # stock filter output values:     0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
+        # modified filter output values:  0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
+        # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
+        ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
+      else:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
 
@@ -112,6 +124,9 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.HONDA_ACCORD:
       ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]  # TODO: determine if there is a dead zone at the top end
+      if eps_modified:
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
+      else:
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
 
     elif candidate == CAR.ACURA_ILX:
@@ -124,6 +139,13 @@ class CarInterface(CarInterfaceBase):
       ret.wheelSpeedFactor = 1.025
 
     elif candidate == CAR.HONDA_CRV_5G:
+      if eps_modified:
+        # stock request input values:     0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
+        # stock request output values:    0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
+        # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
+        ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
+        ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
+      else:
         ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
         ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
       ret.wheelSpeedFactor = 1.025

opendbc_repo/opendbc/car/hyundai/interface.py

@@ -152,6 +152,13 @@ class CarInterface(CarInterfaceBase):
     if candidate in (CAR.KIA_OPTIMA_H,):
       ret.dashcamOnly = True
 
+    # w/ SMDPS, allow steering to 0
+    if 0x2AA in fingerprint[0]:
+      ret.minSteerSpeed = 0.
+      print("----------------------------------------------")
+      print("dragonpilot: SMDPS detected!")
+      print("----------------------------------------------")
+
     return ret
 
   @staticmethod

opendbc_repo/opendbc/car/structs.py

@@ -22,4 +22,10 @@ CarParamsT = capnp.lib.capnp._StructModule
 class DPFlags:
   LateralALKA = 1
   ExtRadar = 2
+  ToyotaLockCtrl = 2 ** 2
+  ToyotaTSS1SnG = 2 ** 3
+  ToyotaStockLon = 2 ** 4
+  VagA0SnG = 2 ** 5
+  VAGPQSteeringPatch = 2 ** 6
+  VagAvoidEPSLockout = 2 ** 7
   pass

opendbc_repo/opendbc/car/subaru/interface.py

@@ -33,6 +33,8 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.subaru)]
       if ret.flags & SubaruFlags.GLOBAL_GEN2:
         ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.GEN2.value
+      elif ret.flags & (SubaruFlags.IMPREZA_2018 | SubaruFlags.HYBRID) :
+        ret.safetyConfigs[0].safetyParam |= SubaruSafetyFlags.IMPREZA_2018.value
 
     ret.steerLimitTimer = 0.4
     ret.steerActuatorDelay = 0.1
@@ -50,11 +52,11 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
 
     elif candidate == CAR.SUBARU_IMPREZA:
-      ret.steerActuatorDelay = 0.4  # end-to-end angle controller
+      ret.steerActuatorDelay = 0.1  # end-to-end angle controller
       ret.lateralTuning.init('pid')
-      ret.lateralTuning.pid.kf = 0.00005
+      ret.lateralTuning.pid.kf = 0.00003
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.12, 0.18], [0.012, 0.018]]
 
     elif candidate == CAR.SUBARU_IMPREZA_2020:
       ret.lateralTuning.init('pid')

opendbc_repo/opendbc/car/subaru/values.py

@@ -26,6 +26,12 @@ class CarControllerParams:
     elif CP.carFingerprint == CAR.SUBARU_IMPREZA_2020:
       self.STEER_DELTA_UP = 35
       self.STEER_MAX = 1439
+      self.STEER_DELTA_UP = 35
+      self.STEER_DELTA_DOWN = 70
+    elif CP.carFingerprint == CAR.SUBARU_IMPREZA:
+      self.STEER_MAX = 3071
+      self.STEER_DELTA_UP = 60
+      self.STEER_DELTA_DOWN = 60
     else:
       self.STEER_MAX = 2047
 
@@ -57,7 +63,7 @@ class SubaruSafetyFlags(IntFlag):
   GEN2 = 1
   LONG = 2
   PREGLOBAL_REVERSED_DRIVER_TORQUE = 4
-
+  IMPREZA_2018 = 8
 
 class SubaruFlags(IntFlag):
   # Detected flags
@@ -74,6 +80,8 @@ class SubaruFlags(IntFlag):
   HYBRID = 32
   LKAS_ANGLE = 64
 
+  # rick
+  IMPREZA_2018 = 2 ** 10
 
 GLOBAL_ES_ADDR = 0x787
 GEN2_ES_BUTTONS_DID = b'\x11\x30'
@@ -143,7 +151,8 @@ class CAR(Platforms):
       SubaruCarDocs("Subaru Crosstrek 2018-19", video="https://youtu.be/Agww7oE1k-s?t=26"),
       SubaruCarDocs("Subaru XV 2018-19", video="https://youtu.be/Agww7oE1k-s?t=26"),
     ],
-    CarSpecs(mass=1568, wheelbase=2.67, steerRatio=15),
+    CarSpecs(mass=1568, wheelbase=2.67, steerRatio=13.5),
+    flags=SubaruFlags.IMPREZA_2018,
   )
   SUBARU_IMPREZA_2020 = SubaruPlatformConfig(
     [

opendbc_repo/opendbc/car/tests/routes.py

@@ -259,6 +259,7 @@ routes = [
   CarTestRoute("6719965b0e1d1737/2023-02-09--22-44-05", TOYOTA.TOYOTA_CHR_TSS2),  # hybrid
   CarTestRoute("6719965b0e1d1737/2023-08-29--06-40-05", TOYOTA.TOYOTA_CHR_TSS2),  # hybrid, openpilot longitudinal, radar disabled
   CarTestRoute("14623aae37e549f3/2021-10-24--01-20-49", TOYOTA.TOYOTA_PRIUS_V),
+  CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.TOYOTA_CHR_TSS2),  # openpilot longitudinal, with Radar Filter
 
   CarTestRoute("202c40641158a6e5/2021-09-21--09-43-24", VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1),
   CarTestRoute("2c68dda277d887ac/2021-05-11--15-22-20", VOLKSWAGEN.VOLKSWAGEN_ATLAS_MK1),

opendbc_repo/opendbc/car/toyota/carcontroller.py

@@ -35,6 +35,17 @@ MAX_STEER_RATE_FRAMES = 18  # tx control frames needed before torque can be cut
 # EPS allows user torque above threshold for 50 frames before permanently faulting
 MAX_USER_TORQUE = 500
 
+# Lock / unlock door commands - Credit goes to AlexandreSato!
+from opendbc.car.common.conversions import Conversions as CV
+LOCK_SPEED = 20 * CV.KPH_TO_MS
+
+LOCK_UNLOCK_CAN_ID = 0x750
+UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00'
+LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00'
+
+from cereal import car
+PARK = car.CarState.GearShifter.park
+DRIVE = car.CarState.GearShifter.drive
 
 def get_long_tune(CP, params):
   if CP.carFingerprint in TSS2_CAR:
@@ -79,6 +90,8 @@ class CarController(CarControllerBase):
     self.secoc_acc_message_counter = 0
     self.secoc_prev_reset_counter = 0
 
+    self.doors_locked = False
+
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
     stopping = actuators.longControlState == LongCtrlState.stopping
@@ -178,6 +191,9 @@ class CarController(CarControllerBase):
       # pcm entered standstill or it's disabled
       self.standstill_req = False
 
+    if (self.CP.flags & ToyotaFlags.TSS1_SNG.value) and CS.out.standstill and not self.last_standstill:
+      self.standstill_req = False
+
     self.last_standstill = CS.out.standstill
 
     # handle UI messages
@@ -249,6 +265,10 @@ class CarController(CarControllerBase):
         elif net_acceleration_request_min > 0.3:
           self.permit_braking = False
 
+        # rick - do not do delay compensation for non-TSS2 vehicles (e.g. car with sDSU?), assign the value back to actuators.accel
+        # from Jason, see https://github.com/sunnypilot/opendbc/compare/dd2016f77f8467ca2f7934db1b8c6d73164b3df7...f90b75b1531d0ef949c1c7fb8c175059448a2a97#diff-dc03b1fc7156134429efc0cdced75bc227d0ceb8bbd0c55763022fb9db6794d9
+        if not self.CP.carFingerprint in TSS2_CAR:
+          pcm_accel_cmd = actuators.accel
         pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
 
         main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd
@@ -293,7 +313,7 @@ class CarController(CarControllerBase):
                                                      hud_control.rightLaneVisible, hud_control.leftLaneDepart,
                                                      hud_control.rightLaneDepart, lat_active, CS.lkas_hud))
 
-      if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
+      if not self.CP.flags & ToyotaFlags.RADAR_FILTER.value and (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
         can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))
 
     # *** static msgs ***
@@ -303,7 +323,7 @@ class CarController(CarControllerBase):
           can_sends.append(CanData(addr, vl, bus))
 
     # keep radar disabled
-    if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+    if not self.CP.flags & ToyotaFlags.RADAR_FILTER.value and self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       can_sends.append(make_tester_present_msg(0x750, 0, 0xF))
 
     new_actuators = actuators.as_builder()
@@ -312,5 +332,13 @@ class CarController(CarControllerBase):
     new_actuators.steeringAngleDeg = self.last_angle
     new_actuators.accel = self.accel
 
+    if self.CP.flags & ToyotaFlags.LOCK_CTRL.value:
+      if not self.doors_locked and CS.out.gearShifter == DRIVE and CS.out.vEgo >= LOCK_SPEED:
+        can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, LOCK_CMD, 0))
+        self.doors_locked = True
+      elif self.doors_locked and CS.out.gearShifter == PARK:
+        can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, UNLOCK_CMD, 0))
+        self.doors_locked = False
+
     self.frame += 1
     return new_actuators, can_sends

opendbc_repo/opendbc/car/toyota/carstate.py

@@ -53,11 +53,28 @@ class CarState(CarStateBase):
     self.gvc = 0.0
     self.secoc_synchronization = None
 
+    # radar filter (mainly for CHR/Camry)
+    # the idea is to place a Panda in between Radar and camera/body (engine room) to block 0x343 (longitudinal)
+    # depends on the firmware, we should be able to read most CAN directly from cp (not cp_cam, its empty)
+    self.dp_radar_filter = bool(self.CP.flags & ToyotaFlags.RADAR_FILTER.value)
+
+    # rick - dsu_bypass from cydia2020: https://github.com/cydia2020/toyota-dsu-reroute-harness/
+    # the idea is to "re-route" the DSU to Panda CAN2 (Which connects to ADAS Camera)
+    # * when comma device is not available, the DSU message can still communicate with ADAS Camera, and over to car.
+    # * when comma device is active, CAN message of DSU and ADAS camera will the be blocked by Panda, only forward some CAN messages over to car (from CAN0).
+    self.dp_dsu_bypass = self.CP.flags & ToyotaFlags.DSU_BYPASS.value
+
+    from opendbc.car.toyota.zss import ZSS
+    self.zss = ZSS(CP.flags)
+
   def update(self, can_parsers) -> structs.CarState:
     cp = can_parsers[Bus.pt]
     cp_cam = can_parsers[Bus.cam]
 
     ret = structs.CarState()
+    if self.dp_dsu_bypass:
+      cp_acc = cp_cam
+    else:
       cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
 
     if not self.CP.flags & ToyotaFlags.SECOC.value:
@@ -78,7 +95,7 @@ class CarState(CarStateBase):
     else:
       ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0  # TODO: these also have GAS_PEDAL, come back and unify
       can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
-      if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+      if (not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value) or self.dp_radar_filter:
         ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
 
     self.parse_wheel_speeds(ret,
@@ -95,6 +112,14 @@ class CarState(CarStateBase):
     ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
     torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
+    if self.zss.enabled:
+      self.zss.set_values(can_parsers[Bus.zss])
+      if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
+        main_on = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
+      else:
+        main_on = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
+      ret.steeringAngleDeg = self.zss.get_steering_angle_deg(main_on, cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"], ret.steeringAngleDeg)
+
     # On some cars, the angle measurement is non-zero while initializing
     if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
       self.accurate_steer_angle_seen = True
@@ -147,7 +172,9 @@ class CarState(CarStateBase):
       conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
       ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
 
-    if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+    if (self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value) or \
+      self.dp_dsu_bypass:
+      if not (self.CP.flags & ToyotaFlags.SDSU.value):
         self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
       ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
 
@@ -207,7 +234,12 @@ class CarState(CarStateBase):
       ("BLINKERS_STATE", float('nan')),
     ]
 
-    return {
+    parsers = {
       Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
       Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
     }
+
+    if CP.flags & ToyotaFlags.ZSS:
+      parsers[Bus.zss] = CANParser("toyota_zss", [("SECONDARY_STEER_ANGLE", float('nan'))], 0)
+
+    return parsers

opendbc_repo/opendbc/car/toyota/interface.py

@@ -57,11 +57,30 @@ class CarInterface(CarInterfaceBase):
     if Ecu.hybrid in found_ecus:
       ret.flags |= ToyotaFlags.HYBRID.value
 
+    # 0x343 should not be present on bus 2 on cars other than TSS2_CAR unless we are re-routing DSU
+    dsu_bypass = False
+    if (0x343 in fingerprint[2] or 0x4CB in fingerprint[2]) and candidate not in TSS2_CAR:
+      print("----------------------------------------------")
+      print("dragonpilot: DSU_BYPASS detected!")
+      print("----------------------------------------------")
+      # rick: breaks TOYOTA_AVALON_2019 model tests.
+      dsu_bypass = True
+      ret.flags |= ToyotaFlags.DSU_BYPASS.value
+
+    if 0x23 in fingerprint[0]:
+      print("----------------------------------------------")
+      print("dragonpilot: ZSS detected!")
+      print("----------------------------------------------")
+      ret.flags |= ToyotaFlags.ZSS.value
+
     if candidate == CAR.TOYOTA_PRIUS:
       stop_and_go = True
       # Only give steer angle deadzone to for bad angle sensor prius
       for fw in car_fw:
         if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
+          if ret.flags & ToyotaFlags.ZSS.value:
+            CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+          else:
             ret.steerActuatorDelay = 0.25
             CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
 
@@ -118,6 +137,30 @@ class CarInterface(CarInterfaceBase):
       if alpha_long and candidate in RADAR_ACC_CAR:
         ret.flags |= ToyotaFlags.DISABLE_RADAR.value
 
+      # RADAR_ACC_CAR = CHR TSS2 / RAV4 TSS2
+      # NO_DSU_CAR = CAMRY / CHR
+      if 0x2FF in fingerprint[0] or 0x2AA in fingerprint[0]:
+        print("----------------------------------------------")
+        print("dragonpilot: RADAR_FILTER detected!")
+        print("----------------------------------------------")
+        ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value
+        ret.alphaLongitudinalAvailable = False
+        ret.flags |= ToyotaFlags.RADAR_FILTER.value | ToyotaFlags.DISABLE_RADAR.value
+
+    sdsu_active = False
+    if not (candidate in (RADAR_ACC_CAR | NO_DSU_CAR)) and 0x2FF in fingerprint[0]:
+      print("----------------------------------------------")
+      print("dragonpilot: SDSU detected!")
+      print("----------------------------------------------")
+      ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value
+
+      ret.enableDsu = False
+      sdsu_active = True
+      stop_and_go = True
+
+      ret.flags |= ToyotaFlags.SDSU.value
+      ret.alphaLongitudinalAvailable = False
+
     # openpilot longitudinal enabled by default:
     #  - cars w/ DSU disconnected
     #  - TSS2 cars with camera sending ACC_CONTROL where we can block it
@@ -126,7 +169,13 @@ class CarInterface(CarInterfaceBase):
 
     ret.openpilotLongitudinalControl = ret.enableDsu or \
       candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
-      bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
+      bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) or \
+      sdsu_active or \
+        dsu_bypass
+
+    if dp_params & structs.DPFlags.ToyotaStockLon:
+      ret.openpilotLongitudinalControl = False
+      ret.alphaLongitudinalAvailable = False
 
     ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
 
@@ -148,12 +197,19 @@ class CarInterface(CarInterfaceBase):
       if ret.flags & ToyotaFlags.HYBRID.value:
         ret.longitudinalActuatorDelay = 0.05
 
+    if dp_params & structs.DPFlags.ToyotaLockCtrl:
+      ret.flags |= ToyotaFlags.LOCK_CTRL.value
+      ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LOCK_CTRL.value
+
+    if dp_params & structs.DPFlags.ToyotaTSS1SnG:
+      ret.flags |= ToyotaFlags.TSS1_SNG.value
+
     return ret
 
   @staticmethod
   def init(CP, can_recv, can_send, communication_control=None):
     # disable radar if alpha longitudinal toggled on radar-ACC car
-    if CP.flags & ToyotaFlags.DISABLE_RADAR.value:
+    if not CP.flags & ToyotaFlags.RADAR_FILTER.value and CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       if communication_control is None:
         communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
       disable_ecu(can_recv, can_send, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)

opendbc_repo/opendbc/car/toyota/values.py

@@ -56,11 +56,15 @@ class ToyotaSafetyFlags(IntFlag):
   STOCK_LONGITUDINAL = (2 << 8)
   LTA = (4 << 8)
   SECOC = (8 << 8)
+  LOCK_CTRL = (16 << 8)
+  LONG_FILTER = (32 << 8)
 
 
 class ToyotaFlags(IntFlag):
   # Detected flags
   HYBRID = 1
+  # use legacy id
+  SDSU = 2
   DISABLE_RADAR = 4
 
   # Static flags
@@ -78,6 +82,11 @@ class ToyotaFlags(IntFlag):
   SECOC = 2048
 
   ALKA = 2 ** 12
+  LOCK_CTRL = 2 ** 13
+  TSS1_SNG = 2 ** 14
+  RADAR_FILTER = 2 ** 15
+  DSU_BYPASS = 2 ** 16
+  ZSS = 2 ** 17
 
 
 def dbc_dict(pt, radar):

opendbc_repo/opendbc/car/toyota/zss.py

@@ -0,0 +1,79 @@
+from opendbc.car.toyota.values import ToyotaFlags
+from opendbc.can.parser import CANParser
+
+ANGLE_DIFF_THRESHOLD = 4.0
+THRESHOLD_COUNT = 10
+
+class ZSS:
+
+  def __init__(self, flags: int):
+    self.enabled = flags & ToyotaFlags.ZSS.value
+    self._alka_enabled = flags & ToyotaFlags.ALKA.value
+
+    self._offset_compute_once = True
+    self._alka_active_prev = False
+    self._cruise_active_prev = False
+    self._offset = 0.
+    self._threshold_count = 0
+    self._zss_value = 0.
+    self._print_allow = True
+
+  def set_values(self, cp: CANParser):
+    self._zss_value = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
+
+  def get_enabled(self):
+    return self.enabled
+
+  def get_steering_angle_deg(self, main_on, cruise_active, stock_steering_angle_deg):
+    # off, fall back to stock
+    if not self.enabled:
+      return stock_steering_angle_deg
+
+    # when lka control is off, use stock
+    alka_active = self._is_alka_active(main_on)
+    if not cruise_active and not alka_active:
+      return stock_steering_angle_deg
+
+    # lka just activated
+    if not self._offset_compute_once and ((alka_active and not self._alka_active_prev) or (cruise_active and not self._cruise_active_prev)):
+      self._threshold_count = 0
+      self._offset_compute_once = True
+      self._print_allow = True
+
+    self._alka_active_prev = alka_active
+    self._cruise_active_prev = cruise_active
+
+    # compute offset when required
+    if self._offset_compute_once:
+      self._offset_compute_once = not self._compute_offset(stock_steering_angle_deg)
+
+    # error checking
+    if self._threshold_count >= THRESHOLD_COUNT:
+      if self._print_allow:
+        print("ZSS: Too many large diff, fallback to stock.")
+        self._print_allow = False
+      return stock_steering_angle_deg
+
+    if self._offset_compute_once:
+      print("ZSS: Compute offset required, fallback to stock.")
+      return stock_steering_angle_deg
+
+    zss_steering_angle_deg = self._zss_value - self._offset
+    angle_diff = abs(stock_steering_angle_deg - zss_steering_angle_deg)
+    if angle_diff > ANGLE_DIFF_THRESHOLD:
+      print(f"ZSS: Diff too large ({angle_diff}), fallback to stock. ")
+      if self._is_alka_active(main_on) or cruise_active:
+        self._threshold_count += 1
+      return stock_steering_angle_deg
+
+    return zss_steering_angle_deg
+
+  def _is_alka_active(self, main_on):
+    return self._alka_enabled and main_on != 0
+
+  def _compute_offset(self, steering_angle_deg):
+    if abs(steering_angle_deg) > 1e-3 and abs(self._zss_value) > 1e-3:
+      self._offset = self._zss_value - steering_angle_deg
+      print(f"ZSS: offset computed: {self._offset}")
+      return True
+    return False

opendbc_repo/opendbc/car/volkswagen/carcontroller.py

@@ -25,6 +25,9 @@ class CarController(CarControllerBase):
     self.eps_timer_soft_disable_alert = False
     self.hca_frame_timer_running = 0
     self.hca_frame_same_torque = 0
+    self._dp_vag_a0_sng = bool(self.CP.flags & VolkswagenFlags.A0SnG)
+    self.dp_vag_pq_steering_patch = 7 if CP.flags & VolkswagenFlags.PQSteeringPatch else 5
+    self.dp_avoid_eps_lockout = CP.flags & VolkswagenFlags.AVOID_EPS_LOCKOUT
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
@@ -44,6 +47,12 @@ class CarController(CarControllerBase):
       # of HCA disabled; this is done whenever output happens to be zero.
 
       if CC.latActive:
+        if self.dp_avoid_eps_lockout:
+          #根據速度縮放new_torque扭力上限
+          torque_scale = np.interp(CS.out.vEgo, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0])
+          scaled_steer_max = self.CCP.STEER_MAX * torque_scale
+          new_torque = int(round(actuators.torque * scaled_steer_max))
+        else:
           new_torque = int(round(actuators.torque * self.CCP.STEER_MAX))
         apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP)
         self.hca_frame_timer_running += self.CCP.STEER_STEP
@@ -64,7 +73,7 @@ class CarController(CarControllerBase):
 
       self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
       self.apply_torque_last = apply_torque
-      can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled))
+      can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled, self.dp_vag_pq_steering_patch))
 
       if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
         # Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque
@@ -113,7 +122,18 @@ class CarController(CarControllerBase):
                                                        lead_distance, hud_control.leadDistanceBars))
 
     # **** Stock ACC Button Controls **************************************** #
+    if self._dp_vag_a0_sng:
+      if self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:
+        standing_resume_spam = CS.out.standstill
+        spam_window = self.frame % 50 < 15
 
+        send_cancel = CC.cruiseControl.cancel
+        send_resume = CC.cruiseControl.resume or (standing_resume_spam and spam_window)
+
+        if send_cancel or send_resume:
+          can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.CAN.ext, CS.gra_stock_values,
+                                                               cancel=send_cancel, resume=send_resume))
+    else:
       gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
       if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
         can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.CAN.ext, CS.gra_stock_values,

opendbc_repo/opendbc/car/volkswagen/interface.py

@@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
       # It is documented in a four-part blog series:
       #   https://blog.willemmelching.nl/carhacking/2022/01/02/vw-part1/
       # Panda ALLOW_DEBUG firmware required.
-      ret.dashcamOnly = True
+      # ret.dashcamOnly = True
 
     else:
       # Set global MQB parameters
@@ -94,4 +94,13 @@ class CarInterface(CarInterfaceBase):
       safety_configs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
     ret.safetyConfigs = safety_configs
 
+    if dp_params & structs.DPFlags.VagA0SnG:
+      ret.flags |= VolkswagenFlags.A0SnG.value
+
+    if ret.flags & VolkswagenFlags.PQ and dp_params & structs.DPFlags.VAGPQSteeringPatch:
+      ret.flags |= VolkswagenFlags.PQSteeringPatch.value
+
+    if dp_params & structs.DPFlags.VagAvoidEPSLockout:
+      ret.flags |= VolkswagenFlags.AVOID_EPS_LOCKOUT.value
+
     return ret

opendbc_repo/opendbc/car/volkswagen/mqbcan.py

@@ -1,7 +1,7 @@
 from opendbc.car.crc import CRC8H2F
 
 
-def create_steering_control(packer, bus, apply_torque, lkas_enabled):
+def create_steering_control(packer, bus, apply_torque, lkas_enabled, dp_vag_pq_steering_patch):
   values = {
     "HCA_01_Status_HCA": 5 if lkas_enabled else 3,
     "HCA_01_LM_Offset": abs(apply_torque),

opendbc_repo/opendbc/car/volkswagen/pqcan.py

@@ -1,8 +1,8 @@
-def create_steering_control(packer, bus, apply_torque, lkas_enabled):
+def create_steering_control(packer, bus, apply_torque, lkas_enabled, dp_vag_pq_steering_patch = 5):
   values = {
     "LM_Offset": abs(apply_torque),
     "LM_OffSign": 1 if apply_torque < 0 else 0,
-    "HCA_Status": 5 if (lkas_enabled and apply_torque != 0) else 3,
+    "HCA_Status": dp_vag_pq_steering_patch if (lkas_enabled and apply_torque != 0) else 3,
     "Vib_Freq": 16,
   }
 

opendbc_repo/opendbc/car/volkswagen/values.py

@@ -170,6 +170,9 @@ class VolkswagenFlags(IntFlag):
   # Static flags
   PQ = 2
 
+  A0SnG = 2 ** 10
+  PQSteeringPatch = 2 ** 11
+  AVOID_EPS_LOCKOUT = 2 ** 12
 
 @dataclass
 class VolkswagenMQBPlatformConfig(PlatformConfig):

opendbc_repo/opendbc/dbc/toyota_zss.dbc

@@ -0,0 +1,4 @@
+BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
+ SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX
+
+CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";

opendbc_repo/opendbc/safety/modes/subaru.h

@@ -72,6 +72,8 @@
 
 static bool subaru_gen2 = false;
 static bool subaru_longitudinal = false;
+// rick
+static bool subaru_impreza_2018 = false;
 
 static uint32_t subaru_get_checksum(const CANPacket_t *msg) {
   return (uint8_t)msg->data[0];
@@ -135,6 +137,8 @@ static void subaru_rx_hook(const CANPacket_t *msg) {
 static bool subaru_tx_hook(const CANPacket_t *msg) {
   const TorqueSteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
   const TorqueSteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
+  // rick
+  const TorqueSteeringLimits SUBARU_IMPREZA_2018_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(3071, 60, 60);
 
   const LongitudinalLimits SUBARU_LONG_LIMITS = {
     .min_gas = 808,       // appears to be engine braking
@@ -156,7 +160,11 @@ static bool subaru_tx_hook(const CANPacket_t *msg) {
 
     bool steer_req = (msg->data[3] >> 5) & 1U;
 
-    const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
+    // const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS : SUBARU_STEERING_LIMITS;
+    // rick
+    const TorqueSteeringLimits limits = subaru_gen2 ? SUBARU_GEN2_STEERING_LIMITS :
+      (subaru_impreza_2018 ? SUBARU_IMPREZA_2018_STEERING_LIMITS: SUBARU_STEERING_LIMITS);
+
     violation |= steer_torque_cmd_checks(desired_torque, steer_req, limits);
   }
 
@@ -237,6 +245,10 @@ static safety_config subaru_init(uint16_t param) {
 
   subaru_gen2 = GET_FLAG(param, SUBARU_PARAM_GEN2);
 
+  // rick
+  const uint16_t SUBARU_PARAM_IMPREZA_2018 = 8;
+  subaru_impreza_2018 = GET_FLAG(param, SUBARU_PARAM_IMPREZA_2018);
+
 #ifdef ALLOW_DEBUG
   const uint16_t SUBARU_PARAM_LONGITUDINAL = 2;
   subaru_longitudinal = GET_FLAG(param, SUBARU_PARAM_LONGITUDINAL);

opendbc_repo/opendbc/safety/modes/toyota.h

@@ -36,6 +36,43 @@
   {0x343, 0, 8, .check_relay = true}, \
   {0x183, 0, 8, .check_relay = true},  /* ACC_CONTROL_2 */ \
 
+// lock ctrl
+// we dont need one for TOYOTA_COMMON_LONG_TX_MSGS as it's already allowing 0x750
+#define TOYOTA_COMMON_LOCK_TX_MSGS \
+  TOYOTA_COMMON_TX_MSGS \
+  /* radar diagnostic address */       \
+  {0x750, 0, 8, .check_relay = false}, \
+
+#define TOYOTA_COMMON_SECOC_LOCK_TX_MSGS \
+  TOYOTA_COMMON_SECOC_TX_MSGS \
+  /* radar diagnostic address */       \
+  {0x750, 0, 8, .check_relay = false}, \
+
+#define TOYOTA_COMMON_SECOC_LONG_LOCK_TX_MSGS \
+  TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
+  /* radar diagnostic address */       \
+  {0x750, 0, 8, .check_relay = false}, \
+
+// long filter
+// same as TOYOTA_COMMON_LONG_TX_MSGS except we don't need to 0x343 to check relay
+#define TOYOTA_COMMON_LONG_FILTER_TX_MSGS \
+  TOYOTA_COMMON_TX_MSGS \
+  /* DSU bus 0 */ \
+  {0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \
+  {0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \
+  /* DSU bus 1 */ \
+  {0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \
+  {0x470, 1, 4, .check_relay = false}, \
+  /* PCS_HUD */                        \
+  {0x411, 0, 8, .check_relay = false}, \
+  /* ACC */                            \
+  {0x343, 0, 8, .check_relay = false},  \
+
+// lock ctrl + long filter
+#define TOYOTA_COMMON_LONG_FILTER_LOCK_TX_MSGS \
+  TOYOTA_COMMON_LONG_FILTER_TX_MSGS \
+  {0x750, 0, 8, .check_relay = false}, \
+
 #define TOYOTA_COMMON_RX_CHECKS(lta)                                                                                                       \
   {.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},  \
   {.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}},                           \
@@ -62,6 +99,11 @@ static bool toyota_stock_longitudinal = false;
 static bool toyota_lta = false;
 static int toyota_dbc_eps_torque_factor = 100;   // conversion factor for STEER_TORQUE_EPS in %: see dbc file
 
+// lock ctrl
+static bool toyota_lock_ctrl = false;
+// long filter
+static bool toyota_long_filter = false;
+
 static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
   int len = GET_LEN(msg);
   uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U) + (uint8_t)(len);
@@ -332,7 +374,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
   }
 
   // UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
-  if (msg->addr == 0x750U) {
+  if (!toyota_lock_ctrl && msg->addr == 0x750U) {
     // this address is sub-addressed. only allow tester present to radar (0xF)
     bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
     if (invalid_uds_msg) {
@@ -360,6 +402,30 @@ static safety_config toyota_init(uint16_t param) {
     TOYOTA_COMMON_SECOC_LONG_TX_MSGS
   };
 
+  // lock ctrl
+  // we dont need one for TOYOTA_COMMON_LONG_TX_MSGS as it's already allowing 0x750
+  static const CanMsg TOYOTA_LOCK_TX_MSGS[] = {
+    TOYOTA_COMMON_LOCK_TX_MSGS
+  };
+
+  static const CanMsg TOYOTA_SECOC_LOCK_TX_MSGS[] = {
+    TOYOTA_COMMON_SECOC_LOCK_TX_MSGS
+  };
+
+  static const CanMsg TOYOTA_SECOC_LONG_LOCK_TX_MSGS[] = {
+    TOYOTA_COMMON_SECOC_LOCK_TX_MSGS
+  };
+
+  // long filter
+  static const CanMsg TOYOTA_LONG_FILTER_TX_MSGS[] = {
+    TOYOTA_COMMON_LONG_FILTER_TX_MSGS
+  };
+
+  // lock ctrl + long filter
+  static const CanMsg TOYOTA_LONG_FILTER_LOCK_TX_MSGS[] = {
+    TOYOTA_COMMON_LONG_FILTER_LOCK_TX_MSGS
+  };
+
   // safety param flags
   // first byte is for EPS factor, second is for flags
   const uint32_t TOYOTA_PARAM_OFFSET = 8U;
@@ -373,6 +439,14 @@ static safety_config toyota_init(uint16_t param) {
   toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC);
 #endif
 
+  // lock ctrl
+  const uint32_t TOYOTA_PARAM_LOCK_CTRL = 16UL << TOYOTA_PARAM_OFFSET;
+  toyota_lock_ctrl = GET_FLAG(param, TOYOTA_PARAM_LOCK_CTRL);
+
+  // long filter
+  const uint32_t TOYOTA_PARAM_LONG_FILTER = 32U << TOYOTA_PARAM_OFFSET;
+  toyota_long_filter = GET_FLAG(param, TOYOTA_PARAM_LONG_FILTER);
+
   toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
   toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
   toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
@@ -381,17 +455,37 @@ static safety_config toyota_init(uint16_t param) {
   safety_config ret;
   if (toyota_secoc) {
     if (toyota_stock_longitudinal) {
+      if (toyota_lock_ctrl) {
+        SET_TX_MSGS(TOYOTA_SECOC_LOCK_TX_MSGS, ret);
+      } else {
         SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
+      }
+    } else {
+      if (toyota_lock_ctrl) {
+        SET_TX_MSGS(TOYOTA_SECOC_LONG_LOCK_TX_MSGS, ret);
       } else {
         SET_TX_MSGS(TOYOTA_SECOC_LONG_TX_MSGS, ret);
       }
+    }
   } else {
     if (toyota_stock_longitudinal) {
+      if (toyota_lock_ctrl) {
+        SET_TX_MSGS(TOYOTA_LOCK_TX_MSGS, ret);
+      } else {
         SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
+      }
+    } else {
+      if (toyota_long_filter) {
+        if (toyota_lock_ctrl) {
+          SET_TX_MSGS(TOYOTA_LONG_FILTER_LOCK_TX_MSGS, ret);
+        } else {
+          SET_TX_MSGS(TOYOTA_LONG_FILTER_TX_MSGS, ret);
+        }
       } else {
         SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
       }
     }
+  }
 
   if (toyota_secoc) {
     static RxCheck toyota_secoc_rx_checks[] = {

selfdrive/car/card.py

@@ -105,6 +105,24 @@ class Car:
       if self.params.get_bool("dp_lat_alka"):
         dp_params |= structs.DPFlags.LateralALKA
 
+      if self.params.get_bool("dp_toyota_door_auto_lock_unlock"):
+        dp_params |= structs.DPFlags.ToyotaLockCtrl
+
+      if self.params.get_bool("dp_toyota_tss1_sng"):
+        dp_params |= structs.DPFlags.ToyotaTSS1SnG
+
+      if self.params.get_bool("dp_toyota_stock_lon"):
+        dp_params |= structs.DPFlags.ToyotaStockLon
+
+      if self.params.get_bool("dp_vag_a0_sng"):
+        dp_params |= structs.DPFlags.VagA0SnG
+
+      if self.params.get_bool("dp_vag_pq_steering_patch"):
+        dp_params |= structs.DPFlags.VAGPQSteeringPatch
+
+      if self.params.get_bool("dp_vag_avoid_eps_lockout"):
+        dp_params |= structs.DPFlags.VagAvoidEPSLockout
+
       dp_fingerprint = str(self.params.get("dp_dev_model_selected") or "")
       self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params, dp_fingerprint=dp_fingerprint)
       self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP)