mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-31 21:23:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

smoother unwind

Browse Source 
Revert "smoother unwind"

This reverts commit 10f9bcc9a8c00ebd1734d4e88359e2c1e4d5e0e2.

Reapply "smoother unwind"

This reverts commit fed67ae4e86695e505563717e59d3d18843a550b.
This commit is contained in:
firestar5683
2025-12-08 15:05:36 -06:00
parent 750638ac6a
commit 3d7ce5237b
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+5 -3
View File
@@ -28,12 +28,12 @@ INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
LOW_SPEED_X = [0, 10, 20, 30]
LOW_SPEED_Y = [15, 13, 10, 5]
LOW_SPEED_Y = [12, 10.5, 8, 5]
MAX_LAT_JERK_UP = 2.5 # m/s^3
LP_FILTER_CUTOFF_HZ = 1.2
JERK_LOOKAHEAD_SECONDS = 0.19
JERK_GAIN = 0.3
JERK_GAIN = 0.22
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 2
@@ -84,12 +84,14 @@ class LatControlTorque(LatControl):
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
raw_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / max(lat_delay, self.dt)
desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
raw_lateral_jerk = np.clip(raw_lateral_jerk, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
desired_lateral_jerk = np.clip(self.jerk_filter.update(raw_lateral_jerk), -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
setpoint = expected_lateral_accel + desired_lateral_jerk * lat_delay
measurement = measured_curvature * CS.vEgo ** 2
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
measurement_rate = np.clip(measurement_rate, -MAX_LAT_JERK_UP, MAX_LAT_JERK_UP)
self.previous_measurement = measurement
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2