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

pra

Browse Source
This commit is contained in:
firestar5683
2026-08-20 19:55:35 -05:00
parent 5130168067
commit 73f64ac754
3 changed files with 90 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+6
View File
@@ -111,6 +111,7 @@ class LatControlTorque(LatControl):
self.is_rav4_tss2 = CP.carFingerprint in RAV4_TSS2_CARS
self.is_rav4_prime = CP.carFingerprint in RAV4_PRIME_CARS
self.is_sienna_4th_gen = CP.carFingerprint in SIENNA_4TH_GEN_CARS
self.is_toyota_highlander_tss2 = CP.carFingerprint in TOYOTA_HIGHLANDER_TSS2_CARS
self.is_toyota_corolla_tss2 = CP.carFingerprint in TOYOTA_COROLLA_TSS2_CARS
self.is_lexus_is = CP.carFingerprint in LEXUS_IS_CARS
self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS
@@ -310,6 +311,7 @@ class LatControlTorque(LatControl):
rav4_tss2_active = self.is_rav4_tss2
rav4_prime_active = self.is_rav4_prime
sienna_4th_gen_active = self.is_sienna_4th_gen
toyota_highlander_tss2_active = self.is_toyota_highlander_tss2
toyota_corolla_tss2_active = self.is_toyota_corolla_tss2
lexus_is_active = self.is_lexus_is
ioniq_5_active = self.is_ioniq_5
@@ -398,6 +400,10 @@ class LatControlTorque(LatControl):
elif sienna_4th_gen_active:
ff *= get_sienna_4th_gen_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_sienna_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
elif toyota_highlander_tss2_active:
ff *= get_toyota_highlander_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
friction_threshold = get_toyota_highlander_tss2_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk)
friction_scale = get_toyota_highlander_tss2_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk)
elif toyota_corolla_tss2_active:
ff *= get_toyota_corolla_tss2_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif lexus_is_active:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+61
View File
@@ -184,6 +184,10 @@ SIENNA_4TH_GEN_CARS = (
TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN,
)
TOYOTA_HIGHLANDER_TSS2_CARS = (
TOYOTA_CAR.TOYOTA_HIGHLANDER_TSS2,
)
TOYOTA_COROLLA_TSS2_CARS = (
TOYOTA_CAR.TOYOTA_COROLLA_TSS2,
)
@@ -1108,6 +1112,17 @@ TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED = 4.5
TOYOTA_COROLLA_TSS2_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 1.5
TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE = 0.12
TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION = 0.10
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN = 0.16
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION = 0.10
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET = 0.20
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH = 0.08
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET = 3.0
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH = 1.5
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX = 15.0
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0
LEXUS_IS_PHASE_SCALE = 0.10
LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06
LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06
@@ -1645,6 +1660,52 @@ def get_toyota_corolla_tss2_center_output_scale(desired_lateral_accel: float, v_
return max(1.0 - reduction, 0.65)
def _toyota_highlander_tss2_unwind_weight(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) /
TOYOTA_HIGHLANDER_TSS2_PHASE_SCALE)
curve_weight = _sigmoid((abs(desired_lateral_accel) - TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH)
speed_weight = (_sigmoid((v_ego - TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_WIDTH) *
_sigmoid((TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX - v_ego) /
TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH))
return max(-phase, 0.0) * curve_weight * speed_weight
def get_toyota_highlander_tss2_ff_scale(desired_lateral_accel: float,
desired_lateral_jerk: float,
v_ego: float) -> float:
reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_ff_reduction",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FF_REDUCTION)
return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
def get_toyota_highlander_tss2_friction_threshold(v_ego: float,
desired_lateral_accel: float = 0.0,
desired_lateral_jerk: float = 0.0) -> float:
gain = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_threshold_gain",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_THRESHOLD_GAIN)
return get_standard_friction_threshold(v_ego) * (
1.0 + gain * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
)
def get_toyota_highlander_tss2_friction_scale(v_ego: float,
desired_lateral_accel: float,
desired_lateral_jerk: float) -> float:
reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_friction_scale_reduction",
TOYOTA_HIGHLANDER_TSS2_UNWIND_FRICTION_SCALE_REDUCTION)
return 1.0 - reduction * _toyota_highlander_tss2_unwind_weight(
desired_lateral_accel, desired_lateral_jerk, v_ego,
)
def get_lexus_is_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
if desired_lateral_accel == 0.0:
return 1.0
selfdrive/controls/tests/test_latcontrol.py
+23
View File
@@ -109,6 +109,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
get_sienna_4th_gen_ff_scale,
get_sienna_4th_gen_friction_threshold,
get_sienna_4th_gen_high_speed_output_taper_scale,
get_toyota_highlander_tss2_ff_scale,
get_toyota_highlander_tss2_friction_scale,
get_toyota_highlander_tss2_friction_threshold,
get_toyota_corolla_tss2_center_output_scale,
get_toyota_corolla_tss2_ff_scale,
get_lexus_is_ff_scale,
@@ -1010,6 +1013,26 @@ class TestLatControl:
assert get_sienna_4th_gen_high_speed_output_taper_scale(10.0) == pytest.approx(1.0, abs=0.002)
assert get_sienna_4th_gen_high_speed_output_taper_scale(22.0) < 1.0
def test_toyota_highlander_tss2_unwind_shaping_is_low_speed_only(self):
base = get_standard_friction_threshold(9.0)
steady = get_toyota_highlander_tss2_ff_scale(0.8, 0.0, 9.0)
unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 9.0)
highway_unwind = get_toyota_highlander_tss2_ff_scale(0.8, -0.8, 28.0)
assert steady == pytest.approx(1.0)
assert 0.90 < unwind < 1.0
assert highway_unwind > unwind
unwind_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, -0.8)
turn_threshold = get_toyota_highlander_tss2_friction_threshold(9.0, 0.8, 0.8)
assert unwind_threshold > base
assert turn_threshold == pytest.approx(base, rel=0.01)
unwind_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, -0.8)
turn_scale = get_toyota_highlander_tss2_friction_scale(9.0, 0.8, 0.8)
assert 0.90 < unwind_scale < 1.0
assert turn_scale == pytest.approx(1.0)
def test_rav4_prime_forced_torque_update_path(self, monkeypatch):
controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(TOYOTA.TOYOTA_RAV4_PRIME, force_torque=True)
CS.vEgo = 13.0