mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-07-26 20:32:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

Project Hateno

Browse Source
This commit is contained in:
firestar5683
2026-06-05 11:13:24 -05:00
parent e01d61feb9
commit 826775bb1f
2 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+15 -1
View File
@@ -328,6 +328,12 @@ KIA_FORTE_TURN_IN_BOOST_LEFT = 0.10
KIA_FORTE_TURN_IN_BOOST_RIGHT = 0.00
KIA_FORTE_UNWIND_TAPER_LEFT = 0.26
KIA_FORTE_UNWIND_TAPER_RIGHT = 0.04
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT = 0.10
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT = 0.14
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED = 4.5
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH = 0.8
KIA_FORTE_CRAWL_TURN_IN_FF_LAT = 0.10
KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH = 0.05
KIA_FORTE_CENTER_TAPER_MAX = 0.14
KIA_FORTE_CENTER_TAPER_LAT = 0.16
KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.03
@@ -1269,7 +1275,15 @@ def get_kia_forte_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: f
turn_in_weight * (0.35 + 0.65 * low_speed_factor))
unwind_taper = 1.0 - (_kia_forte_side_value(desired_lateral_accel, KIA_FORTE_UNWIND_TAPER_LEFT, KIA_FORTE_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
crawl_turn_in_scale = 0.0
if desired_lateral_accel * desired_lateral_jerk > 0.0:
crawl_speed_weight = _kia_forte_sigmoid((KIA_FORTE_CRAWL_TURN_IN_FF_SPEED - max(v_ego, 0.0)) /
KIA_FORTE_CRAWL_TURN_IN_FF_SPEED_WIDTH)
crawl_lat_weight = _kia_forte_sigmoid((abs_lateral_accel - KIA_FORTE_CRAWL_TURN_IN_FF_LAT) /
KIA_FORTE_CRAWL_TURN_IN_FF_LAT_WIDTH)
crawl_turn_in_scale = _kia_forte_side_value(desired_lateral_accel, KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_LEFT,
KIA_FORTE_CRAWL_TURN_IN_FF_BOOST_RIGHT) * crawl_speed_weight * crawl_lat_weight
return ((1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)) + crawl_turn_in_scale
def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
selfdrive/controls/tests/test_latcontrol.py
+4 -1
View File
@@ -296,10 +296,13 @@ class TestLatControl:
assert steady_left < 1.0
assert steady_right < steady_left
assert turn_in_left > steady_left
assert turn_in_right == pytest.approx(steady_right)
assert turn_in_right > steady_right
assert unwind_left < steady_left
assert unwind_right < steady_right
assert unwind_right > unwind_left
assert get_kia_forte_ff_scale(0.30, 0.60, 3.0) > get_kia_forte_ff_scale(0.30, 0.60, 6.0)
assert get_kia_forte_ff_scale(0.30, 0.60, 6.0) > get_kia_forte_ff_scale(0.30, 0.60, 12.0)
assert get_kia_forte_ff_scale(0.30, -0.60, 3.0) < get_kia_forte_ff_scale(0.30, 0.60, 3.0)
def test_kia_forte_center_taper_curve(self):
assert get_kia_forte_center_taper_scale(0.0, 30.0) < get_kia_forte_center_taper_scale(0.0, 15.0)