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

volt lat

Browse Source
This commit is contained in:
firestar5683
2026-04-19 18:11:18 -05:00
parent 5f2dad82a1
commit 392d38c589
2 changed files with 38 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+26 -12
View File
@@ -152,21 +152,26 @@ VOLT_STANDARD_FF_CUTOFF = 1.30
VOLT_STANDARD_FF_CUTOFF_WIDTH = 0.24
VOLT_STANDARD_TRANSITION_SPEED = 10.0
VOLT_STANDARD_PHASE_SCALE = 0.10
VOLT_STANDARD_TURN_IN_BOOST_LEFT = -0.04
VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.24
VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.03
VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.08
VOLT_STANDARD_TURN_IN_BOOST_LEFT = 0.02
VOLT_STANDARD_TURN_IN_BOOST_RIGHT = 0.30
VOLT_STANDARD_UNWIND_TAPER_LEFT = 0.08
VOLT_STANDARD_UNWIND_TAPER_RIGHT = 0.14
VOLT_STANDARD_FRICTION_MULT = 1.04
VOLT_STANDARD_FRICTION_LAT_RISE = 0.20
VOLT_STANDARD_FRICTION_JERK_RISE = 0.24
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = -0.04
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = -0.06
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.04
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = -0.02
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.08
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = -0.04
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.04
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.02
VOLT_STANDARD_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.18
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_LEFT = 0.02
VOLT_STANDARD_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.10
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_LEFT = 0.01
VOLT_STANDARD_TURN_IN_FRICTION_BOOST_RIGHT = 0.10
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_LEFT = 0.02
VOLT_STANDARD_UNWIND_FRICTION_REDUCTION_RIGHT = 0.08
VOLT_STANDARD_CENTER_TAPER_MAX = 0.11
VOLT_STANDARD_CENTER_TAPER_LAT = 0.16
VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH = 0.03
VOLT_STANDARD_CENTER_TAPER_SPEED = 17.0
VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5
GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
GENESIS_G90_FF_GAIN_LEFT = 0.09
@@ -545,6 +550,13 @@ def get_volt_standard_friction_scale(v_ego: float, desired_lateral_accel: float,
return min(max(friction_scale, 0.90), 1.14)
def get_volt_standard_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _volt_standard_sigmoid((v_ego - VOLT_STANDARD_CENTER_TAPER_SPEED) / VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH)
center_weight = _volt_standard_sigmoid((VOLT_STANDARD_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / VOLT_STANDARD_CENTER_TAPER_LAT_WIDTH)
reduction = VOLT_STANDARD_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def genesis_g90_lateral_testing_ground_active() -> bool:
return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID)
@@ -919,6 +931,8 @@ class LatControlTorque(LatControl):
output_torque *= get_bolt_2017_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif bolt_2018_2021_tuned_path_active:
output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif volt_standard_test_active:
output_torque *= get_volt_standard_center_taper_scale(setpoint, CS.vEgo)
pid_log.active = True
pid_log.p = float(self.pid.p)
selfdrive/controls/tests/test_latcontrol.py
+12 -6
View File
@@ -33,6 +33,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_volt_standard_center_taper_scale,
get_volt_standard_ff_scale,
get_volt_standard_friction_scale,
get_volt_standard_friction_threshold,
@@ -147,8 +148,8 @@ class TestLatControl:
def test_volt_standard_ff_scale_curve(self):
assert get_volt_standard_ff_scale(0.0, 0.0, 20.0) == 1.0
assert get_volt_standard_ff_scale(-0.5, 0.0, 20.0) > get_volt_standard_ff_scale(0.5, 0.0, 20.0)
assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) < get_volt_standard_ff_scale(0.6, 0.0, 8.0)
assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0)
assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) > get_volt_standard_ff_scale(0.6, 0.0, 8.0) > get_volt_standard_ff_scale(0.6, -0.7, 8.0)
assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0) > get_volt_standard_ff_scale(-0.6, 0.7, 8.0)
assert get_volt_standard_ff_scale(2.0, 0.0, 20.0) < get_volt_standard_ff_scale(0.8, 0.0, 20.0)
def test_volt_standard_friction_threshold_curve(self):
@@ -157,8 +158,8 @@ class TestLatControl:
right_turn_in = get_volt_standard_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_volt_standard_friction_threshold(6.0, 0.7, -0.8)
right_unwind = get_volt_standard_friction_threshold(6.0, -0.7, 0.8)
assert left_turn_in > base > right_turn_in
assert left_unwind < base < right_unwind
assert right_turn_in < left_turn_in < base
assert base < left_unwind < right_unwind
def test_volt_standard_friction_scale_curve(self):
base = get_volt_standard_friction_scale(25.0, 0.7, 0.8)
@@ -166,8 +167,13 @@ class TestLatControl:
right_turn_in = get_volt_standard_friction_scale(6.0, -0.7, -0.8)
left_unwind = get_volt_standard_friction_scale(6.0, 0.7, -0.8)
right_unwind = get_volt_standard_friction_scale(6.0, -0.7, 0.8)
assert left_turn_in < base < right_turn_in
assert left_unwind > base > right_unwind
assert base < left_turn_in < right_turn_in
assert left_unwind < base and right_unwind < left_unwind
def test_volt_standard_center_taper_curve(self):
assert get_volt_standard_center_taper_scale(0.0, 10.0) > get_volt_standard_center_taper_scale(0.0, 25.0)
assert get_volt_standard_center_taper_scale(0.0, 25.0) < get_volt_standard_center_taper_scale(0.10, 25.0) < get_volt_standard_center_taper_scale(0.20, 25.0) <= 1.0
assert get_volt_standard_center_taper_scale(0.0, 25.0) > 0.85
def test_genesis_g90_ff_scale_curve(self):
assert get_genesis_g90_ff_scale(0.0, 0.0, 20.0) == 1.0