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

Sip-smiling away

Browse Source
This commit is contained in:
firestar5683
2026-07-23 14:37:54 -05:00
parent bd6dced538
commit b72042042f
7 changed files with 53 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+29 -9
View File
@@ -671,10 +671,15 @@ KIA_EV6_UNWIND_TAPER_LEFT = 0.56
KIA_EV6_UNWIND_TAPER_RIGHT = 0.54
KIA_EV6_BASE_UNWIND_TAPER_LEFT = 0.06
KIA_EV6_BASE_UNWIND_TAPER_RIGHT = 0.05
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT = 0.12
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT = 0.14
KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT = 0.15
KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT = 0.16
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT = 0.13
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT = 0.15
KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT = 0.17
KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT = 0.18
KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION = 0.55
KIA_EV6_JWARM_PHASE_STABILITY_SPEED = 10.0
KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH = 1.8
KIA_EV6_JWARM_PHASE_STABILITY_JERK = 0.70
KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH = 0.18
KIA_EV6_FRICTION_MULT = 1.01
KIA_EV6_FRICTION_LAT_RISE = 0.18
KIA_EV6_FRICTION_JERK_RISE = 0.22
@@ -2433,6 +2438,16 @@ def _kia_ev6_transition_envelope(v_ego: float, desired_lateral_accel: float, des
return _kia_ev6_low_speed_factor(v_ego) * lat_factor * jerk_factor
def get_kia_ev6_jwarm_phase_confidence(v_ego: float, desired_lateral_jerk: float) -> float:
low_speed_weight = _kia_ev6_sigmoid(
(KIA_EV6_JWARM_PHASE_STABILITY_SPEED - v_ego) / KIA_EV6_JWARM_PHASE_STABILITY_SPEED_WIDTH
)
abrupt_transition_weight = _kia_ev6_sigmoid(
(abs(desired_lateral_jerk) - KIA_EV6_JWARM_PHASE_STABILITY_JERK) / KIA_EV6_JWARM_PHASE_STABILITY_JERK_WIDTH
)
return 1.0 - (KIA_EV6_JWARM_PHASE_STABILITY_MAX_REDUCTION * low_speed_weight * abrupt_transition_weight)
def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
@@ -2463,17 +2478,22 @@ def get_kia_ev6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo
) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
jwarm_tune = kia_ev6_lateral_testing_ground_active()
jwarm_phase_confidence = get_kia_ev6_jwarm_phase_confidence(v_ego, desired_lateral_jerk) if jwarm_tune else 0.0
base_turn_in_boost = 1.0 + ((_kia_ev6_side_value(
desired_lateral_accel,
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_LEFT,
KIA_EV6_JWARM_BASE_TURN_IN_BOOST_RIGHT,
) if jwarm_tune else 0.0) * turn_in_weight * onset * cutoff)
) if jwarm_tune else 0.0) *
jwarm_phase_confidence * turn_in_weight * onset * cutoff)
base_unwind_taper_left = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_left", KIA_EV6_BASE_UNWIND_TAPER_LEFT)
base_unwind_taper_right = _flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_right", KIA_EV6_BASE_UNWIND_TAPER_RIGHT)
if jwarm_tune:
base_unwind_taper_left += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT - base_unwind_taper_left) * jwarm_phase_confidence
base_unwind_taper_right += (KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT - base_unwind_taper_right) * jwarm_phase_confidence
base_unwind_taper = 1.0 - (_kia_ev6_side_value(
desired_lateral_accel,
KIA_EV6_JWARM_BASE_UNWIND_TAPER_LEFT if jwarm_tune else
_flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_left", KIA_EV6_BASE_UNWIND_TAPER_LEFT),
KIA_EV6_JWARM_BASE_UNWIND_TAPER_RIGHT if jwarm_tune else
_flm_vehicle_knob("hyundai_kia_ev6.base_unwind_taper_right", KIA_EV6_BASE_UNWIND_TAPER_RIGHT),
base_unwind_taper_left,
base_unwind_taper_right,
) * unwind_weight * onset * cutoff)
return (base_unwind_taper * base_turn_in_boost) + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))
selfdrive/controls/tests/test_latcontrol.py
+12
View File
@@ -90,6 +90,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_kia_ev6_ff_scale,
get_kia_ev6_friction_scale,
get_kia_ev6_friction_threshold,
get_kia_ev6_jwarm_phase_confidence,
get_sonata_center_taper_scale,
get_sonata_ff_scale,
get_sonata_hybrid_center_taper_scale,
@@ -1203,6 +1204,17 @@ class TestLatControl:
assert get_kia_ev6_ff_scale(0.45, -0.7, 10.0) < normal_unwind_left - 0.07
assert get_kia_ev6_ff_scale(-0.45, 0.7, 10.0) < normal_unwind_right - 0.08
def test_kia_ev6_jwarm_abrupt_low_speed_phase_correction_is_bounded(self):
calm_low_speed = get_kia_ev6_jwarm_phase_confidence(6.0, 0.25)
abrupt_low_speed = get_kia_ev6_jwarm_phase_confidence(6.0, 1.40)
abrupt_high_speed = get_kia_ev6_jwarm_phase_confidence(18.0, 1.40)
assert abrupt_low_speed < calm_low_speed
assert abrupt_low_speed < abrupt_high_speed
assert 0.45 <= abrupt_low_speed < 0.60
assert calm_low_speed > 0.90
assert abrupt_high_speed > 0.98
def test_kia_ev6_center_taper_curve(self):
assert get_kia_ev6_center_taper_scale(0.0, 25.0) < get_kia_ev6_center_taper_scale(0.0, 10.0)
assert get_kia_ev6_center_taper_scale(0.0, 25.0) < get_kia_ev6_center_taper_scale(0.20, 25.0) <= 1.0