sub 2

opendbc_repo/opendbc/car/ford/interface.py

@@ -54,10 +54,10 @@ class CarInterface(CarInterfaceBase):
       cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
     ret.safetyConfigs = cfgs
 
-    ret.alphaLongitudinalAvailable = ret.radarUnavailable
-    if alpha_long or not ret.radarUnavailable:
+    ret.alphaLongitudinalAvailable = True
+    ret.openpilotLongitudinalControl = bool(alpha_long)
+    if ret.openpilotLongitudinalControl:
       ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.LONG_CONTROL.value
-      ret.openpilotLongitudinalControl = True
 
     if ret.flags & FordFlags.CANFD:
       ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value

opendbc_repo/opendbc/car/ford/tests/test_ford.py

@@ -4,9 +4,11 @@ from collections.abc import Iterable
 from hypothesis import settings, given, strategies as st
 from parameterized import parameterized
 
+from opendbc.car import gen_empty_fingerprint
 from opendbc.car.structs import CarParams
 from opendbc.car.fw_versions import build_fw_dict
-from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes, match_vin_to_car
+from opendbc.car.ford.interface import CarInterface
+from opendbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, FordSafetyFlags, get_platform_codes, match_vin_to_car
 from opendbc.car.ford.fingerprints import FW_VERSIONS
 
 Ecu = CarParams.Ecu
@@ -154,3 +156,19 @@ class TestFordFW:
     live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
     candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
     assert len(candidates) == 0, "Should not match new model year hint"
+
+
+def test_mach_e_longitudinal_toggle_controls_stock_acc_selection():
+  stock = CarInterface.get_params(
+    CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], False, False, False, None)
+  enhanced = CarInterface.get_params(
+    CAR.FORD_MUSTANG_MACH_E_MK1, gen_empty_fingerprint(), [], True, False, False, None)
+
+  assert stock.alphaLongitudinalAvailable
+  assert not stock.openpilotLongitudinalControl
+  assert stock.pcmCruise
+  assert not (stock.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL)
+
+  assert enhanced.alphaLongitudinalAvailable
+  assert enhanced.openpilotLongitudinalControl
+  assert enhanced.safetyConfigs[-1].safetyParam & FordSafetyFlags.LONG_CONTROL

opendbc_repo/opendbc/car/hyundai/values.py

@@ -111,6 +111,7 @@ class HyundaiSafetyFlags(IntFlag):
 
 
 class HyundaiStarPilotSafetyFlags(IntFlag):
+  AOL_MAIN_LKAS_SYNC = 32
   HAS_LDA_BUTTON = 1024
   AOL_LKAS_ON_ENGAGE = 2048
 

opendbc_repo/opendbc/car/interfaces.py

@@ -261,6 +261,10 @@ class CarInterfaceBase(ABC):
         if params.get_bool("AlwaysOnLateral") and params.get_int("LKASButtonControl") == 9:
           fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value
 
+        if candidate == HYUNDAI.HYUNDAI_SONATA_HYBRID and getattr(starpilot_toggles, "always_on_lateral_lkas", False) and \
+            getattr(starpilot_toggles, "main_cruise_aol_toggle", False):
+          fp_ret.safetyConfigs[-1].safetyParam |= HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC.value
+
       elif platform in TOYOTA:
         fp_ret.canUsePedal = not CP.autoResumeSng
         fp_ret.canUseSDSU = candidate not in UNSUPPORTED_DSU_CAR and candidate not in TSS2_CAR

opendbc_repo/opendbc/safety/modes/ford.h

@@ -437,6 +437,11 @@ static safety_config ford_init(uint16_t param) {
     {FORD_LateralMotionControl2, 0, 8, .check_relay = true},
   };
 
+  static const CanMsg FORD_STOCK_TX_MSGS[] = {
+    FORD_COMMON_TX_MSGS
+    {FORD_LateralMotionControl, 0, 8, .check_relay = true},
+  };
+
   static const CanMsg FORD_LONG_TX_MSGS[] = {
     FORD_COMMON_TX_MSGS
     {FORD_ACCDATA, 0, 8, .check_relay = true},
@@ -459,15 +464,13 @@ static safety_config ford_init(uint16_t param) {
   ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL);
 #endif
 
-  // Longitudinal is the default for CAN, and optional for CAN FD w/ ALLOW_DEBUG
-  ford_longitudinal = !ford_canfd || ford_longitudinal;
-
   safety_config ret;
   if (ford_canfd) {
     ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS) : \
                               BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
   } else {
-    ret = BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS);
+    ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS) : \
+                              BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS);
   }
   return ret;
 }

