Lane Change Smoothing 2.0

selfdrive/controls/controlsd.py

@@ -36,6 +36,10 @@ 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
+
 
 def get_gm_hud_set_speed(set_speed_ms: float, starpilot_toggles) -> float:
   spoofed_speed = set_speed_ms
@@ -89,7 +93,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
@@ -214,20 +218,21 @@ class Controls:
     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 limits for the whole maneuver (factors carry 1.3x headroom over the
+      # target-duration profile), 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.
+      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
+        lat_accel_factor = set_accel + (1.0 - set_accel) * release
 
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll,
                                                                jerk_factor, lat_accel_factor)

selfdrive/controls/lib/drive_helpers.py

@@ -34,7 +34,14 @@ def clip_curvature(v_ego, prev_curvature, new_curvature, roll, jerk_factor=1.0,
   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 = min_lat_accel / v_ego ** 2
+  max_curvature = max_lat_accel / 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)
+  new_curvature, limited_accel = 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

@@ -1012,7 +1012,6 @@ class StarPilotVariables:
     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)