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

peter

Browse Source
This commit is contained in:
firestar5683
2026-05-12 22:17:13 -05:00
parent 2516d13bc5
commit dc82e51ef6
2 changed files with 26 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+21 -1
View File
@@ -76,6 +76,11 @@ CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.03
CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.07
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX = 0.12
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT = 0.24
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH = 0.05
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED = 18.0
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH = 2.5
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.50
CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.82
CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.00
@@ -433,6 +438,15 @@ def civic_bosch_modified_a_lateral_testing_ground_active() -> bool:
return testing_ground.use("8", "A")
def get_civic_bosch_modified_a_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sigmoid((v_ego - CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED) /
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sigmoid((CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_LAT_WIDTH)
reduction = CIVIC_BOSCH_MODIFIED_A_VARIANT_CENTER_TAPER_MAX * speed_weight * center_weight
return 1.0 - reduction
def _civic_bosch_modified_b_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED) ** 2)
@@ -1450,6 +1464,9 @@ class LatControlTorque(LatControl):
volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0
ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0
sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0
civic_bosch_modified_a_center_taper = get_civic_bosch_modified_a_center_taper_scale(setpoint, CS.vEgo) if (
self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active()
) else 1.0
friction_threshold = get_friction_threshold(CS.vEgo)
friction_scale = 1.0
if bolt_2022_2023_tuned_path_active:
@@ -1488,9 +1505,10 @@ class LatControlTorque(LatControl):
friction_threshold = get_volt_plexy_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_volt_plexy_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
elif self.is_civic_bosch_modified:
ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
ff *= get_civic_bosch_modified_b_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * civic_bosch_modified_a_center_taper
friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD
friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper)
ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params)
deadzone_boost_active = False
if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL:
@@ -1510,6 +1528,8 @@ class LatControlTorque(LatControl):
output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif volt_standard_test_active:
output_torque *= volt_standard_center_taper
elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active():
output_torque *= civic_bosch_modified_a_center_taper
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
selfdrive/controls/tests/test_latcontrol.py
+5
View File
@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import (
get_civic_bosch_modified_pid_output_scale,
)
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_civic_bosch_modified_a_center_taper_scale,
LatControlTorque,
get_civic_bosch_modified_b_ff_scale,
get_civic_bosch_modified_b_friction_scale,
@@ -588,6 +589,10 @@ class TestLatControl:
assert a_variant_unwind_right < base_unwind_right
assert a_variant_unwind_right_friction < base_unwind_right_friction
def test_modified_civic_a_variant_center_taper_curve(self):
assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.0, 10.0)
assert get_civic_bosch_modified_a_center_taper_scale(0.0, 25.0) < get_civic_bosch_modified_a_center_taper_scale(0.35, 25.0) <= 1.0
def test_kia_ev6_testing_ground_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6)
monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True)