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

honda

Browse Source
This commit is contained in:
firestar5683
2026-05-03 12:51:37 -05:00
parent 5bc24ff148
commit 48ea42c026
2 changed files with 32 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+20
View File
@@ -36,6 +36,23 @@ def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_
return max(scale, 0.70)
def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_angle_delta_deg: float,
v_ego: float, output_torque: float, prev_output_torque: float) -> float:
abs_angle = abs(desired_angle_deg)
if abs_angle < 3.0 or abs_angle > 20.0:
return 1.0
speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0)
onset = min(max((abs_angle - 3.0) / 5.0, 0.0), 1.0)
cutoff = min(max((20.0 - abs_angle) / 6.0, 0.0), 1.0)
band_weight = onset * cutoff
transition_weight = min(abs(desired_angle_delta_deg) / 0.35, 1.0)
sign_change_weight = 1.0 if (output_torque * prev_output_torque) < 0.0 else 0.0
smoothing = band_weight * (0.32 + (0.22 * speed_weight) + (0.10 * transition_weight) + (0.10 * sign_change_weight))
return min(max(1.0 - smoothing, 0.22), 1.0)
class LatControlPID(LatControl):
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
@@ -93,6 +110,9 @@ class LatControlPID(LatControl):
if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active():
desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset
output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo)
output_alpha = get_civic_bosch_modified_pid_output_alpha(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo,
output_torque, self.prev_output_torque)
output_torque = self.prev_output_torque + (output_alpha * (output_torque - self.prev_output_torque))
output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max))
pid_log.active = True
selfdrive/controls/tests/test_latcontrol.py
+12 -1
View File
@@ -13,7 +13,11 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.vehicle_model import VehicleModel
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID, get_civic_bosch_modified_pid_output_scale
from openpilot.selfdrive.controls.lib.latcontrol_pid import (
LatControlPID,
get_civic_bosch_modified_pid_output_alpha,
get_civic_bosch_modified_pid_output_scale,
)
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
LatControlTorque,
get_bolt_2017_center_taper_scale,
@@ -395,6 +399,13 @@ class TestLatControl:
assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0)
assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0)
def test_civic_bosch_modified_pid_output_alpha_curve(self):
assert get_civic_bosch_modified_pid_output_alpha(0.0, 0.0, 12.0, 0.2, 0.1) == 1.0
assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.0, 12.0, 0.2, 0.1) < 1.0
assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 12.0, 0.2, 0.1) < get_civic_bosch_modified_pid_output_alpha(8.0, 0.0, 12.0, 0.2, 0.1)
assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 20.0, -0.2, 0.2) < get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 8.0, -0.2, 0.2)
assert get_civic_bosch_modified_pid_output_alpha(24.0, 0.0, 12.0, 0.2, 0.1) == 1.0
def test_civic_bosch_modified_pid_testing_ground_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(HONDA.HONDA_CIVIC_BOSCH)
monkeypatch.setattr(latcontrol_pid, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)