From c16039e0bcc2a770bf7724cba27765cc59719122 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Wed, 15 Jul 2026 02:17:30 +0000 Subject: [PATCH] lagd: lower max lag (#38307) * Max lag 0.6 * Fix tests * 650ms --- openpilot/selfdrive/locationd/lagd.py | 2 +- openpilot/selfdrive/locationd/test/test_lagd.py | 13 +++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/openpilot/selfdrive/locationd/lagd.py b/openpilot/selfdrive/locationd/lagd.py index 78c673a3b..d3c0c5195 100755 --- a/openpilot/selfdrive/locationd/lagd.py +++ b/openpilot/selfdrive/locationd/lagd.py @@ -25,7 +25,7 @@ MIN_VEGO = 50.0 * CV.MPH_TO_MS MIN_ABS_YAW_RATE = 0.0 MAX_YAW_RATE_SANITY_CHECK = 1.0 MIN_NCC = 0.95 -MAX_LAG = 1.0 +MAX_LAG = 0.65 MIN_LAG = 0.15 MAX_LAG_STD = 0.1 MAX_LAT_ACCEL = 2.0 diff --git a/openpilot/selfdrive/locationd/test/test_lagd.py b/openpilot/selfdrive/locationd/test/test_lagd.py index af401187f..128c19332 100644 --- a/openpilot/selfdrive/locationd/test/test_lagd.py +++ b/openpilot/selfdrive/locationd/test/test_lagd.py @@ -6,7 +6,7 @@ import pytest from openpilot.cereal import messaging, log from opendbc.car.structs import car from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ - BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION + BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION, MIN_LAG, MAX_LAG from openpilot.selfdrive.test.process_replay.migration import migrate, migrate_carParams from openpilot.selfdrive.locationd.test.test_locationd_scenarios import TEST_ROUTE from openpilot.common.params import Params @@ -15,6 +15,7 @@ from openpilot.common.hardware import PC MAX_ERR_FRAMES = 1 DT = 0.05 +LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES = int(round(MIN_LAG / DT)), int(round(MAX_LAG / DT)) def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0): @@ -103,7 +104,7 @@ class TestLagd: assert np.argmax(corr) in range(lag_frames - MAX_ERR_FRAMES, lag_frames + MAX_ERR_FRAMES + 1) def test_empty_estimator(self): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) + mocked_CP = car.CarParams(steerActuatorDelay=0.5) estimator = LateralLagEstimator(mocked_CP, DT) msg = estimator.get_msg(True) assert msg.liveDelay.status == 'unestimated' @@ -113,9 +114,9 @@ class TestLagd: assert msg.liveDelay.calPerc == 0 def test_estimator_basics(self, subtests): - for lag_frames in range(3, 10): + for lag_frames in range(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1): with subtests.test(msg=f"lag_frames={lag_frames}"): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) + mocked_CP = car.CarParams(steerActuatorDelay=0.5) estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0) process_messages(estimator, lag_frames, int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_NUM_NEEDED * BLOCK_SIZE) msg = estimator.get_msg(True) @@ -127,7 +128,7 @@ class TestLagd: assert msg.liveDelay.calPerc == 100 def test_estimator_masking(self): - mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.8), random.randint(3, 19) + mocked_CP, lag_frames = car.CarParams(steerActuatorDelay=0.5), random.randint(LAGD_MIN_LAG_FRAMES, LAGD_MAX_LAG_FRAMES - 1) estimator = LateralLagEstimator(mocked_CP, DT, min_recovery_buffer_sec=0.0, min_yr=0.0, min_valid_block_count=1) process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4) msg = estimator.get_msg(True) @@ -137,7 +138,7 @@ class TestLagd: @pytest.mark.skipif(PC, reason="only on device") def test_estimator_performance(self): - mocked_CP = car.CarParams(steerActuatorDelay=0.8) + mocked_CP = car.CarParams(steerActuatorDelay=0.5) estimator = LateralLagEstimator(mocked_CP, DT) ds = []