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

All Makes

Browse Source
This commit is contained in:
firestar5683
2026-07-01 08:26:02 -05:00
parent f40881657c
commit 74e4dd4c44
3 changed files with 37 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+7 -1
View File
@@ -87,6 +87,7 @@ class LatControlTorque(LatControl):
self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS
self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
self.is_silverado = CP.carFingerprint in SILVERADO_CARS
self.is_gm = CP.brand == "gm"
self.is_hkg_canfd_torque = CP.brand == "hyundai" and bool(CP.flags & HyundaiFlags.CANFD)
if self.is_ioniq_6:
self.low_speed_reset_threshold = min(self.low_speed_reset_threshold, IONIQ_6_LOW_SPEED_PID_RESET_SPEED)
@@ -241,7 +242,12 @@ class LatControlTorque(LatControl):
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_hkg_canfd_base_friction_threshold(CS.vEgo) if self.is_hkg_canfd_torque else get_friction_threshold(CS.vEgo)
if self.is_hkg_canfd_torque:
friction_threshold = get_hkg_canfd_base_friction_threshold(CS.vEgo)
elif self.is_gm:
friction_threshold = get_gm_base_friction_threshold(CS.vEgo)
else:
friction_threshold = get_standard_friction_threshold(CS.vEgo)
friction_scale = 1.0
if bolt_2022_2023_tuned_path_active:
ff *= get_bolt_2022_2023_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+16 -11
View File
@@ -8,6 +8,7 @@ from openpilot.common.constants import CV
from openpilot.starpilot.common.testing_grounds import testing_ground
CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
STANDARD_FRICTION_THRESHOLD = 0.30
HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39
CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00
@@ -655,13 +656,17 @@ def _sigmoid(x: float) -> float:
return z / (1.0 + z)
def get_friction_threshold(v_ego: float) -> float:
# Keep the speed-scaled friction threshold behavior.
def get_gm_base_friction_threshold(v_ego: float) -> float:
# GM's speed-scaled base friction threshold behavior.
return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27]))
def get_standard_friction_threshold(v_ego: float) -> float:
return max(get_gm_base_friction_threshold(v_ego), STANDARD_FRICTION_THRESHOLD)
def get_hkg_canfd_base_friction_threshold(v_ego: float) -> float:
return max(get_friction_threshold(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD)
return max(get_gm_base_friction_threshold(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD)
def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float:
@@ -722,7 +727,7 @@ def get_prius_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float
def get_prius_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _prius_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _prius_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1001,7 +1006,7 @@ def get_bolt_2018_2021_dynamic_torque_scale(desired_lateral_accel: float, desire
def get_bolt_2018_2021_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _bolt_2018_2021_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2018_2021_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1078,7 +1083,7 @@ def get_bolt_2022_2023_ff_scale(desired_lateral_accel: float, desired_lateral_je
def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1150,7 +1155,7 @@ def get_volt_standard_ff_scale(desired_lateral_accel: float, desired_lateral_jer
def get_volt_standard_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _volt_standard_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _volt_standard_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1387,7 +1392,7 @@ def get_kia_niro_phev_2022_center_taper_scale(desired_lateral_accel: float, v_eg
def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
speed_weight = _sigmoid((v_ego - KIA_NIRO_PHEV_2022_FRICTION_SPEED) / KIA_NIRO_PHEV_2022_FRICTION_SPEED_WIDTH)
center_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_NIRO_PHEV_2022_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _sigmoid((KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH)
@@ -1446,7 +1451,7 @@ def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float)
def get_kia_forte_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
speed_weight = _kia_forte_sigmoid((v_ego - KIA_FORTE_FRICTION_SPEED) / KIA_FORTE_FRICTION_SPEED_WIDTH)
center_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CENTER_LAT - abs(desired_lateral_accel)) / KIA_FORTE_FRICTION_CENTER_LAT_WIDTH)
calm_jerk_weight = _kia_forte_sigmoid((KIA_FORTE_FRICTION_CALM_JERK - abs(desired_lateral_jerk)) / KIA_FORTE_FRICTION_CALM_JERK_WIDTH)
@@ -1497,7 +1502,7 @@ def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: fl
def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
@@ -1569,7 +1574,7 @@ def get_genesis_g90_ff_scale(desired_lateral_accel: float, desired_lateral_jerk:
def get_genesis_g90_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float:
base_threshold = get_friction_threshold(v_ego)
base_threshold = get_gm_base_friction_threshold(v_ego)
transition_envelope = _genesis_g90_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk)
phase = _genesis_g90_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
selfdrive/controls/tests/test_latcontrol.py
+14 -8
View File
@@ -25,8 +25,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
LatControlTorque,
get_civic_bosch_modified_b_ff_scale,
get_civic_bosch_modified_b_friction_scale,
get_gm_base_friction_threshold,
get_bolt_2017_center_taper_scale,
get_friction_threshold,
get_standard_friction_threshold,
get_bolt_2017_base_torque_scale,
get_bolt_2017_steer_ratio_scale,
get_bolt_2017_torque_scale,
@@ -157,7 +158,7 @@ class TestLatControl:
assert get_bolt_2018_2021_dynamic_torque_scale(-0.6, 0.6, 8.0) < get_bolt_2018_2021_dynamic_torque_scale(-0.6, -0.6, 8.0)
def test_bolt_2018_2021_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_bolt_2018_2021_friction_threshold(6.0, 0.7, 0.8)
right_turn_in = get_bolt_2018_2021_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_bolt_2018_2021_friction_threshold(6.0, 0.7, -0.8)
@@ -186,7 +187,7 @@ class TestLatControl:
assert get_bolt_2022_2023_ff_scale(0.14, 0.0, 30.0) < get_bolt_2022_2023_ff_scale(0.14, 0.0, 20.0)
def test_bolt_2022_2023_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_bolt_2022_2023_friction_threshold(6.0, 0.7, 0.8)
right_turn_in = get_bolt_2022_2023_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_bolt_2022_2023_friction_threshold(6.0, 0.7, -0.8)
@@ -217,7 +218,7 @@ class TestLatControl:
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):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_volt_standard_friction_threshold(6.0, 0.7, 0.8)
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)
@@ -329,7 +330,7 @@ class TestLatControl:
assert get_genesis_g90_ff_scale(2.0, 0.0, 20.0) < get_genesis_g90_ff_scale(0.8, 0.0, 20.0)
def test_genesis_g90_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_genesis_g90_friction_threshold(6.0, 0.7, 0.8)
right_turn_in = get_genesis_g90_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_genesis_g90_friction_threshold(6.0, 0.7, -0.8)
@@ -366,7 +367,7 @@ class TestLatControl:
assert unwind_right < unwind_left
def test_palisade_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_palisade_friction_threshold(6.0, 0.7, 0.8)
right_turn_in = get_palisade_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_palisade_friction_threshold(6.0, 0.7, -0.8)
@@ -399,7 +400,7 @@ class TestLatControl:
assert unwind_right < unwind_left
def test_prius_friction_curves(self):
base_threshold = get_friction_threshold(12.0)
base_threshold = get_gm_base_friction_threshold(12.0)
left_turn_in_threshold = get_prius_friction_threshold(6.0, 0.7, 0.8)
right_turn_in_threshold = get_prius_friction_threshold(6.0, -0.7, -0.8)
left_unwind_threshold = get_prius_friction_threshold(6.0, 0.7, -0.8)
@@ -417,6 +418,11 @@ class TestLatControl:
assert right_turn_in_scale > left_turn_in_scale > base_scale
assert base_scale > left_unwind_scale > right_unwind_scale
def test_generic_friction_threshold_floor(self):
assert get_standard_friction_threshold(0.0) == 0.30
assert get_standard_friction_threshold(6.0) == 0.30
assert get_standard_friction_threshold(40.0) == 0.30
def test_ioniq_5_ff_scale_curve(self):
assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0
steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0)
@@ -575,7 +581,7 @@ class TestLatControl:
assert base > left_unwind >= right_unwind
def test_volt_plexy_friction_threshold_curve(self):
base = get_friction_threshold(6.0)
base = get_gm_base_friction_threshold(6.0)
left_turn_in = get_volt_plexy_friction_threshold(6.0, 0.7, 0.8)
right_turn_in = get_volt_plexy_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_volt_plexy_friction_threshold(6.0, 0.7, -0.8)