Pause LaneCenter On Turn Signal

common/params_keys.h

@@ -375,6 +375,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}},
     {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}},
     {"LaneCentering", {PERSISTENT, BOOL, "0", "0", 2}},
+    {"LaneCenteringPauseOnSignal", {PERSISTENT, BOOL, "1", "1", 2}},
     {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
     {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
     {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},

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",
   [

starpilot/common/safe_mode.py

@@ -36,6 +36,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "SteerRatio",
   "CameraOffset",
   "LaneCentering",
+  "LaneCenteringPauseOnSignal",
   "LaneCenteringE2EAuthority",
   "LaneCenterOffset",
   "LaneChanges",

starpilot/common/starpilot_variables.py

@@ -714,6 +714,9 @@ class StarPilotVariables:
 
     advanced_lateral_tuning = self.get_value("AdvancedLateralTune")
     toggle.lane_centering = self.get_value("LaneCentering")
+    toggle.lane_centering_pause_on_signal = self.get_value(
+      "LaneCenteringPauseOnSignal", condition=toggle.lane_centering, default=True,
+    )
     toggle.force_auto_tune = self.get_value("ForceAutoTune", condition=advanced_lateral_tuning and not has_auto_tune and is_torque_car and not is_angle_car)
     # Force-off is also meaningful on manually tuned torque cars: it locks the
     # vehicle-model parameters instead of allowing paramsd to learn over them.

starpilot/common/tests/test_lane_centering_galaxy.py

@@ -15,7 +15,7 @@ def _sections():
 
 def test_lane_centering_is_only_in_galaxy_developer_section():
   sections = _sections()
-  keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringE2EAuthority"}
+  keys = {"LaneCentering", "LaneCenterOffset", "LaneCenteringPauseOnSignal", "LaneCenteringE2EAuthority"}
 
   assert keys <= sections["Developer"].keys()
   for name, params in sections.items():
@@ -30,11 +30,15 @@ def test_lane_centering_galaxy_controls():
   developer = _sections()["Developer"]
   centering = developer["LaneCentering"]
   offset = developer["LaneCenterOffset"]
+  pause_on_signal = developer["LaneCenteringPauseOnSignal"]
   e2e_authority = developer["LaneCenteringE2EAuthority"]
 
   assert centering["ui_type"] == "toggle"
   assert centering["is_parent_toggle"] is True
 
+  assert pause_on_signal["ui_type"] == "toggle"
+  assert pause_on_signal["parent_key"] == "LaneCentering"
+
   assert offset["parent_key"] == "LaneCentering"
   assert offset["min"] == -0.3
   assert offset["max"] == 0.3

starpilot/common/tests/test_safe_mode_lane_centering.py

@@ -5,6 +5,7 @@ def test_safe_mode_manages_lane_centering_settings():
   assert {
     "CameraOffset",
     "LaneCentering",
+    "LaneCenteringPauseOnSignal",
     "LaneCenteringE2EAuthority",
     "LaneCenterOffset",
   } <= set(SAFE_MODE_MANAGED_KEYS)

starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json

@@ -4087,6 +4087,15 @@
         "parent_key": "LaneCentering",
         "settings_tier": "advanced"
       },
+      {
+        "key": "LaneCenteringPauseOnSignal",
+        "label": "Pause Lane Centering On Turn Signal",
+        "description": "Fade lane-centering correction out when a turn signal is active so it does not fight a lane change or turn.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "LaneCentering",
+        "settings_tier": "advanced"
+      },
       {
         "key": "LaneCenteringE2EAuthority",
         "label": "E2E Override Strength",

starpilot/system/the_galaxy/the_galaxy.py

@@ -966,6 +966,7 @@ _TROUBLESHOOT_ADVANCED_LATERAL_KEYS = [
   "ForceTorqueController",
   "CameraOffset",
   "LaneCentering",
+  "LaneCenteringPauseOnSignal",
   "LaneCenteringE2EAuthority",
   "LaneCenterOffset",
 ]