From d03dfd8e652006a01bbd69001aecf91060e1e2d6 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 18 Dec 2025 23:43:01 -0600 Subject: [PATCH] Try Lat Changes --- .../controls/lib/curve_speed_controller.py | 8 +++-- selfdrive/controls/lib/latcontrol_torque.py | 29 ++++++++++++++++++- .../controls/lib/longitudinal_planner.py | 11 ++++--- 3 files changed, 41 insertions(+), 7 deletions(-) diff --git a/frogpilot/controls/lib/curve_speed_controller.py b/frogpilot/controls/lib/curve_speed_controller.py index 2feb141a7..7e3814550 100644 --- a/frogpilot/controls/lib/curve_speed_controller.py +++ b/frogpilot/controls/lib/curve_speed_controller.py @@ -93,10 +93,14 @@ class CurveSpeedController: if self.target_set: csc_speed = (lateral_acceleration / abs(self.frogpilot_planner.road_curvature))**0.5 - decel_rate = (v_ego - csc_speed) / self.frogpilot_planner.time_to_curve + + # Apply deceleration rate reduction factor (20% reduction) for smoother curve entry + raw_decel_rate = (v_ego - csc_speed) / max(self.frogpilot_planner.time_to_curve, 0.5) + decel_rate = raw_decel_rate * 0.8 # 20% reduction in deceleration rate self.target -= decel_rate * DT_MDL - self.target = float(np.clip(self.target, CRUISING_SPEED, csc_speed)) + # Slightly lower minimum speed threshold for smoother transition + self.target = float(np.clip(self.target, CRUISING_SPEED * 0.9, csc_speed)) else: self.target_set = True diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index ee918b166..3f61ba8b3 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -35,6 +35,18 @@ LP_FILTER_CUTOFF_HZ = 1.2 JERK_LOOKAHEAD_SECONDS = 0.19 JERK_GAIN = 0.22 LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0 + +# Adaptive lane centering correction based on curvature and speed +CURVE_CORRECTION_BP = [0.0, 0.001, 0.005, 0.01] # curvature breakpoints (1/m) +CURVE_CORRECTION_V = [1.0, 1.05, 1.15, 1.25] # correction factors +SPEED_CURVE_CORRECTION_BP = [5, 15, 25, 35] # speed breakpoints (m/s) +SPEED_CURVE_CORRECTION_V = [0.8, 1.0, 1.1, 1.15] # speed-based correction factors + +# Straight road lane centering correction +STRAIGHT_ROAD_CURVATURE_THRESHOLD = 0.0005 # below this = straight road (1/m) +STRAIGHT_ROAD_CORRECTION_GAIN = 1.12 # boost error correction on straight roads +STRAIGHT_ROAD_INTEGRATOR_BOOST = 1.08 # slightly increase integrator response + VERSION = 2 class LatControlTorque(LatControl): @@ -100,8 +112,23 @@ class LatControlTorque(LatControl): error = setpoint - measurement error_with_lsf = error * (1 + low_speed_factor / max(current_kp, 1e-3)) + # Adaptive lane centering correction based on curvature and speed + current_curvature = abs(desired_curvature) + curve_correction = np.interp(current_curvature, CURVE_CORRECTION_BP, CURVE_CORRECTION_V) + speed_correction = np.interp(CS.vEgo, SPEED_CURVE_CORRECTION_BP, SPEED_CURVE_CORRECTION_V) + adaptive_correction = curve_correction * speed_correction + + # Enhanced straight road lane centering correction + is_straight_road = current_curvature < STRAIGHT_ROAD_CURVATURE_THRESHOLD + if is_straight_road and CS.vEgo > 10: # Only apply on straight roads at speed > 10 m/s (~36 km/h) + # Boost error correction on straight roads to prevent drift + straight_road_boost = STRAIGHT_ROAD_CORRECTION_GAIN + error_with_adaptive = error_with_lsf * straight_road_boost + else: + error_with_adaptive = error_with_lsf * adaptive_correction + # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly - pid_log.error = float(error_with_lsf) + pid_log.error = float(error_with_adaptive) ff = gravity_adjusted_future_lateral_accel # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll ff -= self.torque_params.latAccelOffset diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index c9fc80407..0748af39f 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -28,9 +28,9 @@ MIN_ALLOW_THROTTLE_SPEED = 2.5 UNCERT_SLOPE_TRIG = 0.12 # per second UNCERT_MAG_TRIG = 0.50 -# Lookup table for turns -_A_TOTAL_MAX_V = [1.7, 3.2] -_A_TOTAL_MAX_BP = [20., 40.] +# Lookup table for turns - optimized to reduce unnecessary deceleration +_A_TOTAL_MAX_V = [2.0, 3.5] # increased from [1.7, 3.2] for smoother curve handling +_A_TOTAL_MAX_BP = [15., 35.] # adjusted from [20., 40.] for earlier response def get_max_accel(v_ego): @@ -49,7 +49,10 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP): # The lookup table for turns should also be updated if we do this a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V) a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase) - a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.)) + + # Add margin factor to allow smoother transition and reduce harsh deceleration + margin_factor = 1.15 # 15% margin for smoother curve handling + a_x_allowed = math.sqrt(max(a_total_max ** 2 - (a_y / margin_factor) ** 2, 0.)) return [a_target[0], min(a_target[1], a_x_allowed)]