Close Gap on Lane Changes

common/params_keys.h

@@ -375,6 +375,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LaneCenteringE2EAuthority", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
     {"LaneCenterOffset", {PERSISTENT, FLOAT, "0.0", "0.0", 3}},
     {"LaneChanges", {PERSISTENT, BOOL, "1", "1", 0, SETTINGS_SIMPLE}},
+    {"LaneChangeCloseGap", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
+    {"LaneChangeCloseGapSeconds", {PERSISTENT, FLOAT, "1.0", "1.0", 1, SETTINGS_SIMPLE}},
     {"LaneChangeSmoothing", {PERSISTENT, INT, "5", "10", 1, SETTINGS_SIMPLE}},
     {"LaneChangeTime", {PERSISTENT, FLOAT, "1.0", "0.0", 1, SETTINGS_SIMPLE}},
     {"LaneDetectionWidth", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},

selfdrive/ui/layouts/settings/starpilot/lateral.py

@@ -108,6 +108,9 @@ class StarPilotLateralLayout(_SettingsPage):
     def nlc_on():
       return lc_on() and p.get_bool("NudgelessLaneChange")
 
+    def close_gap_on():
+      return lc_on() and p.get_bool("LaneChangeCloseGap")
+
     def pos_on():
       return p.get_bool("PauseLateralOnSignal")
 
@@ -191,6 +194,20 @@ class StarPilotLateralLayout(_SettingsPage):
         on_click=self._show_lane_smoothing,
         visible=lc_on,
       ),
+      SettingRow(
+        "LaneChangeCloseGap", "toggle", tr_noop("Close Gap On Lane Change"),
+        subtitle=tr_noop("Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes."),
+        get_state=lambda: p.get_bool("LaneChangeCloseGap"),
+        set_state=lambda s: p.put_bool("LaneChangeCloseGap", s),
+        visible=lc_on,
+      ),
+      SettingRow(
+        "LaneChangeCloseGapSeconds", "value", tr_noop("Temporary Follow Distance"),
+        subtitle=tr_noop("Follow distance to hold while changing lanes. Only applied when shorter than your normal gap."),
+        get_value=self._get_lane_change_close_gap_display,
+        on_click=lambda: self._show_slider("LaneChangeCloseGapSeconds", 0.5, 3.0, step=0.05, unit="s", value_type="float"),
+        visible=close_gap_on,
+      ),
     ]
 
     # ── 3. Advanced Lateral Tuning ──
@@ -375,6 +392,9 @@ class StarPilotLateralLayout(_SettingsPage):
       return tr("Stock")
     return str(val)
 
+  def _get_lane_change_close_gap_display(self) -> str:
+    return f"{self._params.get_float('LaneChangeCloseGapSeconds'):.2f}s"
+
   def _show_lane_smoothing(self):
     def on_close(res, val):
       if res == DialogResult.CONFIRM:

starpilot/common/starpilot_variables.py

@@ -1041,6 +1041,8 @@ class StarPilotVariables:
     toggle.lane_change_delay = self.get_value("LaneChangeTime", cast=float, condition=toggle.lane_changes)
     toggle.lane_detection_width = self.get_value("LaneDetectionWidth", cast=float, condition=toggle.lane_changes, conversion=distance_conversion)
     toggle.minimum_lane_change_speed = self.get_value("MinimumLaneChangeSpeed", cast=float, condition=toggle.lane_changes, conversion=speed_conversion)
+    toggle.lane_change_close_gap = self.get_value("LaneChangeCloseGap", condition=toggle.lane_changes)
+    toggle.lane_change_close_gap_seconds = self.get_value("LaneChangeCloseGapSeconds", cast=float, condition=toggle.lane_change_close_gap, min=0.5, max=3.0)
     toggle.nudgeless = self.get_value("NudgelessLaneChange", condition=toggle.lane_changes)
     toggle.nudgeless_lane_change_only_when_engaged = self.get_value("NudgelessLaneChangeOnlyWhenEngaged", condition=toggle.lane_changes and toggle.nudgeless)
     toggle.one_lane_change = self.get_value("OneLaneChange", condition=toggle.lane_changes)

starpilot/controls/lib/starpilot_following.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
 import numpy as np
 
+from cereal import log
 from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
 
@@ -10,6 +12,24 @@ from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX
 TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
 PERSONALITY_BP = [45. * CV.MPH_TO_MS, 70. * CV.MPH_TO_MS]
 
+LaneChangeState = log.LaneChangeState
+LaneChangeDirection = log.LaneChangeDirection
+
+# Lane-change close-gap transition: while signalling out of the lane, temporarily
+# hold a shorter follow distance so openpilot merges out smoothly and can accelerate
+# through the maneuver instead of braking behind a lead it is about to leave.
+# Ramps in gradually, snaps back to the normal gap when the safety gate trips.
+LANE_CHANGE_MIN_T_FOLLOW = 0.5             # hard floor on the reduced gap (s)
+LANE_CHANGE_GAP_RAMP_IN_RATE = 0.6         # seconds of headway per second, toward the shorter gap
+LANE_CHANGE_GAP_RAMP_OUT_RATE = 4.0        # seconds of headway per second, back to the normal gap
+LANE_CHANGE_ABORT_LEAD_BRAKE = 0.8         # lead decel that aborts the reduction (m/s^2)
+
+LANE_CHANGE_ACTIVE_STATES = (
+  LaneChangeState.preLaneChange,
+  LaneChangeState.laneChangeStarting,
+  LaneChangeState.laneChangeFinishing,
+)
+
 
 def get_longitudinal_personality(sm):
   return sm["selfdriveState"].personality
