bett

This reverts commit 15e5d2efb9.

openpilot/cereal/log.capnp

@@ -2309,6 +2309,19 @@ struct LiveDelayData {
   points @4 :List(Float32);
   calPerc @6 :Int8;
   version @7 :Int32;
+  speedBucket @8 :Int8;
+  speedBucketEdges @9 :List(Float32);
+  lateralDelayBuckets @10 :List(Float32);
+  lateralDelayEstimateStdBuckets @11 :List(Float32);
+  validBlocksBuckets @12 :List(Int32);
+  calPercBuckets @13 :List(Int8);
+  statusBuckets @14 :List(Status);
+  lateralDelayAppliedBuckets @15 :List(Float32);
+  blockIdxBuckets @16 :List(Int8);
+  sampleIdxBuckets @17 :List(Int8);
+  blockValuesBuckets @18 :List(List(Float32));
+  learningCountdownBuckets @19 :List(Float32);
+  learningResetReasonBuckets @20 :List(Text);
 
   enum Status {
     unestimated @0;

openpilot/selfdrive/locationd/lagd.py

@@ -16,13 +16,21 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose, fft_next_good_size, parabolic_peak_interp
 from openpilot.sunnypilot.livedelay.lagd_toggle import LagdToggle
 
-BLOCK_SIZE = 100
-BLOCK_NUM = 50
-BLOCK_NUM_NEEDED = 5
-MOVING_WINDOW_SEC = 60.0
-MIN_OKAY_WINDOW_SEC = 25.0
+BLOCK_SIZE = 8
+BLOCK_NUM = 30
+BLOCK_NUM_NEEDED = 3
+MOVING_WINDOW_SEC = 20.0
+MIN_OKAY_WINDOW_SEC = 5.0
 MIN_RECOVERY_BUFFER_SEC = 2.0
 MIN_VEGO = 50.0 * CV.MPH_TO_MS
+SPEED_BUCKET_EDGES = np.arange(0.0, 90.0, 10.0) * CV.MPH_TO_MS
+
+
+def interpolate_bucket_values(speed: float, edges: np.ndarray, values: list[float]) -> float:
+  if len(values) == 1:
+    return values[0]
+  widths = np.append(np.diff(edges), edges[-1] - edges[-2])
+  return float(np.interp(speed, edges + widths / 2, values))
 MIN_ABS_YAW_RATE = 0.0
 MAX_YAW_RATE_SANITY_CHECK = 1.0
 MIN_NCC = 0.95
@@ -31,14 +39,14 @@ MIN_LAG = 0.15
 MAX_LAG_STD = 0.1
 MAX_LAT_ACCEL = 2.0
 MAX_LAT_ACCEL_DIFF = 0.6
-MIN_LAT_ACCEL_RANGE = 0.5
+MIN_LAT_ACCEL_RANGE = 0.1
 MIN_CONFIDENCE = 0.7
 CORR_BORDER_OFFSET = 5
 LAG_CANDIDATE_CORR_THRESHOLD = 0.9
 SMOOTH_K = 5
 SMOOTH_SIGMA = 1.0
 
-VERSION = 1  # bump this to invalidate old parameter caches
+VERSION = 3  # bump this to invalidate old parameter caches
 
 
 def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray:
@@ -152,6 +160,12 @@ class BlockAverage:
       self.block_idx = (self.block_idx + 1) % self.num_blocks
       self.valid_blocks = min(self.valid_blocks + 1, self.num_blocks)
 
+  def restore(self, values: list[float], block_idx: int, idx: int):
+    assert len(values) == self.num_blocks and 0 <= block_idx < self.num_blocks and 0 <= idx < self.block_size
+    self.values = np.asarray(values, dtype=float).reshape(self.num_blocks, 1)
+    self.block_idx = block_idx
+    self.idx = idx
+
   def get(self) -> tuple[float, float, float, float]:
     valid_block_idx = [i for i in range(self.valid_blocks) if i != self.block_idx]
     valid_and_current_idx = valid_block_idx + ([self.block_idx] if self.idx > 0 else [])
@@ -178,7 +192,8 @@ class LateralLagEstimator:
                block_count: int = BLOCK_NUM, min_valid_block_count: int = BLOCK_NUM_NEEDED, block_size: int = BLOCK_SIZE,
                window_sec: float = MOVING_WINDOW_SEC, okay_window_sec: float = MIN_OKAY_WINDOW_SEC, min_recovery_buffer_sec: float = MIN_RECOVERY_BUFFER_SEC,
                min_vego: float = MIN_VEGO, min_yr: float = MIN_ABS_YAW_RATE, min_ncc: float = MIN_NCC,
-               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
+               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE,
+               speed_bucket_edges: np.ndarray | None = None):
     self.dt = dt
     self.window_sec = window_sec
     self.okay_window_sec = okay_window_sec
@@ -193,6 +208,8 @@ class LateralLagEstimator:
     self.min_confidence = min_confidence
     self.max_lat_accel = max_lat_accel
     self.max_lat_accel_diff = max_lat_accel_diff
+    self.speed_bucket_edges = np.asarray(speed_bucket_edges if speed_bucket_edges is not None else [0.0], dtype=float)
+    assert len(self.speed_bucket_edges) > 0 and self.speed_bucket_edges[0] == 0.0 and np.all(np.diff(self.speed_bucket_edges) > 0)
 
     self.t = 0.0
     self.lat_active = False
@@ -208,7 +225,9 @@ class LateralLagEstimator:
     self.last_steering_pressed_t = 0.0
     self.last_steering_saturated_t = 0.0
     self.last_pose_invalid_t = 0.0
-    self.last_estimate_t = 0.0
+    self.last_estimate_ts = [0.0] * len(self.speed_bucket_edges)
+    self.consecutive_okay = [0] * len(self.speed_bucket_edges)
+    self.learning_reset_reasons = ["not started"] * len(self.speed_bucket_edges)
 
     self.calibrator = PoseCalibrator()
 
@@ -216,8 +235,12 @@ class LateralLagEstimator:
 
   def reset(self, initial_lag: float, valid_blocks: int):
     window_len = int(self.window_sec / self.dt)
-    self.points = Points(window_len)
-    self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag)
+    self.points = [Points(window_len) for _ in self.speed_bucket_edges]
+    self.block_avgs = [BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag) for _ in self.speed_bucket_edges]
+
+  @property
+  def speed_bucket(self) -> int:
+    return max(0, int(np.searchsorted(self.speed_bucket_edges, self.v_ego, side="right") - 1))
 
   def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
     msg = messaging.new_message('liveDelay')