opendbc_repo/opendbc/safety/modes/hyundai.h

@@ -90,6 +90,7 @@ static const CanMsg HYUNDAI_LONG_REFRESH_TX_MSGS[] = {
 
 static bool hyundai_legacy = false;
 static bool hyundai_can_canfd_blended_hda2 = false;
+static bool hyundai_acc_main_on_rx_prev = false;
 
 #define HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS()                                                                                                      \
   {.msg = {{0x260, 1, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true},                                                                                \
@@ -193,7 +194,12 @@ static void hyundai_rx_hook(const CANPacket_t *msg) {
   if (msg->addr == 0x420U) {
     if (msg->bus == scc_bus) {
       if (!hyundai_longitudinal) {
-        acc_main_on = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
+        const bool acc_main_on_rx = GET_BIT(msg, hyundai_can_canfd_blended ? 27U : 0U);
+        if (hyundai_aol_main_lkas_sync && (acc_main_on_rx != hyundai_acc_main_on_rx_prev)) {
+          lkas_on = false;
+        }
+        acc_main_on = acc_main_on_rx;
+        hyundai_acc_main_on_rx_prev = acc_main_on_rx;
       }
     }
   }
@@ -442,6 +448,8 @@ static safety_config hyundai_init(uint16_t param) {
   hyundai_common_init(param);
   hyundai_legacy = false;
   hyundai_can_canfd_blended_hda2 = hyundai_can_canfd_blended && hyundai_canfd_lka_steering;
+  hyundai_aol_main_lkas_sync = GET_FLAG(param, 32U);
+  hyundai_acc_main_on_rx_prev = false;
 
   if (hyundai_can_canfd_blended) {
     gen_crc_lookup_table_16(0x1021, hyundai_canfd_crc_lut);

opendbc_repo/opendbc/safety/modes/hyundai_common.h

@@ -60,6 +60,9 @@ bool hyundai_cancel_button_enable = false;
 extern bool hyundai_can_refresh_msgs;
 bool hyundai_can_refresh_msgs = false;
 
+extern bool hyundai_aol_main_lkas_sync;
+bool hyundai_aol_main_lkas_sync = false;
+
 static uint8_t hyundai_last_button_interaction;  // button messages since the user pressed an enable button
 static bool acc_main_on_prev;
 static bool acc_main_on_tx;
@@ -95,6 +98,7 @@ void hyundai_common_init(uint16_t param) {
   hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC);
   hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE);
   hyundai_can_refresh_msgs = GET_FLAG(param, HYUNDAI_PARAM_CAN_REFRESH_MSGS);
+  hyundai_aol_main_lkas_sync = false;
 
   hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES;
   acc_main_on_prev = false;
@@ -160,7 +164,9 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
   }
 
   if (main_button && !main_button_prev) {
-    acc_main_on = !acc_main_on;
+    if (!hyundai_aol_main_lkas_sync) {
+      acc_main_on = !acc_main_on;
+    }
   }
   main_button_prev = main_button;
 }

opendbc_repo/opendbc/safety/tests/test_ford.py

@@ -61,6 +61,7 @@ class Buttons:
 
 
 # Ford safety has four different configurations tested here:
+#  * CAN with stock longitudinal
 #  * CAN with openpilot longitudinal
 #  * CAN FD with stock longitudinal
 #  * CAN FD with openpilot longitudinal
@@ -443,6 +444,30 @@ class TestFordCANFDStockSafety(TestFordSafetyBase):
     self.assertFalse(self._tx(self._lat_ctl_msg(True, 0.0, 0.01, 0.0, 0.0)))
 
 
+class TestFordStockSafety(TestFordSafetyBase):
+  STEER_MESSAGE = MSG_LateralMotionControl
+
+  TX_MSGS = [
+    [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0],
+    [MSG_LateralMotionControl, 0], [MSG_IPMA_Data, 0],
+  ]
+  RELAY_MALFUNCTION_ADDRS = {0: (MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
+                                 MSG_IPMA_Data)}
+
+  FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
+                               MSG_IPMA_Data]}
+
+  def setUp(self):
+    self.packer = CANPackerSafety("ford_lincoln_base_pt")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, 0)
+    self.safety.init_tests()
+
+  def test_max_lateral_acceleration(self):
+    # CAN does not limit curvature from lateral acceleration
+    pass
+
+
 class TestFordLongitudinalSafetyBase(TestFordSafetyBase):
   MAX_ACCEL = 2.0  # accel is used for brakes, but openpilot can set positive values
   MIN_ACCEL = -3.5
