mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-08 15:12:10 +08:00
Code Issues Packages Projects Releases Wiki Activity

Notchy Highways

Browse Source
This commit is contained in:
whoisdomi
2026-07-06 14:56:20 -05:00
parent 39414ee2e4
commit e4e416bfb3
3 changed files with 26 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+7 -1
View File
@@ -59,6 +59,7 @@ class LatControlTorque(LatControl):
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len)
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.ioniq_6_directional_taper_filter = FirstOrderFilter(1.0, IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC, self.dt)
self.previous_measurement = 0.0
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.low_speed_reset_threshold = max(CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED)
@@ -164,6 +165,7 @@ class LatControlTorque(LatControl):
self.measurement_rate_filter.x = 0.0
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len)
self.prev_desired_lateral_accel = 0.0
self.ioniq_6_directional_taper_filter.x = 1.0
else:
if self.prev_steering_pressed and not CS.steeringPressed:
self.pid.i *= self.steer_release_i_decay
@@ -283,7 +285,11 @@ class LatControlTorque(LatControl):
ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper
friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper)
elif ioniq_6_active:
ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_6_center_taper
# smooth the directional taper so jerk-gated unwind cuts can't step the FF in one frame
ioniq_6_directional_taper = self.ioniq_6_directional_taper_filter.update(
get_ioniq_6_directional_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo))
ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo,
directional_taper_scale=ioniq_6_directional_taper) * ioniq_6_center_taper
friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3)
friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper)
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+18 -9
View File
@@ -517,14 +517,20 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 2.15
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.20
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 1.00
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.30
# Unwind detection needs a softer phase transition and time smoothing than the shared
# PHASE_SCALE: desired lateral jerk noise in a sustained curve (~+/-1-2 m/s^3) otherwise
# chatters the taper between its base value and its floor at ~0.5 Hz (felt as notchy
# steering in highway sweepers).
IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE = 0.45
IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC = 0.4
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2
IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5
@@ -569,8 +575,8 @@ IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.03
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.11
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.10
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.40
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
@@ -1764,7 +1770,8 @@ def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, des
return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float,
directional_taper_scale: float | None = None) -> float:
if desired_lateral_accel == 0.0:
return 1.0
@@ -1798,8 +1805,10 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) /
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH)
high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff
if directional_taper_scale is None:
directional_taper_scale = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale +
(extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego)
(extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale
def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
@@ -1859,7 +1868,7 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
band_weight = onset * cutoff
heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE)
unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) /
IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH)
low_speed_relief_weight = 0.0
selfdrive/controls/tests/test_latcontrol.py
+1 -1
View File
@@ -509,7 +509,7 @@ class TestLatControl:
assert get_ioniq_6_directional_taper_scale(1.2, -0.40) > get_ioniq_6_directional_taper_scale(1.2, -0.7)
assert get_ioniq_6_directional_taper_scale(-1.2, -0.40, 8.0) > get_ioniq_6_directional_taper_scale(-1.2, -0.40, 25.0)
assert get_ioniq_6_directional_taper_scale(1.2, 0.40, 8.0) > get_ioniq_6_directional_taper_scale(1.2, 0.40, 25.0)
assert get_ioniq_6_directional_taper_scale(-1.2, 0.7, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 0.7, 25.0), abs=0.02)
assert get_ioniq_6_directional_taper_scale(-1.2, 1.6, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 1.6, 25.0), abs=0.02)
assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0)
assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 20.0)
assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0)