Update controlsd.py

selfdrive/controls/controlsd.py

@@ -21,9 +21,6 @@ from openpilot.selfdrive.controls.lib.longcontrol import LongControl
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
-# 導入 DP 的 HTD (人工轉向偵測) 模組
-from dragonpilot.selfdrive.controls.lib.human_turn_detection import HumanTurnDetection, HTDState
-
 State = log.SelfdriveState.OpenpilotState
 LaneChangeState = log.LaneChangeState
 LaneChangeDirection = log.LaneChangeDirection
@@ -62,30 +59,10 @@ class Controls:
     elif self.CP.lateralTuning.which() == 'torque':
       self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
 
-    # ==========================================
-    # TSS2 動態熱備援控制器攔截邏輯
-    # ==========================================
-    from opendbc.car.toyota.values import TSS2_CAR
-
-    # 必須先用 .which() 判斷當前活躍的 Union 是不是 'torque'，避免 Cap'n Proto 底層報錯
-    if self.CP.carFingerprint in TSS2_CAR and self.CP.lateralTuning.which() == 'torque':
-      # 確認是 torque 後，才能安全讀取裡面的參數長度
-      if len(self.CP.lateralTuning.torque.as_builder().to_dict()) > 0:
-        # ⚠️ 確保 latcontrol_dynamic.py 放在正確的路徑
-        # 由於 DP 沒有 CP_SP，因此直接傳入 (self.CP, self.CI, DT_CTRL)
-        from openpilot.selfdrive.controls.lib.latcontrol_dynamic import LatControlDynamic
-        self.LaC = LatControlDynamic(self.CP, self.CI, DT_CTRL)
-        cloudlog.info("LatControlDynamic successfully initialized for TSS2_CAR.")
-    # ==========================================
-
     # dp - ALKA: cache enabled state (CP doesn't change after init)
     self.alka_enabled = bool(self.CP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALKA)
     self.alka_active = False
 
-    # 初始化 HTD
-    self.htd = HumanTurnDetection()
-    self.htd_state = HTDState.INACTIVE
-
   def update(self):
     self.sm.update(15)
     if self.sm.updated["liveCalibration"]:
@@ -133,24 +110,6 @@ class Controls:
     # 取出橫向控制啟用的初始狀態
     lat_active = self.sm['selfdriveState'].active or self.alka_active
 
-    # --- 防呆開關判斷 (HTD) 修改版 ---
-    # 1. 不管開關有沒有開，永遠執行 update() 讓系統記錄扭力數據 (供 DTSC 提速判斷用)
-    # [修改] 加入 CS.cruiseState.enabled 傳入給 htd
-    htd_allowed, self.htd_state = self.htd.update(
-        lat_active,
-        CS.cruiseState.enabled,
-        CS.steeringAngleDeg,
-        CS.steeringTorque,
-        CS.vEgo,
-        CS.steeringPressed
-    )
-
-    # 2. 只有當車主在介面開啟 HTD 功能時，才真正允許 HTD 切斷自動轉向 (lat_active)
-    # [修正] 直接讀取 HTD 內部快取的 _enabled 狀態，避免 100Hz 狂讀硬碟導致系統崩潰失效
-    if self.htd._enabled:
-        lat_active = lat_active and htd_allowed
-    # -------------------------------------
-
     CC.latActive = lat_active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                    (not standstill or self.CP.steerAtStandstill)
     CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and self.CP.openpilotLongitudinalControl
@@ -179,25 +138,9 @@ class Controls:
     lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
 
     actuators.curvature = self.desired_curvature
-    
-    # ⚠️ 修正重點：回復為 8 個參數，移除 self.calibrated_pose
     steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                        self.steer_limited_by_safety, self.desired_curvature,
                                                        curvature_limited, lat_delay)
-
-    # --- 讀取大腦內部決定並寫入 Enum ---
-    if hasattr(self.LaC, 'use_angle'):
-      if self.LaC.use_angle:
-        actuators.steerControlType = car.CarControl.Actuators.SteerControlType.angle
-      else:
-        actuators.steerControlType = car.CarControl.Actuators.SteerControlType.torque
-    else:
-      if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-        actuators.steerControlType = car.CarControl.Actuators.SteerControlType.angle
-      else:
-        actuators.steerControlType = car.CarControl.Actuators.SteerControlType.torque
-    # -----------------------------------
-
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
     # Ensure no NaNs/Infs
@@ -214,7 +157,7 @@ class Controls:
 
   def publish(self, CC, lac_log):
     CS = self.sm['carState']
-        # Orientation and angle rates can be useful for carcontroller
+    # Orientation and angle rates can be useful for carcontroller
     # Only calibrated (car) frame is relevant for the carcontroller
     CC.currentCurvature = self.curvature
     if self.calibrated_pose is not None:
@@ -266,24 +209,13 @@ class Controls:
     cs.forceDecel = bool((self.sm['driverMonitoringState'].awarenessStatus < 0.) or
                          (self.sm['selfdriveState'].state == State.softDisabling))
 
-    # ==========================================
+    lat_tuning = self.CP.lateralTuning.which()
-    # 🌟 橫向控制 Log 儲存：防崩潰與動態識別機制
+    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-    # ==========================================
+      cs.lateralControlState.angleState = lac_log
-    # 如果當前控制器有 'use_angle' 屬性 (即為動態控制器 LatControlDynamic)
+    elif lat_tuning == 'pid':
-    if hasattr(self.LaC, 'use_angle'):
+      cs.lateralControlState.pidState = lac_log
-      if self.LaC.use_angle:
+    elif lat_tuning == 'torque':
-        cs.lateralControlState.angleState = lac_log
+      cs.lateralControlState.torqueState = lac_log
-      else:
-        cs.lateralControlState.torqueState = lac_log
-    else:
-      # 標準單一控制器流程
-      lat_tuning = self.CP.lateralTuning.which()
-      if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-        cs.lateralControlState.angleState = lac_log
-      elif lat_tuning == 'pid':
-        cs.lateralControlState.pidState = lac_log
-      elif lat_tuning == 'torque':
-        cs.lateralControlState.torqueState = lac_log
 
     self.pm.send('controlsState', dat)
 
@@ -291,7 +223,6 @@ class Controls:
     dat = messaging.new_message('controlsStateExt')
     dat.valid = True
     dat.controlsStateExt.alkaActive = self.alka_active
-    #dat.controlsStateExt.htdAction = self.htd.htd_action_active # 新增：傳遞 HTD 判斷狀態
     self.pm.send('controlsStateExt', dat)
 
     # carControl