diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ce9cf23e5..daa9a0657 100644 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 16fb2d2dd..4d06d79cb 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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 diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index fb91f55dc..8733153cf 100644 --- a/starpilot/common/starpilot_variables.py +++ b/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)