From e0d9e05f9f7c8bc40eb940b7d5645a046a0fcac3 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 21 Aug 2026 00:12:30 -0500 Subject: [PATCH] sub 2 --- opendbc_repo/opendbc/car/ford/interface.py | 6 +- .../opendbc/car/ford/tests/test_ford.py | 20 ++- opendbc_repo/opendbc/car/hyundai/values.py | 1 + opendbc_repo/opendbc/car/interfaces.py | 4 + opendbc_repo/opendbc/safety/modes/ford.h | 11 +- opendbc_repo/opendbc/safety/modes/hyundai.h | 10 +- .../opendbc/safety/modes/hyundai_common.h | 8 +- .../opendbc/safety/tests/test_ford.py | 30 ++++- .../opendbc/safety/tests/test_hyundai.py | 61 +++++++++ selfdrive/modeld/dmonitoringmodeld.py | 26 +--- .../modeld/tests/test_dmonitoring_affinity.py | 38 ------ selfdrive/modeld/tests/test_usbgpu_helpers.py | 4 +- .../ui/layouts/settings/starpilot/lateral.py | 2 +- starpilot/car/ford/lateral.py | 116 ++++++++++++++++-- starpilot/car/ford/tests/test_lateral.py | 73 ++++++++++- starpilot/controls/starpilot_card.py | 2 + .../controls/tests/test_starpilot_card.py | 4 +- tinygrad_repo/tinygrad/runtime/ops_amd.py | 7 +- 18 files changed, 328 insertions(+), 95 deletions(-) delete mode 100644 selfdrive/modeld/tests/test_dmonitoring_affinity.py diff --git a/opendbc_repo/opendbc/car/ford/interface.py b/opendbc_repo/opendbc/car/ford/interface.py index ed336f027..c5b754eea 100644 --- a/opendbc_repo/opendbc/car/ford/interface.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 5b709fffb..40f533aac 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 66d3ae6d3..eecc2ec7a 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index db46c7244..c8f4a5c59 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/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 diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 7f639c4ac..2394a9c86 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/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; } diff --git a/opendbc_repo/opendbc/safety/modes/hyundai.h b/opendbc_repo/opendbc/safety/modes/hyundai.h index 94f237b2f..b2de4b5d7 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai.h +++ b/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); diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index fe48c2cac..5487a9765 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/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; } diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index 6289b1759..fbfc691f5 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/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() diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index c0329bf2b..998c97f71 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/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() diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index b8d70b979..9c3508247 100644 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/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( diff --git a/selfdrive/modeld/tests/test_dmonitoring_affinity.py b/selfdrive/modeld/tests/test_dmonitoring_affinity.py deleted file mode 100644 index 8a8bf79e1..000000000 --- a/selfdrive/modeld/tests/test_dmonitoring_affinity.py +++ /dev/null @@ -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] diff --git a/selfdrive/modeld/tests/test_usbgpu_helpers.py b/selfdrive/modeld/tests/test_usbgpu_helpers.py index aff76bf88..cd3839dad 100644 --- a/selfdrive/modeld/tests/test_usbgpu_helpers.py +++ b/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(): diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index c3409c9e1..005aeb9ea 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/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, ), diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index ae25655f9..6c61a1f6d 100644 --- a/starpilot/car/ford/lateral.py +++ b/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, diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 5fe96e7b3..f650b4cb6 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/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 diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 92e2cadaa..afd3a5a05 100644 --- a/starpilot/controls/starpilot_card.py +++ b/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 diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index 22d3e6988..0666c0cae 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/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) diff --git a/tinygrad_repo/tinygrad/runtime/ops_amd.py b/tinygrad_repo/tinygrad/runtime/ops_amd.py index e46798ff8..7404a1fca 100644 --- a/tinygrad_repo/tinygrad/runtime/ops_amd.py +++ b/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,), {})