Lane Change Smoothing 2.0

1. Reduced jerk
2. Smoother
3. Improved arrest, reducing chances of overshooting if setting is too low

selfdrive/controls/controlsd.py

@@ -36,6 +36,16 @@ LaneChangeDirection = log.LaneChangeDirection
 
 ACTUATOR_FIELDS = tuple(car.CarControl.Actuators.schema.fields.keys())
 
+# After a smoothed lane change ends, ramp the curvature limits back to stock over this
+# time so the final recenter correction is shaped instead of stepping through unclamped.
+LANE_CHANGE_SMOOTH_RELEASE_T = 2.0
+
+# Floor on the jerk factor when the model is unwinding lane-change curvature (the arrest and
+# any correction back toward center). Entry gentleness is comfort, but arrest speed is a
+# correctness constraint: a slow symmetric cap lets the car glide past the new lane center.
+# 0.6 (≈ pace-5 rate) fully tracks the arrest demand seen in logs while staying 40% below stock.
+LANE_CHANGE_ARREST_JERK_FLOOR = 0.6
+
 
 def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   spoofed_speed = set_speed_ms
@@ -89,7 +99,7 @@ class Controls:
     self.steer_limited_by_safety = False
     self.curvature = 0.0
     self.desired_curvature = 0.0
-    self.lc_smooth_elapsed = 0.0
+    self.lc_smooth_release = 0.0
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
@@ -212,25 +222,35 @@ class Controls:
       new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
 
     jerk_factor = 1.0
-    lat_accel_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:
-      t_target = self.starpilot_toggles.lane_change_t_target
       set_jerk = self.starpilot_toggles.lane_change_jerk_factor
-      set_accel = self.starpilot_toggles.lane_change_lat_accel_factor
       in_lane_change = model_v2.meta.laneChangeState in (LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing) \
           and CS.vEgo >= self.starpilot_toggles.minimum_lane_change_speed
-      if in_lane_change or 0.0 < self.lc_smooth_elapsed < t_target:
-        self.lc_smooth_elapsed = min(self.lc_smooth_elapsed + DT_CTRL, t_target)
-        progress = self.lc_smooth_elapsed / t_target  # 0 → 1 over T seconds
-        jerk_factor = set_jerk + (1.0 - set_jerk) * progress
-        lat_accel_factor = set_accel + (1.0 - set_accel) * progress
-        if not in_lane_change and self.lc_smooth_elapsed >= t_target:
-          self.lc_smooth_elapsed = 0.0
-      elif not in_lane_change:
-        self.lc_smooth_elapsed = 0.0
+      # Hold the tight jerk limit for the whole maneuver, then taper back to stock so the
+      # model's recenter step and mid-change corrections stay shaped instead of passing
+      # through a mostly-relaxed clamp. Only the jerk (curvature rate) is tightened: capping
+      # lat accel strangles the end-of-maneuver arrest and lets the car glide past the new
+      # lane center before it can build enough counter-curvature.
+      if in_lane_change:
+        self.lc_smooth_release = LANE_CHANGE_SMOOTH_RELEASE_T
+      else:
+        self.lc_smooth_release = max(self.lc_smooth_release - DT_CTRL, 0.0)
+      if self.lc_smooth_release > 0.0:
+        release = 1.0 - self.lc_smooth_release / LANE_CHANGE_SMOOTH_RELEASE_T  # 0 in maneuver → 1 after
+        jerk_factor = set_jerk + (1.0 - set_jerk) * release
+        # When the model is unwinding curvature (reducing the lane-change curvature magnitude)
+        # and the entry cap would make the command lag it, apply the arrest floor so the car
+        # can stop on the new lane center. Applies only to the unwind direction; the entry
+        # ramp keeps the full pace smoothness. Robust to double lane changes (no baseline).
+        model_unwinding = abs(new_desired_curvature) < abs(self.desired_curvature) and \
+            math.copysign(1.0, new_desired_curvature - self.desired_curvature) == -math.copysign(1.0, self.desired_curvature) and \
+            abs(self.desired_curvature) > 1e-4
+        if model_unwinding:
+          arrest_floor = LANE_CHANGE_ARREST_JERK_FLOOR + (1.0 - LANE_CHANGE_ARREST_JERK_FLOOR) * release
+          jerk_factor = max(jerk_factor, arrest_floor)
 
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
-                                                               jerk_factor, lat_accel_factor)
+                                                               jerk_factor)
     lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
 
     actuators.curvature = self.desired_curvature

