From eb04ec95bfc1ef0748d7ca6ed08d7c86364ddd24 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 6 Apr 2026 18:57:38 -0500 Subject: [PATCH] vision speed scheduler --- selfdrive/controls/controlsd.py | 9 ---- selfdrive/controls/radard.py | 8 +-- selfdrive/controls/tests/test_radard.py | 67 ------------------------- starpilot/common/testing_grounds.py | 5 +- starpilot/system/speed_limit_vision.py | 11 ++++ system/manager/process_config.py | 2 +- 6 files changed, 15 insertions(+), 87 deletions(-) delete mode 100644 selfdrive/controls/tests/test_radard.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f6fbf150..375df1f5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,6 @@ from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles -from openpilot.starpilot.common.testing_grounds import testing_ground from openpilot.starpilot.controls.lib.neural_network_feedforward import LatControlNNFF State = log.SelfdriveState.OpenpilotState @@ -132,14 +131,6 @@ class Controls: # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) - if testing_ground.use_2: - # Keep the actuator loop inside the planner's envelope so Eco/custom accel caps - # constrain the real GM output instead of only the planner target. - pid_accel_limits = [ - max(pid_accel_limits[0], self.sm['starpilotPlan'].minAcceleration), - min(pid_accel_limits[1], self.sm['starpilotPlan'].maxAcceleration, self.starpilot_toggles.max_desired_acceleration), - ] - pid_accel_limits[1] = max(pid_accel_limits[0], pid_accel_limits[1]) self.LoC.experimental_mode = bool(self.sm['selfdriveState'].experimentalMode) actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles), self.starpilot_toggles.max_desired_acceleration)) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a9dff31f..354674ba 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -13,14 +13,11 @@ from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process from openpilot.common.swaglog import cloudlog from openpilot.common.simple_kalman import KF1D from openpilot.selfdrive.controls.lib.desire_helper import LaneChangeDirection, LaneChangeState - -from openpilot.starpilot.common.testing_grounds import testing_ground from openpilot.starpilot.common.starpilot_variables import THRESHOLD, get_starpilot_toggles # Default lead acceleration decay set to 50% at 1s _LEAD_ACCEL_TAU = 0.6 -RADAR_ONLY_FAR_LEAD_MIN_DISTANCE = 35.0 # m # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -222,10 +219,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = closest_track.get_RadarState() if not lead_dict['status'] and len(tracks) > 0: - radar_only_far_lead_min_distance = RADAR_ONLY_FAR_LEAD_MIN_DISTANCE if testing_ground.use_2 else 0.0 - far_lead_tracks = [c for c in tracks.values() - if c.dRel >= radar_only_far_lead_min_distance and - c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD] + far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD] if len(far_lead_tracks) > 0: closest_track = min(far_lead_tracks, key=lambda c: c.dRel) lead_dict = closest_track.get_RadarState() diff --git a/selfdrive/controls/tests/test_radard.py b/selfdrive/controls/tests/test_radard.py deleted file mode 100644 index 4c4b11b8..00000000 --- a/selfdrive/controls/tests/test_radard.py +++ /dev/null @@ -1,67 +0,0 @@ -from types import SimpleNamespace -from unittest.mock import patch - -from openpilot.selfdrive.controls import radard - - -def make_model_data(): - lane_lines = [ - SimpleNamespace(x=[0.0, 150.0], y=[-3.5, -3.5]), - SimpleNamespace(x=[0.0, 150.0], y=[-1.8, -1.8]), - SimpleNamespace(x=[0.0, 150.0], y=[1.8, 1.8]), - SimpleNamespace(x=[0.0, 150.0], y=[3.5, 3.5]), - ] - meta = SimpleNamespace( - laneChangeState=radard.LaneChangeState.off, - laneChangeDirection=radard.LaneChangeDirection.none, - ) - return SimpleNamespace(laneLines=lane_lines, meta=meta) - - -def make_track(d_rel): - track = radard.Track(123, 20.0, radard.KalmanParams(0.05)) - track.update(d_rel=d_rel, y_rel=0.0, v_rel=0.0, v_lead=20.0, measured=1.0) - return track - - -def make_radar_only_lead(tracks): - return radard.get_lead( - v_ego=20.0, - ready=False, - tracks=tracks, - lead_msg=SimpleNamespace(prob=0.0), - model_v_ego=20.0, - model_data=make_model_data(), - standstill=False, - starpilot_plan=SimpleNamespace(increasedStoppedDistance=0.0), - starpilot_toggles=SimpleNamespace(lead_detection_probability=0.35, human_lane_changes=False), - ) - - -def test_close_radar_only_far_lead_is_blocked_in_testing_ground_2(): - track = make_track(5.0) - - with patch.object(radard, "testing_ground", SimpleNamespace(use_2=False)): - lead = {"status": False} - for _ in range(20): - lead = make_radar_only_lead({track.identifier: track}) - assert lead["status"] - assert lead["radarTrackId"] == track.identifier - - track = make_track(5.0) - with patch.object(radard, "testing_ground", SimpleNamespace(use_2=True)): - lead = {"status": False} - for _ in range(20): - lead = make_radar_only_lead({track.identifier: track}) - assert not lead["status"] - - -def test_far_radar_only_lead_survives_testing_ground_2(): - track = make_track(80.0) - - with patch.object(radard, "testing_ground", SimpleNamespace(use_2=True)): - lead = {"status": False} - for _ in range(20): - lead = make_radar_only_lead({track.identifier: track}) - assert lead["status"] - assert lead["radarTrackId"] == track.identifier diff --git a/starpilot/common/testing_grounds.py b/starpilot/common/testing_grounds.py index 9a3987b3..8bc6cea1 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -41,9 +41,8 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( { "id": TESTING_GROUND_2, "name": "volt test tune", - "description": "A/B sandbox for planner-aware GM longitudinal limits and a radar-only far-lead distance guard.", - "aLabel": "A - Current behavior", - "bLabel": "B - Clamp + guard", + "description": "Volt longitudinal tuning sandbox.", + "aLabel": "A - Installed tune", }, { "id": TESTING_GROUND_3, diff --git a/starpilot/system/speed_limit_vision.py b/starpilot/system/speed_limit_vision.py index 89377fcc..fee18fe8 100644 --- a/starpilot/system/speed_limit_vision.py +++ b/starpilot/system/speed_limit_vision.py @@ -13,6 +13,8 @@ import cv2 import numpy as np from openpilot.common.constants import CV +from openpilot.common.realtime import set_core_affinity +from openpilot.system.hardware import PC INFERENCE_INTERVAL = 0.2 FOLLOWUP_INFERENCE_INTERVAL = 0.1 @@ -161,6 +163,7 @@ SCHOOL_ZONE_SHORT_CIRCUIT_CONFIDENCE = 0.78 DEBUG_BASE_DIR = Path("/data/media/0/vision_speed_limit_debug") DEBUG_CAPTURE_DIRNAME = "captures" SNAPSHOT_JPEG_QUALITY = 85 +SPEED_LIMIT_VISION_AFFINITY_CORES = [0, 1, 2, 3] @dataclass @@ -1783,6 +1786,14 @@ class SpeedLimitVisionDaemon: def main(): + # Keep this best-effort helper off the critical control/model/camera cores. + if not PC: + set_core_affinity(SPEED_LIMIT_VISION_AFFINITY_CORES) + + # OpenCV may otherwise fan out across many worker threads and starve more + # important daemons during detection bursts. + cv2.setNumThreads(1) + daemon = SpeedLimitVisionDaemon() daemon.run() diff --git a/system/manager/process_config.py b/system/manager/process_config.py index ae72dd10..b33136a9 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -144,7 +144,7 @@ procs += [ PythonProcess("the_pond", "starpilot.system.the_pond.the_pond", always_run, nice=19), PythonProcess("galaxy", "starpilot.system.galaxy.galaxy", always_run, nice=19), PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler), - PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision), + PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision, nice=19), ] managed_processes = {p.name: p for p in procs}