From 9932f20ed9d046aa04d243c910e604674ac62ba1 Mon Sep 17 00:00:00 2001 From: herizon1054 Date: Sun, 7 Jun 2026 00:18:20 +0800 Subject: [PATCH] Update controlsd.py --- selfdrive/controls/controlsd.py | 85 ++++----------------------------- 1 file changed, 8 insertions(+), 77 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 531212df51..568550d6fa 100755 --- a/selfdrive/controls/controlsd.py +++ b/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)) - # ========================================== - # 🌟 橫向控制 Log 儲存：防崩潰與動態識別機制 - # ========================================== - # 如果當前控制器有 'use_angle' 屬性 (即為動態控制器 LatControlDynamic) - if hasattr(self.LaC, 'use_angle'): - if self.LaC.use_angle: - cs.lateralControlState.angleState = 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 + 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