selfdrive/controls/lib/drive_helpers.py

@@ -32,9 +32,20 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll, jerk_factor=1.0,
 
   effective_lat_accel = MAX_LATERAL_ACCEL_NO_ROLL * lat_accel_factor
   roll_compensation = roll * ACCELERATION_DUE_TO_GRAVITY
-  max_lat_accel = effective_lat_accel + roll_compensation
-  min_lat_accel = -effective_lat_accel + roll_compensation
-  new_curvature, limited_accel = clamp(new_curvature, min_lat_accel / v_ego ** 2, max_lat_accel / v_ego ** 2)
+  min_curvature = (-effective_lat_accel + roll_compensation) / v_ego ** 2
+  max_curvature = (effective_lat_accel + roll_compensation) / v_ego ** 2
+  if lat_accel_factor < 1.0:
+    # A tightened maneuver clamp must not clip curvature already being commanded
+    # (e.g. lane change on a curve); it only limits further growth.
+    min_curvature = min(min_curvature, prev_curvature)
+    max_curvature = max(max_curvature, prev_curvature)
+  # Saturation is reported against the stock envelope only: riding an intentionally
+  # tightened lane-change ceiling is comfort shaping, not steering saturation, and
+  # must not trip the "Turn Exceeds Steering Limit" alert.
+  stock_min_curvature = (-MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2
+  stock_max_curvature = (MAX_LATERAL_ACCEL_NO_ROLL + roll_compensation) / v_ego ** 2
+  limited_accel = bool(new_curvature < stock_min_curvature or new_curvature > stock_max_curvature)
+  new_curvature, _ = clamp(new_curvature, min_curvature, max_curvature)
 
   new_curvature, limited_max_curv = clamp(new_curvature, -MAX_CURVATURE, MAX_CURVATURE)
   return float(new_curvature), limited_accel or limited_max_curv

starpilot/common/starpilot_variables.py

@@ -1039,19 +1039,17 @@ class StarPilotVariables:
     toggle.one_lane_change = self.get_value("OneLaneChange", condition=toggle.lane_changes)
 
     # Lane change pace: 1 = smoothest (~8 s target), 10 = stock (no clamp applied)
-    # Factors are derived from a sinusoidal lane-change profile: a = pi^2 * W / T^2, j = pi^3 * W / T^3.
-    # 1.3x headroom keeps the controller off the ceiling mid-maneuver.
+    # The jerk factor is derived from a sinusoidal lane-change profile: j = pi^3 * W / T^3,
+    # with 1.3x headroom. Only jerk (curvature rate) is shaped; lateral accel stays at the
+    # stock envelope so the end-of-maneuver arrest is never starved of authority.
     pace = self.get_value("LaneChangeSmoothing", cast=int, condition=toggle.lane_changes) or 10
     pace = max(1, min(10, pace))
     lane_w = 3.5
     t_target = 3.0 + (10 - pace) * 5.0 / 9.0
-    a_req = (math.pi ** 2) * lane_w / (t_target ** 2)
     j_req = (math.pi ** 3) * lane_w / (t_target ** 3)
     toggle.lane_change_pace = pace
-    toggle.lane_change_lat_accel_factor = min(1.0, a_req * 1.3 / 3.0)
     toggle.lane_change_jerk_factor = min(1.0, j_req * 1.3 / 5.0)
     toggle.lane_change_time_max = 10.0 + (10 - pace) * 2.0 / 9.0
-    toggle.lane_change_t_target = t_target
 
     lateral_tuning = self.get_value("LateralTune")
     toggle.force_torque_controller = self.get_value("ForceTorqueController", condition=lateral_tuning and not is_torque_car and not is_angle_car)