@@ -226,19 +249,23 @@ class LateralLagEstimator:
 
     liveDelay = msg.liveDelay
 
-    valid_mean_lag, valid_std, current_mean_lag, current_std = self.block_avg.get()
-    if self.block_avg.valid_blocks >= self.min_valid_block_count and not np.isnan(valid_mean_lag) and not np.isnan(valid_std):
-      if valid_std > MAX_LAG_STD:
-        liveDelay.status = log.LiveDelayData.Status.invalid
+    bucket_values = [avg.get() for avg in self.block_avgs]
+    bucket_statuses = []
+    bucket_applied = []
+    for avg, (valid_mean, valid_std, _, _) in zip(self.block_avgs, bucket_values, strict=True):
+      if avg.valid_blocks < self.min_valid_block_count or np.isnan(valid_mean) or np.isnan(valid_std):
+        status = "unestimated"
+      elif valid_std > MAX_LAG_STD:
+        status = "invalid"
       else:
-        liveDelay.status = log.LiveDelayData.Status.estimated
-    else:
-      liveDelay.status = log.LiveDelayData.Status.unestimated
+        status = "estimated"
+      bucket_statuses.append(status)
+      bucket_applied.append(min(MAX_LAG, max(MIN_LAG, valid_mean)) if status == "estimated" else self.initial_lag)
 
-    if liveDelay.status == log.LiveDelayData.Status.estimated:
-      liveDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
-    else:
-      liveDelay.lateralDelay = self.initial_lag
+    block_avg = self.block_avgs[self.speed_bucket]
+    _, _, current_mean_lag, current_std = bucket_values[self.speed_bucket]
+    liveDelay.status = bucket_statuses[self.speed_bucket]
+    liveDelay.lateralDelay = interpolate_bucket_values(self.v_ego, self.speed_bucket_edges, bucket_applied)
 
     if not np.isnan(current_mean_lag) and not np.isnan(current_std):
       liveDelay.lateralDelayEstimate = current_mean_lag
@@ -247,11 +274,25 @@ class LateralLagEstimator:
       liveDelay.lateralDelayEstimate = self.initial_lag
       liveDelay.lateralDelayEstimateStd = 0.0
 
-    liveDelay.validBlocks = self.block_avg.valid_blocks
-    liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) //
+    liveDelay.validBlocks = block_avg.valid_blocks
+    liveDelay.calPerc = min(100 * (block_avg.valid_blocks * self.block_size + block_avg.idx) //
                             (self.min_valid_block_count * self.block_size), 100)