@@ -509,8 +534,7 @@ class TestFordLongitudinalSafety(TestFordLongitudinalSafetyBase):
   def setUp(self):
     self.packer = CANPackerSafety("ford_lincoln_base_pt")
     self.safety = libsafety_py.libsafety
-    # Make sure we enforce long safety even without long flag for CAN
-    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, 0)
+    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.LONG_CONTROL)
     self.safety.init_tests()
 
   def test_max_lateral_acceleration(self):
@@ -524,7 +548,7 @@ class TestFordLKASteeringSafety(TestFordLongitudinalSafety):
   def setUp(self):
     self.packer = CANPackerSafety("ford_lincoln_base_pt")
     self.safety = libsafety_py.libsafety
-    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.LKA_STEERING)
+    self.safety.set_safety_hooks(CarParams.SafetyModel.ford, FordSafetyFlags.LONG_CONTROL | FordSafetyFlags.LKA_STEERING)
     self.safety.init_tests()
 
 

opendbc_repo/opendbc/safety/tests/test_hyundai.py

@@ -4,6 +4,7 @@ import unittest
 
 from opendbc.car.hyundai.values import HyundaiSafetyFlags, HyundaiStarPilotSafetyFlags
 from opendbc.car.structs import CarParams
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
 from opendbc.safety.tests.libsafety import libsafety_py
 import opendbc.safety.tests.common as common
 from opendbc.safety.tests.common import CANPackerSafety
