diff --git a/cereal/log.capnp b/cereal/log.capnp index b7202dfcf7..6cf4781228 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -2259,6 +2259,7 @@ struct LiveDelayData { lateralDelayEstimateStd @5 :Float32; points @4 :List(Float32); calPerc @6 :Int8; + version @7 :Int32; enum Status { unestimated @0; diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index 2df3dc9ad9..0069bb51b9 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -36,6 +36,8 @@ LAG_CANDIDATE_CORR_THRESHOLD = 0.9 SMOOTH_K = 5 SMOOTH_SIGMA = 1.0 +VERSION = 1 # bump this to invalidate old parameter caches + def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray: assert k >= 1 and k % 2 == 1, "k must be positive and odd" @@ -248,6 +250,7 @@ class LateralLagEstimator: (self.min_valid_block_count * self.block_size), 100) if debug: liveDelay.points = self.block_avg.values.flatten().tolist() + liveDelay.version = VERSION return msg @@ -368,9 +371,10 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams): if last_CP.carFingerprint != CP.carFingerprint: raise Exception("Car model mismatch") - lag, valid_blocks, status = ld.lateralDelayEstimate, ld.validBlocks, ld.status + lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks" assert status != log.LiveDelayData.Status.invalid, "Lag estimate is invalid" + assert version == VERSION, f"Lag estimate is from a different version (got {version}, expected {VERSION})" return lag, valid_blocks except Exception as e: cloudlog.error(f"Failed to retrieve initial lag: {e}") diff --git a/selfdrive/locationd/test/test_lagd.py b/selfdrive/locationd/test/test_lagd.py index 7e8d79a7de..0c4e227192 100644 --- a/selfdrive/locationd/test/test_lagd.py +++ b/selfdrive/locationd/test/test_lagd.py @@ -5,7 +5,7 @@ import pytest from cereal import messaging, log, car from openpilot.selfdrive.locationd.lagd import LateralLagEstimator, retrieve_initial_lag, masked_normalized_cross_correlation, \ - BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC + BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION 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 @@ -53,6 +53,7 @@ class TestLagd: msg = messaging.new_message('liveDelay') msg.liveDelay.lateralDelayEstimate = random.random() msg.liveDelay.validBlocks = random.randint(1, 10) + msg.liveDelay.version = VERSION params.put("LiveDelay", msg.to_bytes(), block=True) params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True) @@ -63,6 +64,20 @@ class TestLagd: assert lag == msg.liveDelay.lateralDelayEstimate assert valid_blocks == msg.liveDelay.validBlocks + def test_read_invalid_saved_params(self, subtests): + params = Params() + + lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams]) + CP = next(m for m in lr if m.which() == "carParams").carParams + + for msg_dict in [{'version': 0}, {'status': 'invalid'}, {'validBlocks': 100}]: + with subtests.test(msg=f"liveDelay={msg_dict}"): + msg = messaging.new_message('liveDelay') + msg.liveDelay = msg_dict + params.put("LiveDelay", msg.to_bytes(), block=True) + params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True) + assert retrieve_initial_lag(params, CP) is None + def test_ncc(self): lag_frames = random.randint(1, 19)