+    liveDelay.speedBucket = self.speed_bucket
+    liveDelay.speedBucketEdges = self.speed_bucket_edges.tolist()
+    liveDelay.lateralDelayBuckets = [min(MAX_LAG, max(MIN_LAG, value[2])) if not np.isnan(value[2]) else self.initial_lag for value in bucket_values]
+    liveDelay.lateralDelayEstimateStdBuckets = [value[3] if not np.isnan(value[3]) else 0.0 for value in bucket_values]
+    liveDelay.validBlocksBuckets = [avg.valid_blocks for avg in self.block_avgs]
+    liveDelay.calPercBuckets = [min(100 * (avg.valid_blocks * self.block_size + avg.idx) //
+                                        (self.min_valid_block_count * self.block_size), 100) for avg in self.block_avgs]
+    liveDelay.statusBuckets = bucket_statuses
+    liveDelay.lateralDelayAppliedBuckets = bucket_applied
+    liveDelay.blockIdxBuckets = [avg.block_idx for avg in self.block_avgs]
+    liveDelay.sampleIdxBuckets = [avg.idx for avg in self.block_avgs]
+    liveDelay.blockValuesBuckets = [avg.values.flatten().tolist() for avg in self.block_avgs]
+    liveDelay.learningCountdownBuckets = [max(0.0, self.okay_window_sec - count * self.dt) for count in self.consecutive_okay]
+    liveDelay.learningResetReasonBuckets = self.learning_reset_reasons
     if debug:
-      liveDelay.points = self.block_avg.values.flatten().tolist()
+      liveDelay.points = np.concatenate([avg.values.flatten() for avg in self.block_avgs]).tolist()
     liveDelay.version = VERSION
 
     return msg
@@ -275,11 +316,12 @@ class LateralLagEstimator:
       self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
     self.t = t
 
-  def points_enough(self):
-    return self.points.num_points >= int(self.okay_window_sec / self.dt)
+  def points_enough(self, points: Points):
+    return points.num_points >= int(self.okay_window_sec / self.dt)
 
-  def points_valid(self):
-    return self.points.num_okay >= int(self.okay_window_sec / self.dt)
+  def points_valid(self, points: Points, speed_bucket: int):
+    required_points = int(self.okay_window_sec / self.dt)
+    return points.num_okay >= required_points and self.consecutive_okay[speed_bucket] >= required_points
 
   def update_points(self):
     la_desired = self.desired_curvature * self.v_ego * self.v_ego
@@ -307,17 +349,47 @@ class LateralLagEstimator:
     okay = self.lat_active and not self.steering_pressed and not self.steering_saturated and \
            fast and turning and has_recovered and calib_valid and sensors_valid and la_valid
 
-    self.points.update(self.t, la_desired, la_actual_pose, okay)
+    speed_bucket = self.speed_bucket
+    for i, points in enumerate(self.points):
+      in_bucket_okay = okay and i == speed_bucket
+      points.update(self.t, la_desired, la_actual_pose, in_bucket_okay)
+      if in_bucket_okay:
+        self.consecutive_okay[i] += 1
+        if self.consecutive_okay[i] >= int(self.okay_window_sec / self.dt):
+          self.learning_reset_reasons[i] = ""
+      else:
+        self.consecutive_okay[i] = 0
+        if i != speed_bucket:
+          self.learning_reset_reasons[i] = "outside speed band"
+        elif self.steering_pressed:
+          self.learning_reset_reasons[i] = "driver steering"
+        elif not self.lat_active:
+          self.learning_reset_reasons[i] = "lateral inactive"
+        elif self.steering_saturated:
+          self.learning_reset_reasons[i] = "steering saturated"
+        elif not calib_valid:
+          self.learning_reset_reasons[i] = "calibration invalid"
+        elif not sensors_valid:
+          self.learning_reset_reasons[i] = "pose invalid"
+        elif not la_valid:
+          self.learning_reset_reasons[i] = "lateral error"
+        elif not fast:
+          self.learning_reset_reasons[i] = "speed too low"
+        elif not turning:
+          self.learning_reset_reasons[i] = "insufficient yaw"
 
   def update_estimate(self):
-    if not self.points_enough():
+    speed_bucket = self.speed_bucket
+    points = self.points[speed_bucket]
+    if not self.points_enough(points):
       return
 
-    times, desired, actual, okay = self.points.get()
+    times, desired, actual, okay = points.get()
     # check if there are any new valid data points since the last update
-    is_valid = self.points_valid() and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
-    if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t:
-      new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t)
+    is_valid = self.points_valid(points, speed_bucket) and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
+    last_estimate_t = self.last_estimate_ts[speed_bucket]
+    if last_estimate_t != 0 and times[0] <= last_estimate_t:
+      new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= last_estimate_t)
       is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:]))
 
     desired = masked_symmetric_moving_average(desired, okay, SMOOTH_K, SMOOTH_SIGMA)
