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

honda

Browse Source
This commit is contained in:
firestar5683
2026-05-02 15:54:34 -05:00
parent 5584535842
commit d4f73f56c4
2 changed files with 7 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+5 -4
View File
@@ -15,17 +15,18 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool:
def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float:
abs_angle = abs(desired_angle_deg)
speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0)
center_weight = min(max((10.0 - abs_angle) / 10.0, 0.0), 1.0)
angle_weight = min(max((abs_angle - 10.0) / 18.0, 0.0), 1.0)
center_speed_weight = 0.55 + (0.45 * speed_weight)
center_weight = min(max((12.0 - abs_angle) / 12.0, 0.0), 1.0)
angle_weight = min(max((abs_angle - 12.0) / 16.0, 0.0), 1.0)
phase = desired_angle_deg * desired_angle_delta_deg
is_left = desired_angle_deg > 0.0
center_taper = 0.18
center_taper = 0.22
base_scale = 0.10 if is_left else 0.12
turn_in_scale = 0.10 if is_left else 0.14
unwind_scale = 0.14 if is_left else 0.20
scale = 1.0 - (speed_weight * center_weight * center_taper)
scale = 1.0 - (center_speed_weight * center_weight * center_taper)
scale += speed_weight * angle_weight * base_scale
if phase > 0.2:
scale += speed_weight * angle_weight * turn_in_scale
selfdrive/controls/tests/test_latcontrol.py
+2
View File
@@ -382,6 +382,8 @@ class TestLatControl:
def test_civic_bosch_modified_pid_scale_curve(self):
assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) < 1.0
assert get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) < 1.0
assert get_civic_bosch_modified_pid_output_scale(10.0, 0.0, 12.0) < 1.0
assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 6.0) < 0.9
assert get_civic_bosch_modified_pid_output_scale(12.0, 0.0, 12.0) > get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0)
assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0)
assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0)