Kumars Vibe

openpilot/cereal/log.capnp

@@ -2309,19 +2309,6 @@ 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,21 +16,13 @@ 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 = 8
-BLOCK_NUM = 30
-BLOCK_NUM_NEEDED = 3
-MOVING_WINDOW_SEC = 20.0
-MIN_OKAY_WINDOW_SEC = 5.0
+BLOCK_SIZE = 100
+BLOCK_NUM = 50
+BLOCK_NUM_NEEDED = 5
+MOVING_WINDOW_SEC = 60.0
+MIN_OKAY_WINDOW_SEC = 25.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
@@ -39,14 +31,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.1
+MIN_LAT_ACCEL_RANGE = 0.5
 MIN_CONFIDENCE = 0.7
 CORR_BORDER_OFFSET = 5
 LAG_CANDIDATE_CORR_THRESHOLD = 0.9
 SMOOTH_K = 5
 SMOOTH_SIGMA = 1.0
 
-VERSION = 3  # bump this to invalidate old parameter caches
+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:
@@ -160,12 +152,6 @@ 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 [])
@@ -192,8 +178,7 @@ 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,
-               speed_bucket_edges: np.ndarray | None = None):
+               max_lat_accel: float = MAX_LAT_ACCEL, max_lat_accel_diff: float = MAX_LAT_ACCEL_DIFF, min_confidence: float = MIN_CONFIDENCE):
     self.dt = dt
     self.window_sec = window_sec
     self.okay_window_sec = okay_window_sec
@@ -208,8 +193,6 @@ 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
@@ -225,9 +208,7 @@ 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_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.last_estimate_t = 0.0
 
     self.calibrator = PoseCalibrator()
 
@@ -235,12 +216,8 @@ class LateralLagEstimator:
 
   def reset(self, initial_lag: float, valid_blocks: int):
     window_len = int(self.window_sec / self.dt)
-    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))
+    self.points = Points(window_len)
+    self.block_avg = BlockAverage(self.block_count, self.block_size, valid_blocks, initial_lag)
 
   def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder:
     msg = messaging.new_message('liveDelay')
@@ -249,23 +226,19 @@ class LateralLagEstimator:
 
     liveDelay = msg.liveDelay
 
-    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"
+    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
       else:
-        status = "estimated"
-      bucket_statuses.append(status)
-      bucket_applied.append(min(MAX_LAG, max(MIN_LAG, valid_mean)) if status == "estimated" else self.initial_lag)
+        liveDelay.status = log.LiveDelayData.Status.estimated
+    else:
+      liveDelay.status = log.LiveDelayData.Status.unestimated
 
-    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 liveDelay.status == log.LiveDelayData.Status.estimated:
+      liveDelay.lateralDelay = min(MAX_LAG, max(MIN_LAG, valid_mean_lag))
+    else:
+      liveDelay.lateralDelay = self.initial_lag
 
     if not np.isnan(current_mean_lag) and not np.isnan(current_std):
       liveDelay.lateralDelayEstimate = current_mean_lag
@@ -274,25 +247,11 @@ class LateralLagEstimator:
       liveDelay.lateralDelayEstimate = self.initial_lag
       liveDelay.lateralDelayEstimateStd = 0.0
 
-    liveDelay.validBlocks = block_avg.valid_blocks
-    liveDelay.calPerc = min(100 * (block_avg.valid_blocks * self.block_size + block_avg.idx) //
+    liveDelay.validBlocks = self.block_avg.valid_blocks
+    liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.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 = np.concatenate([avg.values.flatten() for avg in self.block_avgs]).tolist()
+      liveDelay.points = self.block_avg.values.flatten().tolist()
     liveDelay.version = VERSION
 
     return msg
@@ -316,12 +275,11 @@ class LateralLagEstimator:
       self.pose_valid = msg.angularVelocityDevice.valid and msg.posenetOK and msg.inputsOK
     self.t = t
 
-  def points_enough(self, points: Points):
-    return points.num_points >= int(self.okay_window_sec / self.dt)
+  def points_enough(self):
+    return self.points.num_points >= 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 points_valid(self):
+    return self.points.num_okay >= int(self.okay_window_sec / self.dt)
 
   def update_points(self):
     la_desired = self.desired_curvature * self.v_ego * self.v_ego
@@ -349,47 +307,17 @@ 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
 
-    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"
+    self.points.update(self.t, la_desired, la_actual_pose, okay)
 
   def update_estimate(self):
-    speed_bucket = self.speed_bucket
-    points = self.points[speed_bucket]
-    if not self.points_enough(points):
+    if not self.points_enough():
       return
 
-    times, desired, actual, okay = points.get()
+    times, desired, actual, okay = self.points.get()
     # check if there are any new valid data points since the last update
-    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 = 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 = 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)
@@ -399,8 +327,8 @@ class LateralLagEstimator:
     if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
       return
 
