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

@@ -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()

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)

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,
+  )