From f30a8ea20e8a283ef22614ef18aefa9dc96f24b1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 25 Aug 2026 16:24:58 -0500 Subject: [PATCH] controls: gate Kona non-SCC AOL on steering release Avoid EPS torque faults when entering always-on lateral while the driver is steering. The behavior is scoped to HYUNDAI_KONA_NON_SCC and leaves other vehicles on the shared lateral-active path. Original contribution by firestar5683. --- selfdrive/controls/controlsd.py | 30 +++++++++++++++---- selfdrive/controls/lib/drive_helpers.py | 16 ++++++++++ .../controls/tests/test_drive_helpers.py | 23 +++++++++++++- 3 files changed, 62 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3bef259f8..e1ed872e0 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -13,9 +13,15 @@ from opendbc.car.car_helpers import interfaces from opendbc.car.chrysler.values import pacifica_hybrid_aol_stock_acc_mode from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.honda.values import CAR as HONDA_CAR +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from opendbc.car.nissan.values import CAR as NISSAN_CAR from opendbc.car.vehicle_model import VehicleModel -from openpilot.selfdrive.controls.lib.drive_helpers import MAX_LATERAL_JERK, clip_curvature, get_lateral_active +from openpilot.selfdrive.controls.lib.drive_helpers import ( + MAX_LATERAL_JERK, + clip_curvature, + get_kona_non_scc_lateral_active, + get_lateral_active, +) from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID @@ -340,6 +346,7 @@ class Controls: self.turn_hold_handoff_t = 0.0 self.turn_hold_done = False self.turn_blinker_swept = 0.0 + self.kona_non_scc_lateral_active = False self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -434,11 +441,22 @@ class Controls: # Check which actuators can be enabled standstill = abs(CS.vEgo) <= max(self.CP.minSteerSpeed, 0.3) or CS.standstill - CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active, - self.sm['starpilotCarState'].alwaysOnLateralEnabled, - CS.steerFaultTemporary, CS.steerFaultPermanent, - standstill, self.CP.steerAtStandstill, - self.sm['starpilotPlan'].lateralCheck) + if self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC: + CC.latActive = get_kona_non_scc_lateral_active( + CC.enabled, self.sm['selfdriveState'].active, + self.sm['starpilotCarState'].alwaysOnLateralEnabled, + CS.steerFaultTemporary, CS.steerFaultPermanent, + standstill, self.CP.steerAtStandstill, + self.sm['starpilotPlan'].lateralCheck, + CS.steeringPressed, self.kona_non_scc_lateral_active, + ) + self.kona_non_scc_lateral_active = CC.latActive + else: + CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active, + self.sm['starpilotCarState'].alwaysOnLateralEnabled, + CS.steerFaultTemporary, CS.steerFaultPermanent, + standstill, self.CP.steerAtStandstill, + self.sm['starpilotPlan'].lateralCheck) # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected) # Disable longitudinal so stock ACC works instead self.update_ecu_disable_failed() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b6e711a95..cfdd71992 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -76,6 +76,22 @@ def get_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: b return lateral_allowed and not steer_fault_temporary and not steer_fault_permanent and \ (not standstill or steer_at_standstill) and lateral_check + +def get_kona_non_scc_lateral_active(enabled: bool, active: bool, always_on_lateral_enabled: bool, + steer_fault_temporary: bool, steer_fault_permanent: bool, + standstill: bool, steer_at_standstill: bool, lateral_check: bool, + steering_pressed: bool, previous_lateral_active: bool) -> bool: + """Avoid the Kona EPS torque fault when AOL is enabled over driver steering input.""" + lateral_active = get_lateral_active(enabled, active, always_on_lateral_enabled, + steer_fault_temporary, steer_fault_permanent, + standstill, steer_at_standstill, lateral_check) + if not lateral_active: + return False + + aol_rising_edge = always_on_lateral_enabled and not enabled and not previous_lateral_active + return not (aol_rising_edge and steering_pressed) + + def curv_from_psis(psi_target, psi_rate, vego, action_t): vego = np.clip(vego, MIN_SPEED, np.inf) curv_from_psi = psi_target / (vego * action_t) diff --git a/selfdrive/controls/tests/test_drive_helpers.py b/selfdrive/controls/tests/test_drive_helpers.py index f4b1790d9..51ae2b554 100644 --- a/selfdrive/controls/tests/test_drive_helpers.py +++ b/selfdrive/controls/tests/test_drive_helpers.py @@ -1,4 +1,4 @@ -from openpilot.selfdrive.controls.lib.drive_helpers import get_lateral_active +from openpilot.selfdrive.controls.lib.drive_helpers import get_kona_non_scc_lateral_active, get_lateral_active def test_get_lateral_active_requires_enabled_without_aol(): @@ -7,3 +7,24 @@ def test_get_lateral_active_requires_enabled_without_aol(): def test_get_lateral_active_allows_aol_while_disabled(): assert get_lateral_active(False, False, True, False, False, False, False, True) + + +def test_kona_non_scc_aol_waits_for_driver_steering_to_release(): + assert not get_kona_non_scc_lateral_active( + False, False, True, False, False, False, False, True, True, False, + ) + assert get_kona_non_scc_lateral_active( + False, False, True, False, False, False, False, True, False, False, + ) + assert get_kona_non_scc_lateral_active( + False, False, True, False, False, False, False, True, True, True, + ) + + +def test_kona_non_scc_aol_gate_does_not_change_fault_or_normal_lateral_gates(): + assert not get_kona_non_scc_lateral_active( + False, False, True, True, False, False, False, True, False, False, + ) + assert get_kona_non_scc_lateral_active( + True, True, False, False, False, False, False, True, True, False, + )