bett

* relc

* so picky

* clean up

* switch to distance based

* same calls

* all you brother i'm just here to red diff

---------

Co-authored-by: Jason Wen <haibin.wen3@gmail.com>

openpilot/cereal/custom.capnp

@@ -351,6 +351,7 @@ struct OnroadEventSP @0xda96579883444c35 {
     speedLimitChanged @21;
     speedLimitPending @22;
     e2eChime @23;
+    laneChangeRoadEdge @24;
   }
 }
 
@@ -457,6 +458,8 @@ struct LiveMapDataSP @0xf416ec09499d9d19 {
 
 struct ModelDataV2SP @0xa1680744031fdb2d {
   laneTurnDirection @0 :TurnDirection;
+  leftLaneChangeEdgeBlock @1 :Bool;
+  rightLaneChangeEdgeBlock @2 :Bool;
 
   enum TurnDirection {
     none @0;

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/common/params_keys.h

@@ -179,6 +179,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"RoadEdgeLaneChangeEnabled", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"ScreenSaverEnabled", {PERSISTENT | BACKUP, BOOL, "1"}},
     {"ScreenSaverTimeout", {PERSISTENT | BACKUP, INT, "300"}},

openpilot/selfdrive/controls/lib/desire_helper.py

@@ -33,7 +33,7 @@ class DesireHelper:
   def get_lane_change_direction(CS):
     return LaneChangeDirection.left if CS.leftBlinker else LaneChangeDirection.right
 
-  def update(self, carstate, lateral_active, lane_change_prob):
+  def update(self, carstate, lateral_active, lane_change_prob, left_edge_detected=False, right_edge_detected=False):
     self.alc.update_params()
     self.lane_turn_controller.update_params()
     v_ego = carstate.vEgo
@@ -64,8 +64,8 @@ class DesireHelper:
                          ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
                           (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
 
-        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
-                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
+        blindspot_detected = (((carstate.leftBlindspot or left_edge_detected) and self.lane_change_direction == LaneChangeDirection.left) or
+                              ((carstate.rightBlindspot or right_edge_detected) and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

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/modeld.py

@@ -29,6 +29,7 @@ from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link
 
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
@@ -213,6 +214,7 @@ def main(demo=False):
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
+  RELC = RoadEdgeLaneChangeController()
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -313,7 +315,8 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -327,9 +327,16 @@ class SelfdriveD(CruiseHelper):
     # Handle lane change
     if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange:
       direction = self.sm['modelV2'].meta.laneChangeDirection
+      mdv2sp = self.sm['modelDataV2SP']
+
       if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \
          (CS.rightBlindspot and direction == LaneChangeDirection.right):
         self.events.add(EventName.laneChangeBlocked)
+
+      elif (mdv2sp.leftLaneChangeEdgeBlock and direction == LaneChangeDirection.left) or \
+           (mdv2sp.rightLaneChangeEdgeBlock and direction == LaneChangeDirection.right):
+        self.events_sp.add(custom.OnroadEventSP.EventName.laneChangeRoadEdge)
+
       else:
         if direction == LaneChangeDirection.left:
           self.events.add(EventName.preLaneChangeLeft)

openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/lane_change_settings.py

@@ -51,11 +51,18 @@ class LaneChangeSettingsLayout(Widget):
       description=lambda: tr("Toggle to enable a delay timer for seamless lane changes when blind spot monitoring " +
                              "(BSM) detects a obstructing vehicle, ensuring safe maneuvering."),
     )
+    self._road_edge_block = toggle_item_sp(
+      param="RoadEdgeLaneChangeEnabled",
+      title=lambda: tr("Block Lane Change: Road Edge Detection"),
+      description=lambda: tr("Blocks the lane change if the model sees a road edge on your signaled side."),
+    )
 
     items = [
       self._lane_change_timer,
       LineSeparatorSP(40),
       self._bsm_delay,
+      LineSeparatorSP(40),
+      self._road_edge_block,
     ]
 
     return items

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -49,6 +49,7 @@ from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, mak
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import RoadEdgeLaneChangeController
 
 PROCESS_NAME = "openpilot.selfdrive.modeld.modeld_tinygrad"
 
@@ -367,6 +368,7 @@ def main(demo=False):
 
   DH = DesireHelper()
   meta_constants = load_meta_constants()
+  RELC = RoadEdgeLaneChangeController()
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -479,7 +481,8 @@ def main(demo=False):
       l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
       r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
       lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
+      left_edge, right_edge = RELC.update_and_fill(modelv2_send.modelV2, mdv2sp_send.modelDataV2SP, v_ego)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, left_edge, right_edge)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
       mdv2sp_send.modelDataV2SP.laneTurnDirection = DH.lane_turn_direction

openpilot/sunnypilot/selfdrive/controls/lib/dec/dec.py

@@ -1,5 +1,5 @@
 """
-Copyright (c) 2021-, rav4kumar, Haibin Wen, sunnypilot, and a number of other contributors.
+Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
 
 This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.

openpilot/sunnypilot/selfdrive/controls/lib/relc.py

@@ -0,0 +1,98 @@
+"""
+Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
+from openpilot.common.params import Params
+
+NEARSIDE_PROB = 0.2
+EDGE_PROB = 0.35
+EDGE_REACTION_TIME = 1.0
+EDGE_CLEAR_TIME = 0.3
+MIN_SPEED = 20 * CV.MPH_TO_MS
+VEHICLE_EDGE_MARGIN = 1.08
+EDGE_CLEARANCE = 3.7
+
+
+class RoadEdgeLaneChangeController:
+  def __init__(self):
+    self.params = Params()
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+    self.param_read_counter = 0
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def read_params(self) -> None:
+    self.enabled = self.params.get_bool("RoadEdgeLaneChangeEnabled")
+
+  def update_params(self) -> None:
+    if self.param_read_counter % 50 == 0:
+      self.read_params()
+    self.param_read_counter += 1
+
+  def reset(self) -> None:
+    self.left_edge_detected = False
+    self.right_edge_detected = False
+    self.left_edge_timer = 0.0
+    self.right_edge_timer = 0.0
+    self.left_clear_timer = 0.0
+    self.right_clear_timer = 0.0
+
+  def update(self, road_edge_stds, lane_line_probs, v_ego: float, road_edges=None) -> None:
+    self.update_params()
+
+    if not self.enabled or v_ego < MIN_SPEED:
+      self.reset()
+      return
+
+    left_edge_prob = np.clip(1.0 - road_edge_stds[0], 0.0, 1.0)
+    right_edge_prob = np.clip(1.0 - road_edge_stds[1], 0.0, 1.0)
+    left_lane_prob = lane_line_probs[0]
+    right_lane_prob = lane_line_probs[3]
+
+    if road_edges is not None and len(road_edges) == 2 and len(road_edges[0].y) > 0 and len(road_edges[1].y) > 0:
+      left_clearance = abs(road_edges[0].y[0]) - VEHICLE_EDGE_MARGIN
+      right_clearance = abs(road_edges[1].y[0]) - VEHICLE_EDGE_MARGIN
+    else:
+      left_clearance = 0.0
+      right_clearance = 0.0
+
+    left_cond = left_edge_prob > EDGE_PROB and left_lane_prob < NEARSIDE_PROB and left_clearance < EDGE_CLEARANCE
+    right_cond = right_edge_prob > EDGE_PROB and right_lane_prob < NEARSIDE_PROB and right_clearance < EDGE_CLEARANCE
+
+    if left_cond:
+      self.left_edge_timer = min(self.left_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.left_clear_timer = 0.0
+      if self.left_edge_timer > EDGE_REACTION_TIME:
+        self.left_edge_detected = True
+    else:
+      self.left_clear_timer += DT_MDL
+      if self.left_clear_timer > EDGE_CLEAR_TIME:
+        self.left_edge_timer = 0.0
+        self.left_edge_detected = False
+
+    if right_cond:
+      self.right_edge_timer = min(self.right_edge_timer + DT_MDL, EDGE_REACTION_TIME + EDGE_CLEAR_TIME)
+      self.right_clear_timer = 0.0
+      if self.right_edge_timer > EDGE_REACTION_TIME:
+        self.right_edge_detected = True
+    else:
+      self.right_clear_timer += DT_MDL
+      if self.right_clear_timer > EDGE_CLEAR_TIME:
+        self.right_edge_timer = 0.0
+        self.right_edge_detected = False
+
+  def update_and_fill(self, modelv2, mdv2sp, v_ego):
+    self.update(modelv2.roadEdgeStds, modelv2.laneLineProbs, v_ego, modelv2.roadEdges)
+    mdv2sp.leftLaneChangeEdgeBlock = self.left_edge_detected
+    mdv2sp.rightLaneChangeEdgeBlock = self.right_edge_detected
+    return self.left_edge_detected, self.right_edge_detected

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_lane_turn_desire.py

@@ -2,10 +2,11 @@ import pytest
 from openpilot.cereal import log, custom
 from openpilot.common.params import Params
 
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
 from openpilot.sunnypilot.selfdrive.controls.lib.lane_turn_desire import LaneTurnController, LANE_CHANGE_SPEED_MIN
 from openpilot.sunnypilot.selfdrive.controls.lib.auto_lane_change import AutoLaneChangeMode
 
+
 TurnDirection = custom.ModelDataV2SP.TurnDirection
 
 
@@ -109,5 +110,17 @@ def test_desire_helper_integration(carstate, lateral_active, lane_change_prob, e
   dh = DesireHelper()
   dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
   for _ in range(10):
-    dh.update(carstate, lateral_active, lane_change_prob)
-  assert dh.desire == expected_desire  # The first four tests were unit tests to test the controller, where this tests the integration in desire helpers
+    dh.update(carstate, lateral_active, lane_change_prob,
+              left_edge_detected=False, right_edge_detected=False)
+  assert dh.desire == expected_desire
+
+
+def test_edge_blocks_lane_change(set_lane_turn_params):
+  dh = DesireHelper()
+  dh.alc.lane_change_set_timer = AutoLaneChangeMode.NUDGE
+  carstate = DummyCarState(vEgo=15, leftBlinker=True, steeringPressed=True, steeringTorque=1)
+  for _ in range(10):
+    dh.update(carstate, True, 1.0, left_edge_detected=True, right_edge_detected=False)
+  assert dh.lane_change_state == LaneChangeState.preLaneChange
+  assert dh.lane_change_direction == LaneChangeDirection.left
+  assert dh.desire == log.Desire.none

openpilot/sunnypilot/selfdrive/controls/lib/tests/test_relc.py

@@ -0,0 +1,169 @@
+"""
+Copyright (c) 2021-, rav4kumar, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+import pytest
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.sunnypilot.selfdrive.controls.lib.relc import (
+  RoadEdgeLaneChangeController, EDGE_REACTION_TIME, EDGE_CLEAR_TIME, MIN_SPEED,
+  VEHICLE_EDGE_MARGIN, EDGE_CLEARANCE,
+)
+
+V_HIGH = MIN_SPEED + 2.0
+V_LOW = MIN_SPEED - 1.0
+
+
+class MockEdge:
+  def __init__(self, y_val):
+    self.y = [y_val] * 33
+
+
+def edges(left_y, right_y):
+  return [MockEdge(left_y), MockEdge(right_y)]
+
+
+CLOSE_EDGES = edges(-2.0, 1.5)
+FAR_EDGES = edges(-10.0, 10.0)
+
+
+@pytest.fixture
+def relc(mocker):
+  mocker.patch("openpilot.sunnypilot.selfdrive.controls.lib.relc.Params")
+  controller = RoadEdgeLaneChangeController()
+  controller.enabled = True
+  return controller
+
+
+def drive(controller, road_edge_stds, lane_line_probs, seconds, v_ego=V_HIGH, road_edges=CLOSE_EDGES):
+  for _ in range(int(seconds / DT_MDL) + 1):
+    controller.update(road_edge_stds, lane_line_probs, v_ego, road_edges)
+
+
+@pytest.mark.parametrize("road_edge_stds,lane_line_probs,attr", [
+  ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], "left_edge_detected"),
+  ([0.9, 0.0], [0.8, 0.8, 0.8, 0.0], "right_edge_detected"),
+])
+def test_edge_detection(relc, road_edge_stds, lane_line_probs, attr):
+  drive(relc, road_edge_stds, lane_line_probs, EDGE_REACTION_TIME + 0.1)
+  assert getattr(relc, attr)
+
+
+def test_edge_detection_requires_time(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME - 0.05)
+  assert not relc.left_edge_detected
+
+
+def test_both_edges_detected(relc):
+  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+  assert relc.right_edge_detected
+
+
+def test_noise_doesnt_clear(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update(*clear, V_HIGH, CLOSE_EDGES)
+  relc.update(*edge, V_HIGH, CLOSE_EDGES)
+  assert relc.left_edge_detected
+
+
+def test_clears_after_window(relc):
+  edge = ([0.0, 0.9], [0.0, 0.8, 0.8, 0.8])
+  clear = ([0.9, 0.9], [0.8, 0.8, 0.8, 0.8])
+
+  drive(relc, *edge, EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  drive(relc, *clear, EDGE_CLEAR_TIME + 0.05)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_low_speed_skips(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1, v_ego=V_LOW)
+  assert not relc.left_edge_detected
+  assert relc.left_edge_timer == 0.0
+
+
+def test_speed_drop_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_LOW, CLOSE_EDGES)
+  assert not relc.left_edge_detected
+
+
+def test_param_off_resets(relc):
+  drive(relc, [0.0, 0.9], [0.0, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert relc.left_edge_detected
+
+  relc.params.get_bool.return_value = False
+  relc.read_params()
+  relc.update([0.0, 0.9], [0.0, 0.8, 0.8, 0.8], V_HIGH, CLOSE_EDGES)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+def test_lane_line_prevents_detection(relc):
+  drive(relc, [0.0, 0.9], [0.8, 0.8, 0.8, 0.8], EDGE_REACTION_TIME + 0.1)
+  assert not relc.left_edge_detected
+
+
+def test_one_side_blocks_other_allows(relc):
+  drive(relc, [0.9, 0.0], [0.8, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
+  assert relc.right_edge_detected
+  assert not relc.left_edge_detected
+
+
+def test_disabled_no_detection(relc):
+  relc.enabled = False
+  relc.params.get_bool.return_value = False
+  drive(relc, [0.0, 0.0], [0.0, 0.8, 0.8, 0.0], EDGE_REACTION_TIME + 0.1)
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+def test_far_edge_no_block(relc):
+  drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1, road_edges=FAR_EDGES)
+  assert not relc.left_edge_detected
+
+
+def test_close_edge_blocks(relc):
+  drive(relc, [0.9, 0.0], [0.05, 0.8, 0.8, 0.05], EDGE_REACTION_TIME + 0.1,
+        road_edges=edges(-8.0, 1.5))
+  assert relc.right_edge_detected
+  assert not relc.left_edge_detected
+
+
+def test_wide_road_no_lines_no_block(relc):
+  drive(relc, [0.0, 0.0], [0.05, 0.4, 0.4, 0.05], EDGE_REACTION_TIME + 0.1,
+        road_edges=edges(-8.0, 8.0))
+  assert not relc.left_edge_detected
+  assert not relc.right_edge_detected
+
+
+def test_narrow_road_both_block(relc):
+  drive(relc, [0.0, 0.0], [0.02, 0.4, 0.4, 0.02], EDGE_REACTION_TIME + 0.1,
+        road_edges=edges(-2.5, 2.5))
+  assert relc.left_edge_detected
+  assert relc.right_edge_detected
+
+
+def test_clearance_boundary(relc):
+  boundary = VEHICLE_EDGE_MARGIN + EDGE_CLEARANCE  # 4.78m
+  drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1,
+        road_edges=edges(-(boundary - 0.1), 10.0))
+  assert relc.left_edge_detected
+
+  relc.reset()
+
+  drive(relc, [0.0, 0.9], [0.05, 0.5, 0.5, 0.08], EDGE_REACTION_TIME + 0.1,
+        road_edges=edges(-(boundary + 0.1), 10.0))
+  assert not relc.left_edge_detected

openpilot/sunnypilot/selfdrive/selfdrived/events.py

@@ -244,4 +244,12 @@ EVENTS_SP: dict[int, dict[str, Alert | AlertCallbackType]] = {
       AlertStatus.normal, AlertSize.none,
       Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.),
   },
+
+  EventNameSP.laneChangeRoadEdge: {
+    ET.WARNING: Alert(
+      "Lane Change Unavailable: Road Edge",
+      "",
+      AlertStatus.userPrompt, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 0.1),
+  },
 }

openpilot/sunnypilot/sunnylink/settings_ui.json

@@ -545,6 +545,12 @@
                 }
               ]
             },
+            {
+              "key": "RoadEdgeLaneChangeEnabled",
+              "widget": "toggle",
+              "title": "Block Lane Change: Road Edge Detection",
+              "description": "Blocks lane change when the model sees a road edge on the side you signal."
+            },
             {
               "key": "AutoLaneChangeBsmDelay",
               "widget": "toggle",

openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml

@@ -257,6 +257,10 @@ sections:
       label: 2 seconds
     - value: 5
       label: 3 seconds
+  - key: RoadEdgeLaneChangeEnabled
+    widget: toggle
+    title: 'Block Lane Change: Road Edge Detection'
+    description: Blocks lane change when the model sees a road edge on the side you signal.
   - key: AutoLaneChangeBsmDelay
     widget: toggle
     title: 'Auto Lane Change: Delay with Blind Spot'

openpilot/system/manager/process_config.py

@@ -88,7 +88,7 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool:
   return use_sunnylink_uploader(params)
 
 def is_tinygrad_model(started, params, CP: car.CarParams) -> bool:
-  """Check if the active model runner is SNPE."""
+  """Check if the active model runner is tinygrad."""
   return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad)
 
 def is_stock_model(started, params, CP: car.CarParams) -> bool:

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()