-    self.block_avgs[speed_bucket].update(delay)
-    self.last_estimate_ts[speed_bucket] = self.t
+    self.block_avg.update(delay)
+    self.last_estimate_t = self.t
 
   @staticmethod
   def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray,
@@ -445,24 +373,11 @@ def retrieve_initial_lag(params: Params, CP: car.CarParams):
         if last_CP.carFingerprint != CP.carFingerprint:
           raise Exception("Car model mismatch")
 
-        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"
+        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})"
-        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))
+        return lag, valid_blocks
     except Exception as e:
       cloudlog.error(f"Failed to retrieve initial lag: {e}")
       params.remove("LiveDelay")
@@ -481,15 +396,12 @@ 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, min_vego=0.0, speed_bucket_edges=SPEED_BUCKET_EDGES)
+  lag_learner = LateralLagEstimator(CP, 1. / SERVICE_LIST['livePose'].frequency)
   if (initial_lag_params := retrieve_initial_lag(params, CP)) is not None:
-    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)
+    lag, valid_blocks = initial_lag_params
+    lag_learner.reset(lag, valid_blocks)
 
   lagd_toggle = LagdToggle(CP)
-  last_cache_t = 0.0
-  last_cached_progress = None
 
   while True:
     sm.update()
@@ -507,11 +419,8 @@ def main():
       lag_msg_dat = lag_msg.to_bytes()
       pm.send('liveDelay', lag_msg_dat)
 
-      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:
+      if sm.frame % 1200 == 0: # cache every 60 seconds
         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,4 +1,3 @@
-import math
 import random
 import numpy as np
 import time
@@ -7,13 +6,11 @@ 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, 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
+                                               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
 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
@@ -21,9 +18,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, start_frame=0):
+def process_messages(estimator, lag_frames, n_frames, vego=25.0, rejection_threshold=0.0):
   for i in range(n_frames):
-    t = (start_frame + i) * estimator.dt
+    t = i * estimator.dt
     desired_la = np.cos(10 * t) * 0.3
     actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3
 
@@ -49,30 +46,6 @@ 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()
 
@@ -80,11 +53,8 @@ class TestLagd:
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
     msg = messaging.new_message('liveDelay')
-    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.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)
@@ -92,20 +62,9 @@ class TestLagd:
     saved_lag_params = retrieve_initial_lag(params, CP)
     assert saved_lag_params is not None
 
-    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
+    lag, valid_blocks = saved_lag_params
+    assert lag == msg.liveDelay.lateralDelayEstimate
+    assert valid_blocks == msg.liveDelay.validBlocks
 
   def test_read_invalid_saved_params(self, subtests):
     params = Params()
@@ -113,9 +72,7 @@ class TestLagd:
     lr = migrate(LogReader(TEST_ROUTE), [migrate_carParams])
     CP = next(m for m in lr if m.which() == "carParams").carParams
 
-    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}]:
+    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
@@ -174,40 +131,12 @@ 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)
-    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)
+    process_messages(estimator, lag_frames, (int(MIN_OKAY_WINDOW_SEC / DT) + BLOCK_SIZE) * 2, rejection_threshold=0.4)
     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

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

openpilot/selfdrive/modeld/models/driving_policy.onnx

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

openpilot/selfdrive/modeld/models/driving_supercombo.onnx

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

openpilot/selfdrive/modeld/models/driving_vision.onnx

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

openpilot/tools/lagd_buckets.py

@@ -1,182 +0,0 @@
-#!/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()