@@ -327,8 +399,8 @@ class LateralLagEstimator:
     if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
       return
 
-    self.block_avg.update(delay)
-    self.last_estimate_t = self.t
+    self.block_avgs[speed_bucket].update(delay)
+    self.last_estimate_ts[speed_bucket] = self.t
 
   @staticmethod
   def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray,
@@ -373,11 +445,24 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams):
         if last_CP.carFingerprint != CP.carFingerprint:
           raise Exception("Car model mismatch")
 
-        lag, valid_blocks, status, version = ld.lateralDelayEstimate, ld.validBlocks, ld.status, ld.version
-        assert valid_blocks <= BLOCK_NUM, "Invalid number of valid blocks"
+        lags, valid_blocks, status, version = list(ld.lateralDelayBuckets), list(ld.validBlocksBuckets), ld.status, ld.version
+        assert len(lags) == len(SPEED_BUCKET_EDGES) and len(valid_blocks) == len(SPEED_BUCKET_EDGES), "Invalid number of speed buckets"
+        assert all(blocks <= BLOCK_NUM for blocks in valid_blocks), "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
+        values = [list(v) for v in ld.blockValuesBuckets]
+        if len(values) == len(SPEED_BUCKET_EDGES):
+          block_indices, sample_indices = list(ld.blockIdxBuckets), list(ld.sampleIdxBuckets)
+          assert len(block_indices) == len(values) and len(sample_indices) == len(values), "Invalid saved bucket state"
+          assert all(len(v) == BLOCK_NUM for v in values), "Invalid saved block values"
+        else:  # migrate version 3 state saved before per-bucket progress persistence
+          values = [[lag] * BLOCK_NUM for lag in lags]
+          block_indices = [blocks % BLOCK_NUM for blocks in valid_blocks]
+          sample_indices = [0] * len(valid_blocks)
+          active_bucket = ld.speedBucket
+          accepted = round(ld.calPerc * (BLOCK_NUM_NEEDED * BLOCK_SIZE) / 100) - valid_blocks[active_bucket] * BLOCK_SIZE
+          sample_indices[active_bucket] = min(max(accepted, 0), BLOCK_SIZE - 1)
+        return list(zip(values, block_indices, sample_indices, valid_blocks, strict=True))
     except Exception as e:
       cloudlog.error(f"Failed to retrieve initial lag: {e}")
       params.remove("LiveDelay")
@@ -396,12 +481,15 @@ def main():
   params = Params()
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
 
-  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
+  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency, min_vego=0.0, speed_bucket_edges=SPEED_BUCKET_EDGES)
   if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
-    lag, valid_blocks = initial_lag_params
-    lag_learner.reset(lag, valid_blocks)
+    for avg, (values, block_idx, idx, valid_blocks) in zip(lag_learner.block_avgs, initial_lag_params, strict=True):
+      avg.valid_blocks = valid_blocks
+      avg.restore(values, block_idx, idx)
 
   lagd_toggle = LagdToggle(CP)
+  last_cache_t = 0.0
+  last_cached_progress = None
 
   while True:
     sm.update()
@@ -419,8 +507,11 @@ def main():
       lag_msg_dat = lag_msg.to_bytes()
       pm.send('liveDelay', lag_msg_dat)
 
-      if sm.frame % 1200 == 0: # cache every 60 seconds
+      progress = tuple((avg.valid_blocks, avg.idx) for avg in lag_learner.block_avgs)
+      if progress != last_cached_progress and lag_learner.t - last_cache_t >= 5.0:
         params.put("LiveDelay", lag_msg_dat)
+        last_cache_t = lag_learner.t
+        last_cached_progress = progress
 
       if sm.frame % 60 == 0:  # read from and write to params every 3 seconds
         lagd_toggle.update(lag_msg)

openpilot/selfdrive/locationd/test/test_lagd.py

@@ -1,3 +1,4 @@
+import math
 import random
 import numpy as np
 import time
@@ -6,11 +7,13 @@ 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, MIN_LAG, MAX_LAG
+                                               BLOCK_NUM, BLOCK_NUM_NEEDED, BLOCK_SIZE, MIN_OKAY_WINDOW_SEC, VERSION, MIN_LAG, MAX_LAG
+from openpilot.selfdrive.locationd.lagd import SPEED_BUCKET_EDGES, interpolate_bucket_values
 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
 from openpilot.tools.lib.logreader import LogReader
+from openpilot.tools.lagd_buckets import line_graph, ping_pong_metrics
 from openpilot.common.hardware import PC
 
 MAX_ERR_FRAMES = 1
