From eac56eea22d07cd9badb4c9b8cd8fe9dc6aa7d3b Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Thu, 6 Aug 2026 20:57:29 -0500
Subject: [PATCH] smooth center

---
 selfdrive/controls/lib/lane_centering.py      | 17 ++++++++++++-----
 .../controls/tests/test_lane_centering.py     | 19 ++++++++++++++++++-
 2 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py
index 89c6784c5..d2d17654d 100644
--- a/selfdrive/controls/lib/lane_centering.py
+++ b/selfdrive/controls/lib/lane_centering.py
@@ -16,10 +16,12 @@ _MAX_RAW_CORRECTION = 0.004
 _MAX_GAIN = 0.30
 _SMOOTH_TAU = 0.4
 _SIGNAL_RELEASE_TAU = 0.20
+_CONFIDENCE_RELEASE_TAU = 0.20
+_CENTER_ERROR_DEADBAND = 0.08
 
 _E2E_MAX_PATH_STD = 0.35
-_E2E_BREAK_IN_START = 0.25
-_E2E_BREAK_IN_FULL = 0.75
+_E2E_BREAK_IN_START = 0.15
+_E2E_BREAK_IN_FULL = 0.50
 
 
 class LaneCenteringController:
@@ -68,8 +70,8 @@ class LaneCenteringController:
       float(np.clip(e2e_authority, 0.0, 1.0)),
     )
     if not valid:
-      self.reset()
-      return model_curvature
+      self._correction = float(smooth_value(0.0, self._correction, _CONFIDENCE_RELEASE_TAU, dt=DT_CTRL))
+      return model_curvature + self._correction
 
     target = float(np.clip(raw_correction, -_MAX_RAW_CORRECTION, _MAX_RAW_CORRECTION)) * _MAX_GAIN
     self._correction = float(smooth_value(target, self._correction, _SMOOTH_TAU, dt=DT_CTRL))
@@ -120,6 +122,11 @@ class LaneCenteringController:
       target_y = 0.5 * (left + right) + float(np.clip(offset, -max_safe_offset, max_safe_offset))
       model_y = float(np.interp(lookahead, pos_x, pos_y))
       error = target_y - model_y
+      error_abs = abs(error)
+      if error_abs <= _CENTER_ERROR_DEADBAND:
+        error = 0.0
+      else:
+        error = np.copysign(error_abs - _CENTER_ERROR_DEADBAND, error)
 
       try:
         pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
@@ -127,7 +134,7 @@ class LaneCenteringController:
           path_std = float(np.interp(lookahead, pos_x, pos_y_std))
           if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
             break_in = np.clip(
-              (abs(error) - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START),
+              (error_abs - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START),
               0.0,
               1.0,
             )
diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py
index 5ba98b437..a5c6551e1 100644
--- a/selfdrive/controls/tests/test_lane_centering.py
+++ b/selfdrive/controls/tests/test_lane_centering.py
@@ -108,6 +108,11 @@ def test_lane_center_error_steers_toward_center():
   assert left < 0.0
 
 
+def test_small_center_error_does_not_chatter():
+  _, output = _converge(_model(left=-1.75, right=1.85), authority=0.0)
+  assert output == 0.0
+
+
 def test_offset_direction():
   _, right = _converge(_model(), offset=0.2, authority=0.0)
   _, left = _converge(_model(), offset=-0.2, authority=0.0)
@@ -144,10 +149,22 @@ def test_e2e_authority_blends_lane_correction():
   assert lane_only > blended > e2e >= 0.0
 
 
+def test_confident_e2e_authority_starts_before_large_offset():
+  model = _model(left=-1.7, right=2.1, model_y=0.0, path_std=0.1)
+  _, lane_only = _converge(model, authority=0.0)
+  _, e2e = _converge(model, authority=1.0)
+  assert lane_only > e2e > 0.0
+
+
 def test_confidence_loss_drops_filtered_correction():
   controller, output = _converge(_model(left=-1.5, right=2.1), authority=0.0)
   assert output > 0.0
-  assert _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0) == 0.0
+  fading = _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0)
+  assert 0.0 < fading < output
+
+  for _ in range(300):
+    fading = _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0)
+  assert abs(fading) < 1e-6
 
 
 def test_correction_is_smoothed_and_capped():