frumpy

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

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
 

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(

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]