@@ -18,9 +21,9 @@ 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):
+def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0, start_frame=0):
   for i in range(n_frames):
-    t = i * estimator.dt
+    t = (start_frame + i) * estimator.dt
     desired_la = np.cos(10 * t) * 0.3
     actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3
 
@@ -46,6 +49,30 @@ def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_thres
 
 
 class TestLagd:
+  def test_interpolate_bucket_values(self):
+    edges = np.array([0.0, 10.0, 20.0])
+    values = [.2, .4, .3]
+    assert interpolate_bucket_values(0, edges, values) == .2
+    assert interpolate_bucket_values(10, edges, values) == pytest.approx(.3)
+    assert interpolate_bucket_values(20, edges, values) == pytest.approx(.35)
+    assert interpolate_bucket_values(30, edges, values) == .3
+
+  def test_ping_pong_metrics(self):
+    samples = [(i / 20, 2.5 * math.sin(2 * math.pi * 1.2 * i / 20)) for i in range(101)]
+    severity, amplitude, frequency, duration = ping_pong_metrics(samples)
+    assert severity == "MODERATE"
+    assert 2.4 < amplitude < 2.6
+    assert 1.0 < frequency < 1.3
+    assert duration == 5
+
+  def test_line_graph(self):
+    graph = line_graph([0.15, 0.4, 0.65], ["estimated", "unestimated", "invalid"], 1)
+    assert all(marker in graph for marker in ["●", "◆", "×"])
+    assert "0.65s" in graph and "0.15s" in graph
+    assert "centers" in graph and "5" in graph
+    assert any("⠀" < char <= "⣿" for char in graph)
+    assert "⠒" in line_graph([0.3, 0.3], ["estimated", "estimated"], -1)
+
   def test_read_saved_params(self):
     params = Params()
 
@@ -53,8 +80,11 @@ class TestLagd:
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
     msg = messaging.new_message('liveDelay')
-    msg.liveDelay.lateralDelayEstimate = random.random()
-    msg.liveDelay.validBlocks = random.randint(1, 10)
+    msg.liveDelay.lateralDelayBuckets = [random.random() for _ in SPEED_BUCKET_EDGES]
+    msg.liveDelay.validBlocksBuckets = [random.randint(1, 10) for _ in SPEED_BUCKET_EDGES]
+    msg.liveDelay.blockValuesBuckets = [[random.random() for _ in range(BLOCK_NUM)] for _ in SPEED_BUCKET_EDGES]
+    msg.liveDelay.blockIdxBuckets = [random.randrange(BLOCK_NUM) for _ in SPEED_BUCKET_EDGES]
+    msg.liveDelay.sampleIdxBuckets = [random.randrange(BLOCK_SIZE) for _ in SPEED_BUCKET_EDGES]
     msg.liveDelay.version = VERSION
     params.put("LiveDelay", msg.to_bytes(), block=True)
     params.put("CarParamsPrevRoute", CP.as_builder().to_bytes(), block=True)
@@ -62,9 +92,20 @@ class TestLagd:
     saved_lag_params = retrieve_initial_lag(params, CP)
     assert saved_lag_params is not None
 
-    lag, valid_blocks = saved_lag_params
-    assert lag == msg.liveDelay.lateralDelayEstimate
-    assert valid_blocks == msg.liveDelay.validBlocks
+    for state, values, block_idx, sample_idx, valid_blocks in zip(saved_lag_params, msg.liveDelay.blockValuesBuckets,
+                                                                  msg.liveDelay.blockIdxBuckets, msg.liveDelay.sampleIdxBuckets,
+                                                                  msg.liveDelay.validBlocksBuckets, strict=True):
+      assert np.allclose(state[0], values)
+      assert state[1:] == (block_idx, sample_idx, valid_blocks)
+
+    old_msg = messaging.new_message('liveDelay')
+    old_msg.liveDelay.version = VERSION
+    old_msg.liveDelay.speedBucket = 4
+    old_msg.liveDelay.calPerc = 12
+    old_msg.liveDelay.lateralDelayBuckets = [0.3] * len(SPEED_BUCKET_EDGES)
+    old_msg.liveDelay.validBlocksBuckets = [0] * len(SPEED_BUCKET_EDGES)
+    params.put("LiveDelay", old_msg.to_bytes(), block=True)
+    assert retrieve_initial_lag(params, CP)[4][2] == 3
 
   def test_read_invalid_saved_params(self, subtests):
     params = Params()
@@ -72,7 +113,9 @@ class TestLagd:
     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}]:
