Merge branch 'master' into ccnc-port-lite

openpilot/cereal/custom.capnp

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

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

@@ -179,7 +179,6 @@ 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, left_edge_detected=False, right_edge_detected=False):
+  def update(self, carstate, lateral_active, lane_change_prob):
     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 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))
+        blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
+                              (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right))
 
         self.alc.update_lane_change(blindspot_detected, carstate.brakePressed)
 

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

@@ -29,7 +29,6 @@ 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')
@@ -214,7 +213,6 @@ 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
@@ -315,8 +313,7 @@ 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
-      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)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       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,16 +327,9 @@ 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,18 +51,11 @@ 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,7 +49,6 @@ 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"
 
@@ -368,7 +367,6 @@ 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
@@ -481,8 +479,7 @@ 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
-      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)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
       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, sunnypilot, and a number of other contributors.
+Copyright (c) 2021-, rav4kumar, Haibin Wen, 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

@@ -1,98 +0,0 @@
-"""
-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,11 +2,10 @@ import pytest
 from openpilot.cereal import log, custom
 from openpilot.common.params import Params
 
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeState, LaneChangeDirection
+from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 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
 
 
@@ -110,17 +109,5 @@ 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,
-              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
+    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

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

@@ -1,169 +0,0 @@
-"""
-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,12 +244,4 @@ 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,12 +545,6 @@
                 }
               ]
             },
-            {
-              "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,10 +257,6 @@ 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 tinygrad."""
+  """Check if the active model runner is SNPE."""
   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

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