diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 375df1f5..f6fbf150 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,6 +21,7 @@ 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 @@ -131,6 +132,14 @@ 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 71c4594c..a9dff31f 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -14,11 +14,13 @@ 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 @@ -220,7 +222,10 @@ 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: - far_lead_tracks = [c for c in tracks.values() if c.potential_far_lead(standstill, model_data) and c.radarfulFilter.x >= THRESHOLD] + 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] 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 new file mode 100644 index 00000000..4c4b11b8 --- /dev/null +++ b/selfdrive/controls/tests/test_radard.py @@ -0,0 +1,67 @@ +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 74fda265..9a3987b3 100644 --- a/starpilot/common/testing_grounds.py +++ b/starpilot/common/testing_grounds.py @@ -40,10 +40,10 @@ TESTING_GROUNDS_SLOT_DEFINITIONS = ( }, { "id": TESTING_GROUND_2, - "name": "Unused", - "description": "", - "aLabel": "A", - "bLabel": "B", + "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", }, { "id": TESTING_GROUND_3,