Pause LaneCenter On Turn Signal

selfdrive/controls/controlsd.py

@@ -594,7 +594,9 @@ class Controls:
       self.starpilot_toggles.lane_center_offset,
       self.starpilot_toggles.lane_centering_e2e_authority,
       CC.latActive,
-      bool(self.sm.all_checks(['modelV2'])))
+      bool(self.sm.all_checks(['modelV2'])),
+      self.starpilot_toggles.lane_centering_pause_on_signal,
+      bool(CS.leftBlinker or CS.rightBlinker))
 
     jerk_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:

selfdrive/controls/lib/lane_centering.py

@@ -15,6 +15,7 @@ _MIN_CENTER_TO_LINE = 1.1
 _MAX_RAW_CORRECTION = 0.004
 _MAX_GAIN = 0.30
 _SMOOTH_TAU = 0.4
+_SIGNAL_RELEASE_TAU = 0.20
 
 _E2E_MAX_PATH_STD = 0.35
 _E2E_BREAK_IN_START = 0.25
@@ -28,7 +29,8 @@ class LaneCenteringController:
   def reset(self) -> None:
     self._correction = 0.0
 
-  def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid) -> float:
+  def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid,
+             pause_on_signal=False, turn_signal_active=False) -> float:
     model_curvature = float(model_curvature)
 
     try:
@@ -47,6 +49,10 @@ class LaneCenteringController:
       self.reset()
       return model_curvature
 
+    if pause_on_signal and turn_signal_active:
+      self._correction = float(smooth_value(0.0, self._correction, _SIGNAL_RELEASE_TAU, dt=DT_CTRL))
+      return model_curvature + self._correction
+
     try:
       if model_v2.meta.laneChangeState != log.LaneChangeState.off:
         self.reset()

selfdrive/controls/tests/test_lane_centering.py

@@ -28,8 +28,10 @@ def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_
   )
 
 
-def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO):
-  return controller.update(0.0, model, speed, enabled, offset, authority, active, valid)
+def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO,
+            pause_on_signal=False, turn_signal_active=False):
+  return controller.update(0.0, model, speed, enabled, offset, authority, active, valid,
+                           pause_on_signal, turn_signal_active)
 
 
 def _converge(model, *, offset=0.0, authority=1.0):
@@ -57,6 +59,25 @@ def test_lane_change_is_noop():
   assert _update(LaneCenteringController(), _model(left=-1.5, right=2.1, lane_change=1)) == 0.0
 
 
+def test_turn_signal_fades_lane_centering_correction():
+  model = _model(left=-1.5, right=2.1)
+  controller, centered = _converge(model, authority=0.0)
+  fading = _update(controller, model, authority=0.0, pause_on_signal=True, turn_signal_active=True)
+  assert 0.0 < fading < centered
+
+  for _ in range(300):
+    fading = _update(controller, model, authority=0.0, pause_on_signal=True, turn_signal_active=True)
+  assert abs(fading) < 1e-6
+
+
+def test_turn_signal_pause_can_be_disabled():
+  model = _model(left=-1.5, right=2.1)
+  _, output = _converge(model, authority=0.0)
+  controller, _ = _converge(model, authority=0.0)
+  signaled = _update(controller, model, authority=0.0, turn_signal_active=True)
+  assert signaled == pytest.approx(output, abs=1e-7)
+
+
 @pytest.mark.parametrize(
   "field,value",
   [