Try Lat Changes

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
 

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

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)]