From 73f64ac7545405ea0da29b8c7c267cc7c161b713 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 20 Aug 2026 19:55:35 -0500 Subject: [PATCH] pra --- selfdrive/controls/lib/latcontrol_torque.py | 6 ++ .../controls/lib/latcontrol_vehicle_tunes.py | 61 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 23 +++++++ 3 files changed, 90 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 29ad860fd..daa53988c 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -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: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 2ef890079..1fce8c029 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -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 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 9ffbc9f50..ec4b16a81 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -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