+    valid = {'version': VERSION, 'lateralDelayBuckets': [0.3] * len(SPEED_BUCKET_EDGES),
+             'validBlocksBuckets': [1] * len(SPEED_BUCKET_EDGES)}
+    for msg_dict in [valid | {'version': 0}, valid | {'status': 'invalid'}, valid | {'validBlocksBuckets': [100] * 4}]:
       with subtests.test(msg=f"liveDelay={msg_dict}"):
         msg = messaging.new_message('liveDelay')
         msg.liveDelay = msg_dict
@@ -131,12 +174,40 @@ class TestLagd:
   def test_estimator_masking(self):
     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)
+    masked_frames = int(MIN_OKAY_WINDOW_SEC / DT)
+    process_messages(estimator, lag_frames, masked_frames, rejection_threshold=0.4)
+    process_messages(estimator, lag_frames, masked_frames + BLOCK_SIZE * 2, start_frame=masked_frames)
     msg = estimator.get_msg(True)
     assert np.allclose(msg.liveDelay.lateralDelayEstimate, lag_frames * DT, atol=0.01)
     assert np.allclose(msg.liveDelay.lateralDelayEstimateStd, 0.0, atol=0.01)
     assert msg.liveDelay.calPerc == 100
 
+  def test_learning_countdown_resets(self):
+    mocked_CP = car.CarParams(steerActuatorDelay=0.3)
+    estimator = LateralLagEstimator(mocked_CP, DT, window_sec=10.0, okay_window_sec=5.0,
+                                    min_recovery_buffer_sec=0.0, min_vego=0.0, min_yr=0.0)
+    process_messages(estimator, 5, 80)
+    assert np.allclose(estimator.get_msg(True).liveDelay.learningCountdownBuckets, [1.0])
+
+    process_messages(estimator, 5, 1, rejection_threshold=1.0, start_frame=80)
+    msg = estimator.get_msg(True).liveDelay
+    assert np.allclose(msg.learningCountdownBuckets, [5.0])
+    assert list(msg.learningResetReasonBuckets) == ["lateral inactive"]
+
+  def test_speed_buckets(self):
+    mocked_CP = car.CarParams(steerActuatorDelay=0.3)
+    estimator = LateralLagEstimator(mocked_CP, DT, block_size=10, min_valid_block_count=1, window_sec=5.0,
+                                    okay_window_sec=2.0, min_recovery_buffer_sec=0.0, min_vego=0.0, min_yr=0.0,
+                                    speed_bucket_edges=np.array([0.0, 20.0]))
+    frame_count = int(5.0 / DT) + 10
+    process_messages(estimator, 5, frame_count, vego=15.0)
+    process_messages(estimator, 9, frame_count, vego=25.0, start_frame=frame_count)
+
+    msg = estimator.get_msg(True).liveDelay
+    assert msg.speedBucket == 1
+    assert np.allclose(msg.lateralDelayBuckets, [5 * DT, 9 * DT], atol=0.01)
+    assert all(blocks > 0 for blocks in msg.validBlocksBuckets)
+
   @pytest.mark.skipif(PC, reason="only on device")
   def test_estimator_performance(self):
     mocked_CP = car.CarParams(steerActuatorDelay=0.5)

openpilot/selfdrive/modeld/models/big_driving_supercombo.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a501760a9d1d5fef0eab2b8c5d122d06124fc26dc8e0782e0aa94b82a208f0ff
+size 1757355221

openpilot/selfdrive/modeld/models/driving_policy.onnx

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:2b85e82079a2d31c5ce8616f2b429ccfcdcb8ebb5aefd72a62b8bd78aa9c7621
-size 15583592

openpilot/selfdrive/modeld/models/driving_supercombo.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:659727c4d4839adc4992a254409a54259a8756a743f2d567bf5fdc6579f8009b
+size 60881999

openpilot/selfdrive/modeld/models/driving_vision.onnx

@@ -1,3 +0,0 @@
-version https://git-lfs.github.com/spec/v1
-oid sha256:c824f68646a3b94f117f01c70dc8316fb466e05fbd42ccdba440b8a8dc86914b
-size 46265993

openpilot/tools/lagd_buckets.py

