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

Float The Buffalo

Browse Source
This commit is contained in:
firestar5683
2026-07-09 17:34:56 -05:00
parent f2fbd3f97c
commit 987de36f54
1 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+36
View File
@@ -570,6 +570,18 @@ IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22
IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035
IONIQ_6_CURVY_SPEED_MIN = 10.5
IONIQ_6_CURVY_SPEED_MAX = 19.0
IONIQ_6_CURVY_SPEED_MIN_WIDTH = 1.5
IONIQ_6_CURVY_SPEED_MAX_WIDTH = 1.8
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT = 0.12
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT = 0.16
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT = 0.10
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT = 0.14
IONIQ_6_CURVY_UNWIND_LAT_START = 0.55
IONIQ_6_CURVY_UNWIND_LAT_END = 3.4
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.16
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.50
IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
# Friction compensation near zero lateral accel amplifies planner jerk noise into a slow
# (~0.5 Hz) weave on straights: the 0.09/0.39 small-signal slope plus the jerk feed acts as
@@ -1780,6 +1792,12 @@ 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 _ioniq_6_curvy_speed_weight(v_ego: float) -> float:
onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_CURVY_SPEED_MIN) / IONIQ_6_CURVY_SPEED_MIN_WIDTH)
cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_SPEED_MAX - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH)
return onset * cutoff
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:
@@ -1902,8 +1920,26 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
heavy_base_reduction *= 1.0 - low_speed_relief_weight
reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight)
reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight)
curvy_unwind_weight = 0.0
curvy_unwind_floor_relief = 0.0
if v_ego is not None:
curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego)
curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) /
IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH)
curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) /
IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH)
curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * unwind_weight
curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel,
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT,
IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT) *
curvy_unwind_weight)
reduction += (_ioniq_6_side_value(desired_lateral_accel,
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT,
IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT) *
curvy_unwind_weight)
floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT)
floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight
floor -= curvy_unwind_floor_relief
return max(1.0 - reduction, floor)