加入 Toyota/Lexus 原廠 DSU 模式

common/params.py

@@ -98,6 +98,7 @@ keys = {
   "DragonCachedVIN": [TxType.PERSISTENT],
   "DragonAllowGas": [TxType.PERSISTENT],
   "DragonBBUI": [TxType.PERSISTENT],
+  "DragonToyotaStockDSU": [TxType.PERSISTENT],
 }
 
 

selfdrive/car/toyota/carstate.py

@@ -5,6 +5,9 @@ from selfdrive.can.parser import CANParser
 from selfdrive.config import Conversions as CV
 from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR
 
+from common.params import Params
+params = Params()
+
 def parse_gear_shifter(gear, vals):
 
   val_to_capnp = {'P': 'park', 'R': 'reverse', 'N': 'neutral',
@@ -108,6 +111,8 @@ class CarState(object):
                          K=[[0.12287673], [0.29666309]])
     self.v_ego = 0.0
 
+    self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+
   def update(self, cp):
     # update prevs, update must run once per loop
     self.prev_left_blinker_on = self.left_blinker_on
@@ -173,3 +178,10 @@ class CarState(object):
       self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0
     else:
       self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM'])
+
+    if self.dragon_toyota_stock_dsu and self.generic_toggle and self.main_on:
+      self.pcm_acc_active = True
+      if self.standstill:
+        self.pcm_acc_status = 7
+      else:
+        self.pcm_acc_status = 1

selfdrive/car/toyota/interface.py

@@ -33,6 +33,8 @@ class CarInterface(object):
     if CarController is not None:
       self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
 
+    self.dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+
   @staticmethod
   def compute_gb(accel, speed):
     return float(accel) / 3.0
@@ -370,18 +372,19 @@ class CarInterface(object):
     elif not ret.cruiseState.enabled:
       events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
 
-    # DragonAllowGas
-    if params.get("DragonAllowGas") == "0":
-      # disable on pedals rising edge or when brake is pressed and speed isn't zero
-      if (ret.gasPressed and not self.gas_pressed_prev) or \
-         (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
-        events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+    if self.dragon_toyota_stock_dsu:
+      # DragonAllowGas
+      if params.get("DragonAllowGas") == "0":
+        # disable on pedals rising edge or when brake is pressed and speed isn't zero
+        if (ret.gasPressed and not self.gas_pressed_prev) or \
+           (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)):
+          events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
-      if ret.gasPressed:
-        events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
-    else:
-      if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
-        events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
+        if ret.gasPressed:
+          events.append(create_event('pedalPressed', [ET.PRE_ENABLE]))
+      else:
+        if ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001):
+          events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE]))
 
     ret.events = events
 

selfdrive/controls/controlsd.py

@@ -479,6 +479,8 @@ def controlsd_thread(gctx=None):
 
   prof = Profiler(False)  # off by default
 
+  dragon_toyota_stock_dsu = False if params.get("DragonToyotaStockDSU") == "0" else True
+
   while True:
     start_time = sec_since_boot()
     prof.checkpoint("Ratekeeper", ignore=True)
@@ -505,9 +507,10 @@ def controlsd_thread(gctx=None):
     if not CS.canValid:
       events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
-    # Only allow engagement with brake pressed when stopped behind another stopped car
-    if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
-      events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+    if dragon_toyota_stock_dsu:
+      # Only allow engagement with brake pressed when stopped behind another stopped car
+      if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3:
+        events.append(create_event('noTarget', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
 
     if not read_only:
       # update control state

selfdrive/dragonpilot/dragonconf/__init__.py

@@ -24,6 +24,7 @@ default_conf = {
   'DragonCachedVIN': '', # for cache car
   'DragonAllowGas': '0',
   'DragonBBUI': '0',
+  'DragonToyotaStockDSU': '0',
 }
 
 deprecated_conf = {