diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ca048a438..6b44c8ff7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -103,6 +103,7 @@ class LatControlTorque(LatControl): self.is_tucson_4th_gen = CP.carFingerprint in TUCSON_4TH_GEN_CARS self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED) self.is_silverado = CP.carFingerprint in SILVERADO_CARS + self.is_ram_1500 = CP.carFingerprint in RAM_1500_CARS self.is_gm = CP.brand == "gm" self.is_hkg_canfd_torque = CP.brand == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD) self.flm_surface_profile_key = get_flm_surface_profile_key(CP.carFingerprint, torque_control=True) @@ -414,6 +415,8 @@ class LatControlTorque(LatControl): if ioniq_6_active: output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo) output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) + elif self.is_ram_1500: + output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif rav4_prime_active: output_torque *= get_rav4_prime_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif prius_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 6489a2af6..4adc89cb6 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -2,6 +2,7 @@ import json import math import numpy as np +from opendbc.car.chrysler.values import CAR as CHRYSLER_CAR from opendbc.car.gm.values import CAR as GM_CAR from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from opendbc.car.subaru.values import CAR as SUBARU_CAR @@ -145,6 +146,10 @@ SUBARU_IMPREZA_CARS = ( SUBARU_CAR.SUBARU_IMPREZA, ) +RAM_1500_CARS = ( + CHRYSLER_CAR.RAM_1500_5TH_GEN, +) + BOLT_2017_LATERAL_TESTING_GROUND_ID = testing_ground.id_3 BOLT_2017_STEER_RATIO_TEST_SCALE = 1.045 BOLT_2017_STEER_RATIO_ONSET_SPEED = 20.0 * CV.MPH_TO_MS @@ -765,6 +770,14 @@ SUBARU_IMPREZA_PID_TAPER_START_DEG = 0.75 SUBARU_IMPREZA_PID_TAPER_FULL_DEG = 4.0 SUBARU_IMPREZA_PID_TAPER_MIN = 0.58 +RAM_1500_TRANSITION_TAPER_MAX = 0.34 +RAM_1500_TRANSITION_SPEED_ONSET = 10.0 +RAM_1500_TRANSITION_SPEED_FULL = 15.0 +RAM_1500_TRANSITION_JERK_ONSET = 0.35 +RAM_1500_TRANSITION_JERK_FULL = 1.10 +RAM_1500_TRANSITION_LAT_FADE_START = 0.65 +RAM_1500_TRANSITION_LAT_FADE_END = 1.85 + TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS @@ -1081,6 +1094,15 @@ def get_subaru_impreza_pid_output_scale(angle_error_deg: float) -> float: return 1.0 - ((1.0 - SUBARU_IMPREZA_PID_TAPER_MIN) * error_weight) +def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + speed_weight = float(np.interp(v_ego, [RAM_1500_TRANSITION_SPEED_ONSET, RAM_1500_TRANSITION_SPEED_FULL], [0.0, 1.0])) + jerk_weight = float(np.interp(abs(desired_lateral_jerk), + [RAM_1500_TRANSITION_JERK_ONSET, RAM_1500_TRANSITION_JERK_FULL], [0.0, 1.0])) + lat_weight = 1.0 - float(np.interp(abs(desired_lateral_accel), + [RAM_1500_TRANSITION_LAT_FADE_START, RAM_1500_TRANSITION_LAT_FADE_END], [0.0, 1.0])) + return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight) + + def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 576b055d7..02a4a115e 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -16,7 +16,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import shoul from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC from openpilot.selfdrive.controls.lib.lead_behavior import is_radarless_matched_follow_window -from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_far_follow_output_slew_rates +from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import get_far_follow_output_slew_rates, get_untracked_slow_lead_decel_scale from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.car.cruise import V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog @@ -674,6 +674,7 @@ class LongitudinalPlanner: self.output_a_target = 0.0 self.output_should_stop = False self.far_follow_brake_slew_rate, self.far_follow_release_slew_rate = get_far_follow_output_slew_rates(CP) + self.untracked_slow_lead_decel_scale = get_untracked_slow_lead_decel_scale(CP) self.far_follow_output_slew_active = False self.model_launch_armed = False self.model_launch_stop_seen = False @@ -1012,7 +1013,7 @@ class LongitudinalPlanner: closing_factor = float(np.clip((closing_ratio - min_closing_ratio) / (VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO - min_closing_ratio), 0.0, 1.0)) - approach_decel = VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL * np.clip( + approach_decel = VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL * self.untracked_slow_lead_decel_scale * np.clip( 0.5 * time_factor + 0.3 * prob_factor + 0.2 * closing_factor, 0.0, 1.0) if approach_decel < VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL: return None diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index c9647ba17..b12d7f311 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -1,5 +1,6 @@ HONDA_HRV_3G_FAR_FOLLOW_BRAKE_SLEW_RATE = 3.0 HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE = 2.0 +HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE = 1.35 def get_far_follow_output_slew_rates(CP): @@ -9,3 +10,9 @@ def get_far_follow_output_slew_rates(CP): HONDA_HRV_3G_FAR_FOLLOW_RELEASE_SLEW_RATE, ) return 0.0, 0.0 + + +def get_untracked_slow_lead_decel_scale(CP): + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_HRV_3G": + return HONDA_HRV_3G_UNTRACKED_SLOW_LEAD_DECEL_SCALE + return 1.0 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 4b447bb27..40de639d4 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -8,6 +8,7 @@ import openpilot.selfdrive.controls.lib.latcontrol_pid as latcontrol_pid import openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes as latcontrol_vehicle_tunes from opendbc.car.car_helpers import interfaces from opendbc.car.interfaces import CarInterfaceBase +from opendbc.car.chrysler.values import CAR as CHRYSLER from opendbc.car.honda.values import CAR as HONDA, HondaFlags from opendbc.car.toyota.values import CAR as TOYOTA from opendbc.car.nissan.values import CAR as NISSAN @@ -26,6 +27,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( clear_flm_runtime_overrides, get_flm_runtime_overrides, get_hkg_canfd_base_friction_threshold, + get_ram_1500_transition_output_scale, get_subaru_impreza_pid_output_scale, normalize_flm_overrides, set_flm_runtime_overrides, @@ -626,6 +628,33 @@ class TestLatControl: assert lac_log.active assert tapered_output == pytest.approx(base_output * 0.5) + def test_ram_1500_transition_taper_curve(self): + assert get_ram_1500_transition_output_scale(0.4, 0.2, 17.0) == pytest.approx(1.0) + assert get_ram_1500_transition_output_scale(0.4, 1.1, 8.0) == pytest.approx(1.0) + + center_transition = get_ram_1500_transition_output_scale(0.4, 1.1, 17.0) + medium_transition = get_ram_1500_transition_output_scale(1.2, -1.1, 17.0) + assert 0.6 < center_transition < medium_transition < 1.0 + assert get_ram_1500_transition_output_scale(1.85, 2.5, 17.0) == pytest.approx(1.0) + + def test_ram_1500_transition_taper_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(CHRYSLER.RAM_1500_5TH_GEN) + base_output, _, lac_log = controller.update( + True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles, + ) + + monkeypatch.setattr(latcontrol_torque, "get_ram_1500_transition_output_scale", lambda *_args: 0.5) + tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller( + CHRYSLER.RAM_1500_5TH_GEN, + ) + tapered_output, _, _ = tapered_controller.update( + True, tapered_CS, tapered_VM, tapered_params, False, 0.0025, False, 0.2, None, None, tapered_toggles, + ) + + assert controller.is_ram_1500 + assert lac_log.active + assert tapered_output == pytest.approx(base_output * 0.5) + def test_ioniq_5_center_taper_curve(self): assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0) assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0 diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 5e50e1d60..cd81aff5c 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -639,6 +639,21 @@ def test_vision_untracked_slow_lead_cap_relaxes_confidence_for_near_stopped_high assert route_cap < -0.55 +def test_hrv_untracked_slow_lead_cap_prepares_more_for_high_speed_stop(): + v_ego = 21.8 + lead = make_lead(status=True, d_rel=86.5, v_lead=8.3, a_lead=0.0, radar=False, model_prob=0.93, y_rel=-0.14) + + civic_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_CIVIC), init_v=v_ego) + hrv_planner = LongitudinalPlanner(CarInterface.get_non_essential_params(CAR.HONDA_HRV_3G), init_v=v_ego) + civic_cap = civic_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5) + hrv_cap = hrv_planner.get_vision_untracked_slow_lead_cap(lead, v_ego, -3.5) + + assert civic_cap is not None + assert hrv_cap is not None + assert hrv_cap < civic_cap - 0.15 + assert -1.2 < hrv_cap < -0.8 + + def test_vision_untracked_slow_lead_cap_keeps_low_confidence_floor_for_less_threatening_lead(): v_ego = 20.35 diff --git a/tinygrad_repo/tinygrad/runtime/ops_amd.py b/tinygrad_repo/tinygrad/runtime/ops_amd.py index ab997443e..775214cb3 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 +import os, ctypes, struct, hashlib, functools, importlib, mmap, errno, array, contextlib, sys, weakref, itertools, collections, atexit, time assert sys.platform != 'win32' from dataclasses import dataclass from tinygrad.runtime.support.hcq import HCQCompiled, HCQAllocator, HCQBuffer, HWQueue, CLikeArgsState, HCQSignal, HCQProgram, FileIOInterface @@ -912,7 +912,11 @@ class PCIIface(PCIIfaceBase): class USBIface(PCIIface): def __init__(self, dev, dev_id): # pylint: disable=super-init-not-called - if dev_id >= len(visible:=hcq_filter_visible_devices(USB3.list_devices(0xADD1, 0x0001), "AMD")): + deadline, visible = time.monotonic() + 5.0, [] + while dev_id >= len(visible) and time.monotonic() < deadline: + visible = hcq_filter_visible_devices(USB3.list_devices(0xADD1, 0x0001), "AMD") + if dev_id >= len(visible): time.sleep(0.1) + if dev_id >= len(visible): raise RuntimeError(f"AMD:{dev_id} does not exist ({pluralize('device', len(visible))} available)") self.dev, self.pci_dev, self.vram_bar, self.count = dev, USBPCIDevice("AM", *visible[dev_id]), 0, len(visible) self.dev_impl = AMDev(self.pci_dev) diff --git a/tinygrad_repo/tinygrad/runtime/support/usb.py b/tinygrad_repo/tinygrad/runtime/support/usb.py index c90612ca9..e9990af66 100644 --- a/tinygrad_repo/tinygrad/runtime/support/usb.py +++ b/tinygrad_repo/tinygrad/runtime/support/usb.py @@ -22,7 +22,6 @@ class USB3: return ctx @classmethod - @functools.cache def list_devices(cls, vendor:int, dev:int) -> list[tuple[c.POINTER[libusb.struct_libusb_device], str]]: ret = [] for i in range(checked(libusb.libusb_get_device_list)(cls.ctx(), devs:=ctypes.POINTER(ctypes.POINTER(libusb.struct_libusb_device))())): @@ -32,6 +31,20 @@ class USB3: libusb.libusb_free_device_list(devs, 1) return ret + @classmethod + def reopen_device(cls, vendor:int, product:int, bus_number:int, timeout:float=10.0): + deadline = time.monotonic() + timeout + while True: + for dev, _ in cls.list_devices(vendor, product): + handle = ctypes.POINTER(libusb.struct_libusb_device_handle)() + rc = libusb.libusb_open(dev, ctypes.byref(handle)) if libusb.libusb_get_bus_number(dev) == bus_number else libusb.LIBUSB_ERROR_NOT_FOUND + libusb.libusb_unref_device(dev) + if rc == 0: + return handle + if time.monotonic() >= deadline: + raise RuntimeError(f"device {vendor:04x}:{product:04x} did not reappear after reset") + time.sleep(0.1) + def __init__(self, dev:c.POINTER[libusb.struct_libusb_device], ep_data_in:int, ep_stat_in:int, ep_data_out:int, ep_cmd_out:int, max_streams:int=31, use_bot=False): self.ep_data_in, self.ep_stat_in, self.ep_data_out, self.ep_cmd_out = ep_data_in, ep_stat_in, ep_data_out, ep_cmd_out @@ -40,7 +53,9 @@ class USB3: self._bulk_in_buf, self._bulk_in_mv = alloc_cbuffer(4 << 20) self._bulk_out_buf, self._bulk_out_mv = alloc_cbuffer(4 << 20) - self.handle = c.init_c_var(c.POINTER[libusb.struct_libusb_device_handle], lambda x: checked(libusb.libusb_open)(dev, x)) + bus_number = libusb.libusb_get_bus_number(dev) + self.handle = c.init_c_var(c.POINTER(libusb.struct_libusb_device_handle), lambda x: checked(libusb.libusb_open)(dev, x)) + libusb.libusb_unref_device(dev) # Read product string descriptor _buf = (ctypes.c_ubyte * 256)() @@ -54,7 +69,14 @@ class USB3: # Detach kernel driver if needed if checked(libusb.libusb_kernel_driver_active)(self.handle, 0): checked(libusb.libusb_detach_kernel_driver)(self.handle, 0) - checked(libusb.libusb_reset_device)(self.handle) + reset_rc = libusb.libusb_reset_device(self.handle) + if reset_rc in (libusb.LIBUSB_ERROR_NO_DEVICE, libusb.LIBUSB_ERROR_NOT_FOUND): + libusb.libusb_close(self.handle) + self.handle = self.reopen_device(_desc.idVendor, _desc.idProduct, bus_number) + if checked(libusb.libusb_kernel_driver_active)(self.handle, 0): + checked(libusb.libusb_detach_kernel_driver)(self.handle, 0) + elif reset_rc < 0: + raise RuntimeError(f"libusb_reset_device: {ctypes.string_at(libusb.libusb_strerror(reset_rc)).decode()}") # Set configuration and claim interface checked(libusb.libusb_set_configuration)(self.handle, 1)