@@ -0,0 +1,182 @@
+#!/usr/bin/env python3
+from collections import deque
+import math
+import time
+
+from openpilot.cereal import messaging
+from openpilot.common.constants import CV
+from openpilot.common.params import Params
+from opendbc.car.structs import car
+from opendbc.car.vehicle_model import VehicleModel
+
+
+PING_PONG_WINDOW = 5.0
+PING_PONG_MIN_WINDOW = 3.0
+PING_PONG_ERROR_DEG = 1.0
+
+
+def ping_pong_metrics(samples: list[tuple[float, float]]) -> tuple[str, float, float, float]:
+  duration = samples[-1][0] - samples[0][0] if len(samples) > 1 else 0.0
+  if duration < PING_PONG_MIN_WINDOW:
+    return "COLLECTING", 0.0, 0.0, duration
+
+  values = [sample[1] for sample in samples]
+  center = sum(values) / len(values)
+  residual = [value - center for value in values]
+  amplitude = (max(residual) - min(residual)) / 2
+  state = 0
+  transition_times = []
+  for (sample_time, _), value in zip(samples, residual, strict=True):
+    new_state = 1 if value >= PING_PONG_ERROR_DEG else -1 if value <= -PING_PONG_ERROR_DEG else state
+    if state and new_state != state:
+      transition_times.append(sample_time)
+    state = new_state
+  transition_duration = transition_times[-1] - transition_times[0] if len(transition_times) > 1 else 0.0
+  frequency = (len(transition_times) - 1) / (2 * transition_duration) if transition_duration else 0.0
+
+  if frequency < 1 or amplitude < PING_PONG_ERROR_DEG:
+    severity = "NONE"
+  elif amplitude < 2:
+    severity = "MILD"
+  elif amplitude < 3:
+    severity = "MODERATE"
+  else:
+    severity = "SEVERE"
+  return severity, amplitude, frequency, duration
+
+
+def line_graph(values, statuses, active_bucket: int, edges=None, low: float = .15, high: float = .65, height: int = 11, step: int = 8) -> str:
+  width = step * len(values) + 1
+  dot_height = height * 4
+  dots = set()
+  value_rows = [round((high - max(low, min(high, value))) / (high - low) * (dot_height - 1)) for value in values]
+  marker_rows = [min(row // 4, height - 1) for row in value_rows]
+  rows = [row * 4 + 1 for row in marker_rows]
+  point_cols = [i * step + step // 2 for i in range(len(rows))]
+  point_xs = [col * 2 for col in point_cols]
+
+  knot_xs = [0, *point_xs, width * 2 - 2]
+  knot_rows = [rows[0], *rows, rows[-1]]
+  for i in range(len(knot_rows) - 1):
+    x0, x1 = knot_xs[i], knot_xs[i + 1]
+    for x in range(x0, x1 + 1):
+      row = round(knot_rows[i] + (knot_rows[i + 1] - knot_rows[i]) * (x - x0) / (x1 - x0))
+      dots.add((x, row))
+
+  braille_bits = ((0, 3), (1, 4), (2, 5), (6, 7))
+  grid = [[" " for _ in range(width)] for _ in range(height)]
+  for cell_row in range(height):
+    for cell_col in range(width):
+      bits = 0
+      for dot_row in range(4):
+        for dot_col in range(2):
+          if (cell_col * 2 + dot_col, cell_row * 4 + dot_row) in dots:
+            bits |= 1 << braille_bits[dot_row][dot_col]
+      if bits:
+        grid[cell_row][cell_col] = chr(0x2800 + bits)
+
+  markers = {"unestimated": "○", "estimated": "●", "invalid": "×"}
+  for i, (col, row, status) in enumerate(zip(point_cols, marker_rows, statuses, strict=True)):
+    grid[row][col] = "◆" if i == active_bucket else markers[str(status)]
+
+  chart = []
+  for row, cells in enumerate(grid):
+    value = high - row * (high - low) / (height - 1)
+    chart.append(f" {value:.2f}s │{''.join(cells)}")
+  axis_edges = edges if edges is not None else [i * 10 for i in range(len(values))]
+  axis = ["─" for _ in range(width)]
+  for i in range(1, len(axis_edges)):
+    axis[i * step] = "┬"
+  chart.append("       └" + "".join(axis))
+  labels = [" " for _ in range(width)]
+  for i, edge in enumerate(axis_edges):
+    label = f"{edge:.0f}+" if i == len(axis_edges) - 1 else f"{edge:.0f}"
+    start = min(i * step, width - len(label))
+    labels[start:start + len(label)] = label
+  chart.append(" edges  " + "".join(labels) + " mph")
+  center_labels = [" " for _ in range(width)]
+  widths = [axis_edges[i + 1] - edge for i, edge in enumerate(axis_edges[:-1])]
+  widths.append(widths[-1] if widths else 0)
+  for col, edge, bucket_width in zip(point_cols, axis_edges, widths, strict=True):
+    label = f"{edge + bucket_width / 2:.0f}"
+    start = max(0, min(col - len(label) // 2, width - len(label)))
+    center_labels[start:start + len(label)] = label
+  chart.append(" centers" + "".join(center_labels) + " mph (dots)")
+  chart.append("        ● READY   ○ LEARNING   × UNSTABLE   ◆ ACTIVE")
+  return "\n".join(chart)
+
+
+def main() -> None:
+  CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
+  VM = VehicleModel(CP)
+  sm = messaging.SubMaster(["carControl", "carState", "controlsState", "liveCalibration", "liveDelay", "liveParameters"])
+  ping_pong_samples: deque[tuple[float, float]] = deque()
+  ping_pong_waiting = "lateral inactive"
+  last_sample = 0.0
+  next_redraw = 0.0
+  print("\033[2J", end="")
+  while True:
+    sm.update(500)
+    now = time.monotonic()
+    speed_mps = sm["carState"].vEgo
+    lp = sm["liveParameters"]
+    VM.update_params(max(lp.stiffnessFactor, .1), max(lp.steerRatio, .1))
+    desired_angle = math.degrees(VM.get_steer_from_curvature(-sm["controlsState"].desiredCurvature, speed_mps, lp.roll))
+    actual_angle = sm["carState"].steeringAngleDeg - lp.angleOffsetDeg
+    if now - last_sample >= .05:
+      if not sm["carControl"].latActive:
+        ping_pong_waiting = "lateral inactive"
+      elif sm["carState"].steeringPressed:
+        ping_pong_waiting = "driver steering"
+      elif speed_mps < 2:
+        ping_pong_waiting = "speed below 5 mph"
+      else:
+        ping_pong_waiting = ""
+        ping_pong_samples.append((now, desired_angle - actual_angle))
+        while ping_pong_samples and now - ping_pong_samples[0][0] > PING_PONG_WINDOW:
+          ping_pong_samples.popleft()
+      if ping_pong_waiting:
+        ping_pong_samples.clear()
+      last_sample = now
+
+    if not sm.alive["liveDelay"] or now < next_redraw:
+      continue
+    next_redraw = now + .25
+
+    ld = sm["liveDelay"]
+    edges = [edge * CV.MS_TO_MPH for edge in ld.speedBucketEdges]
+    speed = sm["carState"].vEgo * CV.MS_TO_MPH
+    calibration = sm["liveCalibration"].calStatus
+    lines = [f"speed {speed:5.1f} mph   calibration {calibration}   status {ld.status}   applied {ld.lateralDelay:.3f} s"]
+    severity, amplitude, frequency, duration = ping_pong_metrics(list(ping_pong_samples))
+    ping_pong = f"PING-PONG {severity:<10}  angle-error amplitude ±{amplitude:.2f}°   {frequency:.2f} Hz   window {duration:.0f}s"
+    if ping_pong_waiting:
+      ping_pong = f"PING-PONG WAITING     {ping_pong_waiting}"
+    elif severity == "COLLECTING":
+      ping_pong = f"PING-PONG COLLECTING  {duration:.0f}/{PING_PONG_MIN_WINDOW:.0f}s of steering-angle error"
+    lines.extend([ping_pong, "", "APPLIED LAG BY SPEED (INTERPOLATED)",
+                  *line_graph(ld.lateralDelayAppliedBuckets, ld.statusBuckets, ld.speedBucket, edges).splitlines(), "",
+                  "    range       estimate   applied    std    blocks   progress   starts in   state      last reset"])
+    for i, (estimate, applied, std, blocks, percent, countdown, status, reason) in enumerate(zip(ld.lateralDelayBuckets,
+                                                                                                 ld.lateralDelayAppliedBuckets,
+                                                                                                 ld.lateralDelayEstimateStdBuckets,
+                                                                                                 ld.validBlocksBuckets,
+                                                                                                 ld.calPercBuckets,
+                                                                                                 ld.learningCountdownBuckets,
+                                                                                                 ld.statusBuckets,
+                                                                                                 ld.learningResetReasonBuckets,
+                                                                                                 strict=True)):
+      speed_range = f"{edges[i]:.0f}-{edges[i + 1]:.0f} mph" if i + 1 < len(edges) else f"{edges[i]:.0f}+ mph"
+      active = "*" if i == ld.speedBucket else " "
+      status_name = str(status)
+      state = {"unestimated": "LEARNING", "estimated": "READY", "invalid": "UNSTABLE"}[status_name]
+      row = f" {active} {speed_range:<9}   {estimate:.3f} s   {applied:.3f} s   {std:.3f}"
+      waiting = "  --" if status_name == "estimated" else f"{countdown:4.1f}s"
+      reset_reason = "--" if status_name == "estimated" else (reason or "--")
+      row += f"     {blocks:2}       {percent:3}%        {waiting}     {state:<9}  {reset_reason}"
+      lines.append(row)
+    print("\033[H" + "\033[K\n".join(lines) + "\033[K\033[J", end="", flush=True)
+
+
+if __name__ == "__main__":
+  main()