diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index f57762328..2ef890079 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -235,7 +235,7 @@ GENESIS_G70_FRICTION_CENTER_LAT = 0.28 GENESIS_G70_FRICTION_CENTER_LAT_WIDTH = 0.10 GENESIS_G70_FRICTION_CALM_JERK = 0.35 GENESIS_G70_FRICTION_CALM_JERK_WIDTH = 0.10 -GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.24 +GENESIS_G70_FRICTION_JERK_DEADZONE_MAX = 0.30 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30 GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0 @@ -263,14 +263,14 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0 GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5 -GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.02 +GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.00 GENESIS_G70_CURVE_UNWIND_SPEED = 18.0 GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0 GENESIS_G70_CURVE_UNWIND_LAT = 0.25 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12 GENESIS_G70_CURVE_UNWIND_JERK = 0.08 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08 -GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.20 +GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28 GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.12 GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.12 GENESIS_G70_UNWIND_FF_JERK = 0.10 @@ -369,7 +369,7 @@ BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED = 2.5 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_WIDTH = 0.7 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX = 7.2 BOLT_2022_2023_LOW_SPEED_CENTER_OUTPUT_SPEED_MAX_WIDTH = 0.5 -BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.050 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.080 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06 BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2f2da4e8d..9ffbc9f50 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -881,9 +881,10 @@ class TestLatControl: assert get_genesis_g70_low_speed_output_limit(0.0, 2.0) < 0.30 assert get_genesis_g70_low_speed_angle_damping(0.0, -20.0, 0.0, 2.0) < 0.0 assert get_genesis_g70_low_speed_angle_damping(0.0, 20.0, 0.0, 2.0) > 0.0 - assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) > 1.0 + assert get_genesis_g70_curve_unwind_output_scale(0.7, -0.5, 25.0) == pytest.approx(1.0) assert get_genesis_g70_curve_unwind_output_scale(0.7, 0.5, 25.0) == 1.0 - assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 1.0 + assert get_genesis_g70_friction_jerk_deadzone(25.0, 0.0) > 0.25 + assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, 0.5, 25.0) < 0.90 assert get_genesis_g70_unwind_ff_scale(-0.7, -0.95, -0.5, 25.0) == 1.0 assert get_genesis_g70_unwind_ff_scale(-0.7, 0.2, 0.5, 25.0) == 1.0 diff --git a/selfdrive/modeld/dmonitoringmodeld.py b/selfdrive/modeld/dmonitoringmodeld.py index 383b52571..b8d70b979 100644 --- a/selfdrive/modeld/dmonitoringmodeld.py +++ b/selfdrive/modeld/dmonitoringmodeld.py @@ -16,7 +16,8 @@ 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.realtime import config_realtime_process +from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process, set_core_affinity 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 @@ -29,6 +30,19 @@ 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: @@ -132,6 +146,9 @@ 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): @@ -156,6 +173,12 @@ def main(): buf = vipc_client.recv() 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 new file mode 100644 index 000000000..8a8bf79e1 --- /dev/null +++ b/selfdrive/modeld/tests/test_dmonitoring_affinity.py @@ -0,0 +1,38 @@ +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]