@@ -28,6 +48,9 @@ class StarPilotFollowing:
     self.speed_jerk = 0
     self.t_follow = 0
 
+    # Rate-limited lane-change follow distance (s). None until the first reduction.
+    self.lane_change_t_follow = None
+
   def update(self, long_control_active, v_ego, sm, starpilot_toggles):
     personality = get_longitudinal_personality(sm)
 
@@ -84,6 +107,8 @@ class StarPilotFollowing:
     if self.starpilot_planner.starpilot_weather.weather_id != 0:
       self.t_follow = min(self.t_follow + self.starpilot_planner.starpilot_weather.increase_following_distance, MAX_T_FOLLOW)
 
+    self.update_lane_change_gap(long_control_active, v_ego, sm, starpilot_toggles)
+
     self.disable_throttle = False
     if self.starpilot_planner.tracking_lead and self.starpilot_planner.lead_one.status:
       lead_distance = self.starpilot_planner.lead_one.dRel
@@ -98,6 +123,55 @@ class StarPilotFollowing:
     else:
       self.desired_follow_distance = 0
 
+  def update_lane_change_gap(self, long_control_active, v_ego, sm, starpilot_toggles):
+    # Hold a shorter follow distance while signalling out of the lane so openpilot
+    # merges out smoothly and can accelerate through the maneuver, instead of braking
+    # behind a lead it is about to leave. The target is an absolute headway that can
+    # only ever shorten the current gap, never lengthen it.
+    if not long_control_active:
+      self.lane_change_t_follow = None
+      return
+
+    target = self.t_follow
+
+    if getattr(starpilot_toggles, "lane_change_close_gap", False):
+      meta = sm["modelV2"].meta
+      lane_change_active = meta.laneChangeState in LANE_CHANGE_ACTIVE_STATES
+
+      if lane_change_active and self.lane_change_gap_safe(v_ego, sm, meta.laneChangeDirection, starpilot_toggles):
+        requested = float(np.clip(starpilot_toggles.lane_change_close_gap_seconds, LANE_CHANGE_MIN_T_FOLLOW, MAX_T_FOLLOW))
+        # Absolute cap: only apply when it is actually shorter than the normal gap.
+        target = min(self.t_follow, requested)
+
+    if self.lane_change_t_follow is None:
+      self.lane_change_t_follow = self.t_follow
+
+    rate = LANE_CHANGE_GAP_RAMP_IN_RATE if target < self.lane_change_t_follow else LANE_CHANGE_GAP_RAMP_OUT_RATE
+    step = rate * DT_MDL
+    self.lane_change_t_follow = float(np.clip(target,
+                                              self.lane_change_t_follow - step,
+                                              self.lane_change_t_follow + step))
+
+    # Never let the ramp raise the gap above what the rest of the stack asked for.
+    self.t_follow = min(self.t_follow, max(self.lane_change_t_follow, LANE_CHANGE_MIN_T_FOLLOW))
+
+  def lane_change_gap_safe(self, v_ego, sm, direction, starpilot_toggles):
+    CS = sm["carState"]
+    if CS.standstill or v_ego < starpilot_toggles.minimum_lane_change_speed:
+      return False
+
+    # Do not close the gap toward an occupied blindspot on the lane-change side.
+    if (direction == LaneChangeDirection.left and CS.leftBlindspot) or \
+       (direction == LaneChangeDirection.right and CS.rightBlindspot):
+      return False
+
+    lead = self.starpilot_planner.lead_one
+    if self.starpilot_planner.tracking_lead and lead.status:
+      if max(0.0, -float(lead.aLeadK)) >= LANE_CHANGE_ABORT_LEAD_BRAKE:
+        return False
+
+    return True
+
   def update_follow_values(self, lead_distance, v_ego, v_lead, starpilot_toggles):
     if starpilot_toggles.conditional_slower_lead and v_lead < v_ego:
       distance_factor = max(lead_distance - (v_lead * self.t_follow), 1)

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

@@ -228,6 +228,27 @@
         "parent_key": "LaneChanges",
         "settings_tier": "simple"
       },
+      {
+        "key": "LaneChangeCloseGap",
+        "label": "Close Gap On Lane Change",
+        "description": "Allows for a temporary shorter follow distance behind lead so that openpilot merges smoothly out of current lane, it will allow car to accelerate as it changes lanes.",
+        "data_type": "bool",
+        "ui_type": "toggle",
+        "parent_key": "LaneChanges",
+        "settings_tier": "simple"
+      },
+      {
+        "key": "LaneChangeCloseGapSeconds",
+        "label": "Temporary Follow Distance",
+        "description": "Follow distance to hold while changing lanes. Only applied when shorter than your normal gap.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.5,
+        "max": 3.0,
+        "step": 0.05,
+        "parent_key": "LaneChanges",
+        "settings_tier": "simple"
+      },
       {
         "key": "LateralTune",
         "label": "Lateral Tuning",