lagd: invalidate cached estimates on major estimator update (#38162)

* Versioning

* comment

* Tests

* fix issues

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;

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}")

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)