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

Sentry and Tunes

Browse Source
This commit is contained in:
firestar5683
2026-08-14 12:05:45 -05:00
parent 8e1caf8fe4
commit cdfd5d1d66
19 changed files with 621 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+2
View File
@@ -404,6 +404,8 @@ class LatControlTorque(LatControl):
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
if not self.is_ioniq_6_2025:
ff *= get_ioniq_6_2023_unwind_ff_scale(setpoint, measurement, desired_lateral_jerk, CS.vEgo)
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
+32
View File
@@ -836,6 +836,15 @@ IONIQ_6_CURVY_TURN_IN_TRIM_LAT_START = 1.0
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END = 2.5
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_ONSET_WIDTH = 0.18
IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH = 0.30
IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX = 0.18
IONIQ_6_2023_UNWIND_FF_OVERSHOOT = 0.15
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH = 0.18
IONIQ_6_2023_UNWIND_FF_JERK = 0.10
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH = 0.10
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET = 8.0
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH = 2.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF = 23.5
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH = 2.0
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
@@ -3070,6 +3079,29 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
(extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale
def get_ioniq_6_2023_unwind_ff_scale(setpoint: float, measured_lateral_accel: float,
desired_lateral_jerk: float, v_ego: float) -> float:
"""Trim residual curve feedforward when the 2023 car is already over-rotated."""
if setpoint * desired_lateral_jerk >= 0.0 or setpoint * measured_lateral_accel <= 0.0:
return 1.0
overshoot = max(abs(measured_lateral_accel) - abs(setpoint), 0.0)
if overshoot <= 0.0:
return 1.0
overshoot_weight = _ioniq_6_sigmoid((overshoot - IONIQ_6_2023_UNWIND_FF_OVERSHOOT) /
IONIQ_6_2023_UNWIND_FF_OVERSHOOT_WIDTH)
jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_2023_UNWIND_FF_JERK) /
IONIQ_6_2023_UNWIND_FF_JERK_WIDTH)
speed_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_2023_UNWIND_FF_SPEED_ONSET) /
IONIQ_6_2023_UNWIND_FF_SPEED_ONSET_WIDTH)
speed_cutoff = _ioniq_6_sigmoid((IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF - v_ego) /
IONIQ_6_2023_UNWIND_FF_SPEED_CUTOFF_WIDTH)
reduction = (IONIQ_6_2023_UNWIND_FF_REDUCTION_MAX * overshoot_weight * jerk_weight *
speed_onset * speed_cutoff)
return 1.0 - reduction
def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = max(get_hkg_canfd_base_friction_threshold(v_ego), IONIQ_6_BASE_FRICTION_THRESHOLD)
transition_envelope = _ioniq_6_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
selfdrive/controls/tests/test_latcontrol.py
+7
View File
@@ -116,6 +116,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_ioniq_6_directional_taper_scale,
get_ioniq_6_output_taper_scale,
get_ioniq_6_ff_scale,
get_ioniq_6_2023_unwind_ff_scale,
get_ioniq_6_friction_center_fade_scale,
get_ioniq_6_friction_scale,
get_ioniq_6_friction_threshold,
@@ -1149,6 +1150,12 @@ class TestLatControl:
assert get_ioniq_6_ff_scale(0.30, 0.60, 6.0) > get_ioniq_6_ff_scale(0.30, 0.60, 12.0)
assert get_ioniq_6_ff_scale(0.30, -0.60, 3.0) < get_ioniq_6_ff_scale(0.30, 0.60, 3.0)
def test_ioniq_6_2023_unwind_ff_scale_only_trims_measured_overshoot(self):
assert get_ioniq_6_2023_unwind_ff_scale(-1.2, -2.0, 1.0, 18.0) < 1.0
assert get_ioniq_6_2023_unwind_ff_scale(-1.2, -1.0, 1.0, 18.0) == 1.0
assert get_ioniq_6_2023_unwind_ff_scale(-1.2, -2.0, -1.0, 18.0) == 1.0
assert get_ioniq_6_2023_unwind_ff_scale(-1.2, -2.0, 1.0, 30.0) > get_ioniq_6_2023_unwind_ff_scale(-1.2, -2.0, 1.0, 18.0)
def test_ioniq_6_low_speed_angle_assist_curve(self):
base = 0.05
boosted = get_ioniq_6_low_speed_angle_assist_torque(22.0, 0.0, base, 1.0)