@@ -621,5 +622,65 @@ class TestHyundaiAolLkasOnEngageStockSafety(HyundaiAolLkasOnEngageStockBase, Tes
     self.safety.init_tests()
 
 
+class TestHyundaiAolMainLkasSyncSafety(TestHyundaiSafety):
+  def setUp(self):
+    self.packer = CANPackerSafety("hyundai_kia_generic")
+    self.safety = libsafety_py.libsafety
+    self.safety.set_safety_hooks(
+      CarParams.SafetyModel.hyundai,
+      HyundaiStarPilotSafetyFlags.HAS_LDA_BUTTON | HyundaiStarPilotSafetyFlags.AOL_MAIN_LKAS_SYNC,
+    )
+    self.safety.init_tests()
+
+  @staticmethod
+  def _lkas_button_msg(pressed):
+    dat = bytearray(8)
+    dat[0] = int(pressed) << 4
+    return libsafety_py.make_CANPacket(0x391, 0, bytes(dat))
+
+  def test_confirmed_main_state_rephases_lkas_button(self):
+    self.safety.set_alternative_experience(ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL)
+    self.safety.set_controls_allowed(False)
+
+    self._rx(self._lkas_button_msg(True))
+    self._rx(self._lkas_button_msg(False))
+    self.assertTrue(self.safety.get_lkas_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
+
+    self._rx(self._button_msg(Buttons.NONE, main_button=True))
+    self._rx(self._button_msg(Buttons.NONE, main_button=False))
+    self.assertFalse(self.safety.get_acc_main_on())
+    self.assertTrue(self.safety.get_lkas_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+
+    self._rx(self._acc_state_msg(True))
+    self.assertFalse(self.safety.get_lkas_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
+
+    self._rx(self._button_msg(Buttons.NONE, main_button=True))
+    self._rx(self._button_msg(Buttons.NONE, main_button=False))
+    self.assertTrue(self.safety.get_acc_main_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+
+    self._rx(self._acc_state_msg(False))
+    self.assertFalse(self.safety.get_controls_allowed())
+    self.assertFalse(self.safety.get_acc_main_on())
+    self.assertFalse(self.safety.get_lkas_on())
+    self.assertFalse(self.safety.get_aol_allowed())
+    self._set_prev_torque(0)
+    self.assertFalse(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
+
+    self._rx(self._lkas_button_msg(True))
+    self._rx(self._lkas_button_msg(False))
+    self.assertTrue(self.safety.get_lkas_on())
+    self.assertTrue(self.safety.get_aol_allowed())
+    self._set_prev_torque(0)
+    self.assertTrue(self._tx(self._torque_cmd_msg(self.MAX_RATE_UP)))
+
+
 if __name__ == "__main__":
   unittest.main()

selfdrive/modeld/dmonitoringmodeld.py

@@ -16,8 +16,7 @@ from cereal import messaging
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionBuf, VisionIpcClient, VisionStreamType
 from openpilot.common.file_chunker import read_file_chunked
-from openpilot.common.params import Params
-from openpilot.common.realtime import config_realtime_process, set_core_affinity
+from openpilot.common.realtime import config_realtime_process
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
 from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
@@ -30,21 +29,6 @@ SEND_RAW_PRED = os.getenv("SEND_RAW_PRED")
 MODELS_DIR = Path(__file__).parent / "models"
 MODEL_PKL_PATH = MODELS_DIR / "dmonitoring_model_tinygrad.pkl"
 METADATA_PATH = MODELS_DIR / "dmonitoring_model_metadata.pkl"
-AFFINITY_CHECK_INTERVAL_SECONDS = 0.5
-DEFAULT_AFFINITY_CORES = [7]
-EXTERNAL_GPU_AFFINITY_CORES = [6, 7]
-
-
-def update_external_gpu_affinity(params: Params, external_gpu_affinity: bool) -> bool:
-  """Let driver monitoring use the otherwise-idle camera core only while the big model is active."""
-  external_gpu_active = params.get_bool("UsbGpuActive")
-  if external_gpu_active != external_gpu_affinity:
-    cores = EXTERNAL_GPU_AFFINITY_CORES if external_gpu_active else DEFAULT_AFFINITY_CORES
-    set_core_affinity(cores)
-    cloudlog.warning(f"dmonitoringmodeld affinity set to {cores}; external GPU active: {external_gpu_active}")
-  return external_gpu_active
-
-
 class ModelState:
   def __init__(self, cam_w: int, cam_h: int):
     self.device = get_tg_input_devices(PROCESS_NAME, usbgpu=False)["DEV"]
@@ -146,9 +130,6 @@ def get_driverstate_packet(model_output, frame_id: int, exec_time: float, gpu_ex
 
 def main():
   config_realtime_process(7, 5)
-  params = Params()
-  external_gpu_affinity = False
-  next_affinity_check = 0.0
   cloudlog.warning("connecting to driver stream")
   vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
   while not vipc_client.connect(False):
@@ -174,11 +155,6 @@ def main():
     if buf is None:
       continue
 
-    now = time.monotonic()
-    if now >= next_affinity_check:
-      external_gpu_affinity = update_external_gpu_affinity(params, external_gpu_affinity)
-      next_affinity_check = now + AFFINITY_CHECK_INTERVAL_SECONDS
-
     if model_transform is None:
       camera = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
       model_transform = np.linalg.inv(

selfdrive/modeld/tests/test_dmonitoring_affinity.py

@@ -1,38 +0,0 @@
-from openpilot.selfdrive.modeld import dmonitoringmodeld
-
-
-class FakeParams:
-  def __init__(self, active: bool):
-    self.active = active
-
-  def get_bool(self, key: str) -> bool:
-    assert key == "UsbGpuActive"
-    return self.active
-
-
-def test_non_gpu_affinity_is_unchanged(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), False)
-  assert calls == []
-
-
-def test_external_gpu_adds_idle_camera_core(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), False)
-  assert calls == [dmonitoringmodeld.EXTERNAL_GPU_AFFINITY_CORES]
-
-  calls.clear()
-  assert dmonitoringmodeld.update_external_gpu_affinity(FakeParams(True), True)
-  assert calls == []
-
-
-def test_affinity_returns_to_upstream_default_after_gpu_fallback(monkeypatch):
-  calls = []
-  monkeypatch.setattr(dmonitoringmodeld, "set_core_affinity", calls.append)
-
-  assert not dmonitoringmodeld.update_external_gpu_affinity(FakeParams(False), True)
-  assert calls == [dmonitoringmodeld.DEFAULT_AFFINITY_CORES]

selfdrive/modeld/tests/test_usbgpu_helpers.py

@@ -37,7 +37,7 @@ def test_external_gpu_uses_a_longer_load_watchdog():
   assert modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS == 3000
 
 
-def test_external_gpu_signal_wait_yields_cpu():
+def test_external_gpu_signal_wait_matches_upstream_busy_poll():
   from tinygrad.runtime import ops_amd
 
   sleeps = []
@@ -47,7 +47,7 @@ def test_external_gpu_signal_wait_yields_cpu():
 
   signal._sleep(0)
 
-  assert sleeps == [1]
+  assert sleeps == []
 
 
 def test_native_amd_signal_keeps_existing_short_wait_behavior():

selfdrive/ui/layouts/settings/starpilot/lateral.py

@@ -341,7 +341,7 @@ class StarPilotLateralLayout(_SettingsPage):
     self._ford_rows = [
       SettingRow(
         "FordLateralMode", "value", tr_noop("Steering Strategy"),
-        subtitle=tr_noop("Native keeps the existing Ford controls. Curvature and Angle enable the enhanced Ford strategies."),
+        subtitle=tr_noop("Curvature is the tuned default. Angle is available for comparison; Native preserves the original controls."),
         get_value=self._get_ford_lateral_mode,
         on_click=self._show_ford_lateral_mode,
       ),

starpilot/car/ford/lateral.py

@@ -27,6 +27,15 @@ MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - ACCELERATION_DUE_TO_GRAVITY * 0.06
 PATH_ANGLE_MIN = -0.5
 PATH_ANGLE_MAX = 0.5235
 STEER_DT = CarControllerParams.STEER_STEP * DT_CTRL
+CURVATURE_LOOKAHEAD_MIN = 0.20
+CURVATURE_LOOKAHEAD_MAX = 0.40
+HANDOFF_PRESS_SECONDS = 0.5
+HANDOFF_PAUSE_FRAMES = 6
+HANDOFF_COOLDOWN_SECONDS = 2.0
+HANDOFF_MAX_PATH_ANGLE = 0.10
+STALL_GAP_MIN = 2.0 * CarControllerParams.CURVATURE_ERROR
+STALL_HOLD_SECONDS = 0.5
+STALL_MAX_RECOVERIES = 3
 
 CANFD_BODY_ON_FRAME = frozenset({
   CAR.FORD_F_150_MK14,
@@ -113,6 +122,12 @@ class FordLateralController:
     self.curvature_samples = deque(maxlen=max(2, round(0.3 / STEER_DT)))
     self.path_angle_last = 0.0
     self.curvature_last = 0.0
+    self.handoff_press_timer = 0.0
+    self.handoff_driver_override = False
+    self.angle_pause_frames = 0
+    self.angle_pause_cooldown = 0.0
+    self.angle_stall_timer = 0.0
+    self.angle_stall_recoveries = 0
     self._frame = 0
     self._update_params()
 
@@ -152,6 +167,14 @@ class FordLateralController:
     curvatures = np.asarray(self.model.orientationRate.z) / max(v_ego, 0.01)
     return float(np.interp(lookup_time, ModelConstants.T_IDXS, curvatures))
 
+  def _curvature_lookahead(self) -> float:
+    if self.sm is None:
+      return CURVATURE_LOOKAHEAD_MIN
+    live_delay = float(self.sm["liveDelay"].lateralDelay)
+    if not np.isfinite(live_delay):
+      return CURVATURE_LOOKAHEAD_MIN
+    return float(np.clip(live_delay, CURVATURE_LOOKAHEAD_MIN, CURVATURE_LOOKAHEAD_MAX))
+
   def _lane_change(self) -> tuple[bool, int]:
     if self.model is None:
       return False, 0
@@ -188,16 +211,57 @@ class FordLateralController:
     return self.human_turn.update(
       self.human_turn_enabled, CS.out.steeringPressed, CS.out.steeringAngleDeg)
 
+  def _reset_handoff(self):
+    self.handoff_press_timer = 0.0
+    self.handoff_driver_override = False
+    self.angle_pause_frames = 0
+    self.angle_pause_cooldown = 0.0
+    self.angle_stall_timer = 0.0
+    self.angle_stall_recoveries = 0
+
+  def _driver_handoff_active(self, CS) -> bool:
+    if not self.human_turn_enabled:
+      self._reset_handoff()
+      return False
+
+    if CS.out.steeringPressed:
+      self.handoff_press_timer += STEER_DT
+      self.handoff_driver_override |= self.handoff_press_timer + 1e-9 >= HANDOFF_PRESS_SECONDS
+    else:
+      if self.handoff_driver_override:
+        self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS
+      self.handoff_driver_override = False
+      self.handoff_press_timer = 0.0
+
+    return self.handoff_driver_override
+
+  def _inactive_angle_result(self, current_curvature: float) -> FordLateralResult:
+    self.path_angle_last = 0.0
+    return FordLateralResult(shadow_curvature=current_curvature)
+
   def update_curvature(self, CC, CS, actuators) -> FordLateralResult:
-    if not CC.latActive or self._manual_turn(CC, CS) or CS.out.vEgoRaw < 0.1:
+    current = self._current_curvature(CS)
+    if not CC.latActive:
+      self.human_turn.reset()
+      self._reset_handoff()
       self.curvature_samples.clear()
       self.curvature_last = 0.0
-      return FordLateralResult(shadow_curvature=self._current_curvature(CS))
+      return FordLateralResult(shadow_curvature=current)
+
+    if self._manual_turn(CC, CS):
+      self._reset_handoff()
+      self.curvature_samples.clear()
+      self.curvature_last = 0.0
+      return FordLateralResult(shadow_curvature=current)
+
+    if self._driver_handoff_active(CS) or CS.out.vEgoRaw < 0.1:
+      self.curvature_samples.clear()
+      self.curvature_last = 0.0
+      return FordLateralResult(shadow_curvature=current)
 
     v_ego = float(CS.out.vEgoRaw)
-    predicted = self._predicted_curvature(v_ego, 0.2)
+    predicted = self._predicted_curvature(v_ego, self._curvature_lookahead())
     requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, False)
-    current = self._current_curvature(CS)
 
     if v_ego > 9.0:
       requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
@@ -237,9 +301,26 @@ class FordLateralController:
 
   def update_angle(self, CC, CS, actuators) -> FordLateralResult:
     current = self._current_curvature(CS)
-    if not CC.latActive or self._manual_turn(CC, CS):
-      self.path_angle_last = 0.0
-      return FordLateralResult(shadow_curvature=current)
+    if not CC.latActive:
+      self.human_turn.reset()
+      self._reset_handoff()
+      return self._inactive_angle_result(current)
+
+    if self._manual_turn(CC, CS):
+      self._reset_handoff()
+      return self._inactive_angle_result(current)
+
+    if self._driver_handoff_active(CS):
+      return self._inactive_angle_result(current)
+
+    if self.human_turn_enabled:
+      if self.angle_pause_frames > 0:
+        self.angle_pause_frames -= 1
+        if self.angle_pause_frames == 0:
+          self.angle_pause_cooldown = HANDOFF_COOLDOWN_SECONDS
+        return self._inactive_angle_result(current)
+
+    self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT)
 
     v_ego = float(CS.out.vEgoRaw)
     live_delay = 0.12 if self.sm is None else float(np.clip(self.sm["liveDelay"].lateralDelay, 0.1, 0.15))
@@ -249,9 +330,11 @@ class FordLateralController:
     predicted = self._predicted_curvature(v_ego, lookup_time)
     requested, precision = self._blend_and_scale(float(actuators.curvature), predicted, v_ego, True)
 
+    requested_before_deviation_limit = requested
     if v_ego > 9.0:
       requested = float(np.clip(requested, current - CarControllerParams.CURVATURE_ERROR,
                                 current + CarControllerParams.CURVATURE_ERROR))
+    deviation_limited = abs(requested - requested_before_deviation_limit) > 1e-9
 
     low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains()
     low_gain = float(np.interp(v_ego, [13.5, 26.82],
@@ -266,6 +349,25 @@ class FordLateralController:
     path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta))
     self.path_angle_last = path_angle
 
+    lane_change = self._lane_change()[0]
+    stall_gap = float(actuators.curvature) - current
+    stalled = (self.human_turn_enabled and not CS.out.steeringPressed and not lane_change and v_ego > 9.0
+               and abs(stall_gap) > STALL_GAP_MIN
+               and abs(float(actuators.curvature)) > abs(current))
+    if stalled:
+      if deviation_limited and self.angle_pause_cooldown <= 0.0:
+        self.angle_stall_timer += STEER_DT
+      if (self.angle_stall_timer + 1e-9 >= STALL_HOLD_SECONDS
+          and self.angle_stall_recoveries < STALL_MAX_RECOVERIES
+          and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE):
+        self.angle_pause_frames = HANDOFF_PAUSE_FRAMES
+        self.angle_stall_timer = 0.0
+        self.angle_stall_recoveries += 1
+    else:
+      self.angle_stall_timer = 0.0
+      if CS.out.steeringPressed or abs(stall_gap) < 0.5 * STALL_GAP_MIN:
+        self.angle_stall_recoveries = 0
+
     shadow = current if CS.out.steeringPressed else requested
     return FordLateralResult(
       path_angle=path_angle,

starpilot/car/ford/tests/test_lateral.py

@@ -3,7 +3,7 @@ from types import SimpleNamespace
 
 import pytest
 
-from ..lateral import FordLateralController, HumanTurnDetector
+from ..lateral import HANDOFF_PAUSE_FRAMES, FordLateralController, HumanTurnDetector
 
 
 class FakeSubMaster(dict):
@@ -20,7 +20,9 @@ def controller(monkeypatch):
   messaging = SimpleNamespace(SubMaster=FakeSubMaster)
   monkeypatch.setitem(sys.modules, "cereal.messaging", messaging)
   CP = SimpleNamespace(flags=0, carFingerprint="FORD_EDGE_MK2")
-  return FordLateralController(CP)
+  controller = FordLateralController(CP)
+  controller.sm = FakeSubMaster(["modelV2", "liveDelay"])
+  return controller
 
 
 def car_state(speed=15.0, curvature=0.0, steering_pressed=False, steering_angle=0.0):
@@ -49,6 +51,29 @@ def test_curvature_strategy_uses_polynomial_signals(controller):
   assert result.ramp_type == 2
 
 
+def test_curvature_lookahead_tracks_bounded_live_delay(controller):
+  controller.sm["liveDelay"].lateralDelay = 0.38
+  assert controller._curvature_lookahead() == pytest.approx(0.38)
+
+  controller.sm["liveDelay"].lateralDelay = 0.1
+  assert controller._curvature_lookahead() == pytest.approx(0.2)
+
+  controller.sm["liveDelay"].lateralDelay = 0.6
+  assert controller._curvature_lookahead() == pytest.approx(0.4)
+
+
+def test_curvature_strategy_uses_learned_lookahead(controller, monkeypatch):
+  controller.sm["liveDelay"].lateralDelay = 0.38
+  lookaheads = []
+  monkeypatch.setattr(controller, "_predicted_curvature",
+                      lambda _v_ego, lookahead: lookaheads.append(lookahead) or 0.0)
+
+  controller.update_curvature(SimpleNamespace(latActive=True), car_state(),
+                              SimpleNamespace(curvature=0.001))
+
+  assert lookaheads == [pytest.approx(0.38)]
+
+
 def test_lane_change_accepts_capnp_enum_wrappers(controller):
   controller.model = SimpleNamespace(meta=SimpleNamespace(
     laneChangeState=SimpleNamespace(raw=2),
@@ -75,3 +100,47 @@ def test_manual_turn_releases_lateral(controller):
     result = controller.update_angle(CC, CS, actuators)
   assert not result.active
   assert result.path_angle == 0.0
+
+
+@pytest.mark.parametrize("strategy", ["update_curvature", "update_angle"])
+def test_enhanced_control_yields_during_sustained_driver_correction(controller, strategy):
+  controller.human_turn_enabled = True
+  CC = SimpleNamespace(latActive=True)
+  actuators = SimpleNamespace(curvature=0.001)
+  update = getattr(controller, strategy)
+
+  for _ in range(9):
+    assert update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active
+
+  result = update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators)
+  assert not result.active
+  assert result.path_angle == 0.0
+
+  assert update(CC, car_state(), actuators).active
+
+
+@pytest.mark.parametrize("strategy", ["update_curvature", "update_angle"])
+def test_short_driver_correction_does_not_pause_enhanced_control(controller, strategy):
+  controller.human_turn_enabled = True
+  CC = SimpleNamespace(latActive=True)
+  actuators = SimpleNamespace(curvature=0.001)
+  update = getattr(controller, strategy)
+
+  for _ in range(9):
+    assert update(CC, car_state(steering_pressed=True, steering_angle=10.0), actuators).active
+
+  assert update(CC, car_state(), actuators).active
+
+
+def test_angle_control_recovers_from_bounded_tracking_stall(controller):
+  controller.human_turn_enabled = True
+  controller.angle_blend = 0.0
+  CC = SimpleNamespace(latActive=True)
+  CS = car_state(speed=15.0, curvature=0.0)
+  actuators = SimpleNamespace(curvature=0.01)
+
+  for _ in range(10):
+    assert controller.update_angle(CC, CS, actuators).active
+
+  for _ in range(HANDOFF_PAUSE_FRAMES):
+    assert not controller.update_angle(CC, CS, actuators).active

starpilot/controls/starpilot_card.py

@@ -168,6 +168,8 @@ class StarPilotCard:
 
     button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents]
     button_aol_supported = self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol
+    if getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID:
+      button_aol_supported = bool(starpilot_toggles.lkas_allowed_for_aol)
     button_managed_aol = starpilot_toggles.always_on_lateral_lkas or (button_aol_supported and starpilot_toggles.main_cruise_aol_toggle)
     g70_main_cruise_aol_managed = (
       getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G70_2020

starpilot/controls/tests/test_starpilot_card.py

@@ -280,7 +280,7 @@ def test_sonata_hybrid_lkas_button_can_start_aol_before_normal_engagement(monkey
   car_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
   starpilot_car_state = SimpleNamespace(distancePressed=False)
   sm = make_sm()
-  toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True)
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True, lkas_allowed_for_aol=True)
 
   ret = card.update(car_state, starpilot_car_state, sm, toggles)
 
@@ -300,7 +300,7 @@ def test_sonata_hybrid_preserves_aol_latch_across_reverse(monkeypatch, tmp_path)
 
   starpilot_car_state = SimpleNamespace(distancePressed=False)
   sm = make_sm()
-  toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True)
+  toggles = make_toggles(always_on_lateral=True, always_on_lateral_lkas=True, lkas_allowed_for_aol=True)
 
   enabled_state = make_car_state(available=False, enabled=False, button_events=[SimpleNamespace(type=spc.ButtonType.lkas, pressed=True)])
   ret = card.update(enabled_state, starpilot_car_state, sm, toggles)

tinygrad_repo/tinygrad/runtime/ops_amd.py

@@ -1,6 +1,6 @@
 from __future__ import annotations
 from typing import cast
-import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time
+import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit
 assert sys.platform != 'win32'
 from dataclasses import dataclass
 from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface
@@ -45,9 +45,6 @@ class AMDSignal(HCQSignal):
   def __init__(self, *args, **kwargs): super().__init__(*args, **{**kwargs, 'timestamp_divider': 100})
 
   def _sleep(self, time_spent_since_last_sleep_ms:int):
-    if self.owner is not None and self.owner.is_usb():
-      self.owner.iface.sleep(1)
-      return
     # Reasonable to sleep for long workloads (which take more than 200ms) and only timeline signals.
     if time_spent_since_last_sleep_ms > 200 and self.owner is not None: self.owner.iface.sleep(200)
 
@@ -936,7 +933,7 @@ class USBIface(PCIIface):
     # force devmem
     return super().alloc(size, host=False, uncached=uncached, cpu_access=cpu_access, contiguous=contiguous, force_devmem=True, **kwargs)
 
-  def sleep(self, timeout): time.sleep(timeout / 1000)
+  def sleep(self, timeout): pass
 
 def _mock(iface, name=None): return type(name or f"MOCK{iface.__name__}", (iface,), {})