From 1d9fdc25cab886bd8288906693f5776f0714ab23 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 21 Aug 2026 19:16:28 -0500 Subject: [PATCH] toy --- selfdrive/controls/lib/latcontrol_torque.py | 2 ++ .../controls/lib/latcontrol_vehicle_tunes.py | 12 +++++++ selfdrive/controls/lib/longcontrol.py | 3 ++ .../controls/lib/longcontrol_vehicle_tunes.py | 31 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 8 +++++ selfdrive/controls/tests/test_longcontrol.py | 27 ++++++++++++++++ 6 files changed, 83 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index daa53988cc..0d44460f7a 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -592,6 +592,8 @@ class LatControlTorque(LatControl): elif sienna_4th_gen_active: output_torque *= get_sienna_4th_gen_center_taper_scale(setpoint, CS.vEgo) output_torque *= get_sienna_4th_gen_high_speed_output_taper_scale(CS.vEgo) + elif toyota_highlander_tss2_active: + output_torque *= get_toyota_highlander_tss2_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif toyota_corolla_tss2_active: output_torque *= get_toyota_corolla_tss2_center_output_scale(setpoint, CS.vEgo) elif prius_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 1fce8c0296..88ca71fc7d 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -1116,6 +1116,7 @@ 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_OUTPUT_REDUCTION = 0.15 TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_ONSET = 0.20 TOYOTA_HIGHLANDER_TSS2_UNWIND_LAT_WIDTH = 0.08 TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_ONSET = 3.0 @@ -1706,6 +1707,17 @@ def get_toyota_highlander_tss2_friction_scale(v_ego: float, ) +def get_toyota_highlander_tss2_output_taper_scale(desired_lateral_accel: float, + desired_lateral_jerk: float, + v_ego: float) -> float: + """Slow low-speed unwind reversals without reducing turn-in authority.""" + reduction = _flm_vehicle_knob("toyota_highlander_tss2.unwind_output_reduction", + TOYOTA_HIGHLANDER_TSS2_UNWIND_OUTPUT_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/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b6aea11171..3edec4f45d 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -295,6 +295,9 @@ class LongControl: a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target( a_target, CS.vEgo, should_stop, leads=leads, ) + a_target = self.vehicle_tuning.cap_toyota_sienna_lead_departure_accel( + a_target, CS.vEgo, leads=leads, + ) a_target = self.vehicle_tuning.shape_hyundai_elantra_lead_target( a_target, CS.vEgo, should_stop, leads, ) diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 3c57fc5a3d..d36a7431b8 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py @@ -36,6 +36,10 @@ TOYOTA_SIENNA_COMFORT_FILTER_MIN_TTC = 4.5 TOYOTA_SIENNA_COMFORT_FILTER_MAX_CLOSING_SPEED = 4.0 TOYOTA_SIENNA_COMFORT_FILTER_MAX_LEAD_BRAKE = 2.5 TOYOTA_SIENNA_COMFORT_FILTER_BRAKE_BYPASS = -2.5 +TOYOTA_SIENNA_LEAD_DEPARTURE_MAX_SPEED = 8.0 +TOYOTA_SIENNA_LEAD_DEPARTURE_MIN_SPEED_DELTA = 0.25 +TOYOTA_SIENNA_LEAD_DEPARTURE_ACCEL_CAP_BP = [0.0, 1.0, 3.0, 6.0, TOYOTA_SIENNA_LEAD_DEPARTURE_MAX_SPEED] +TOYOTA_SIENNA_LEAD_DEPARTURE_ACCEL_CAP_V = [1.0, 1.15, 1.35, 1.55, 1.70] TOYOTA_COROLLA_TARGET_FILTER_MAX_SPEED = 3.0 TOYOTA_COROLLA_TARGET_FILTER_UP_TAU = 0.30 TOYOTA_COROLLA_TARGET_FILTER_DOWN_TAU = 0.18 @@ -336,6 +340,33 @@ class LongControlVehicleTuning: self.toyota_sienna_filtered_a_target += alpha * (float(a_target) - self.toyota_sienna_filtered_a_target) return self.toyota_sienna_filtered_a_target + def cap_toyota_sienna_lead_departure_accel(self, a_target, v_ego, leads=None): + """Keep a nearby departing lead from producing a low-speed launch kick.""" + if ( + not self.is_toyota_sienna_4g or + a_target <= 0.0 or + v_ego >= TOYOTA_SIENNA_LEAD_DEPARTURE_MAX_SPEED or + not leads + ): + return a_target + + departing_lead = next(( + lead for lead in leads + if bool(getattr(lead, "status", False)) and + abs(float(getattr(lead, "yRel", 0.0))) <= 1.75 and + 0.0 < float(getattr(lead, "dRel", 0.0)) <= 30.0 and + float(getattr(lead, "vLead", 0.0)) > v_ego + TOYOTA_SIENNA_LEAD_DEPARTURE_MIN_SPEED_DELTA + ), None) + if departing_lead is None: + return a_target + + accel_cap = float(interp( + v_ego, + TOYOTA_SIENNA_LEAD_DEPARTURE_ACCEL_CAP_BP, + TOYOTA_SIENNA_LEAD_DEPARTURE_ACCEL_CAP_V, + )) + return min(float(a_target), accel_cap) + def shape_toyota_corolla_accel_target(self, a_target, v_ego, should_stop, last_output_accel): """Smooth low-speed Corolla TSS2 stop releases without delaying hard braking.""" if not self.is_toyota_corolla_tss2 or should_stop or v_ego >= TOYOTA_COROLLA_TARGET_FILTER_MAX_SPEED: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index ec4b16a81e..71f258a7cd 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -112,6 +112,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_toyota_highlander_tss2_ff_scale, get_toyota_highlander_tss2_friction_scale, get_toyota_highlander_tss2_friction_threshold, + get_toyota_highlander_tss2_output_taper_scale, get_toyota_corolla_tss2_center_output_scale, get_toyota_corolla_tss2_ff_scale, get_lexus_is_ff_scale, @@ -1033,6 +1034,13 @@ class TestLatControl: assert 0.90 < unwind_scale < 1.0 assert turn_scale == pytest.approx(1.0) + unwind_output = get_toyota_highlander_tss2_output_taper_scale(0.8, -0.8, 9.0) + turn_output = get_toyota_highlander_tss2_output_taper_scale(0.8, 0.8, 9.0) + highway_output = get_toyota_highlander_tss2_output_taper_scale(0.8, -0.8, 28.0) + assert 0.80 < unwind_output < 1.0 + assert turn_output == pytest.approx(1.0) + assert highway_output > unwind_output + 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 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 7a33eaa6cb..799282b3a3 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -1291,6 +1291,33 @@ def test_toyota_sienna_target_filter_ramps_low_speed_acceleration(): assert tuning.shape_toyota_sienna_accel_target(-1.5, 3.0, False) == pytest.approx(-1.5) +def test_toyota_sienna_caps_acceleration_for_nearby_departing_lead(): + CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN) + tuning = vehicle_tunes.LongControlVehicleTuning(CP) + lead = SimpleNamespace(status=True, yRel=0.0, dRel=9.0, vLead=3.0, aLeadK=0.0) + + capped = tuning.cap_toyota_sienna_lead_departure_accel(1.9, 0.8, leads=(lead,)) + + assert capped == pytest.approx(1.12) + + +def test_toyota_sienna_departure_cap_does_not_change_stopped_lead_or_braking(): + CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN) + tuning = vehicle_tunes.LongControlVehicleTuning(CP) + stopped_lead = SimpleNamespace(status=True, yRel=0.0, dRel=9.0, vLead=0.0, aLeadK=-1.0) + + assert tuning.cap_toyota_sienna_lead_departure_accel(1.9, 0.8, leads=(stopped_lead,)) == pytest.approx(1.9) + assert tuning.cap_toyota_sienna_lead_departure_accel(-2.0, 0.8, leads=(stopped_lead,)) == pytest.approx(-2.0) + + +def test_toyota_sienna_departure_cap_does_not_change_other_vehicles(): + CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_CAMRY) + tuning = vehicle_tunes.LongControlVehicleTuning(CP) + lead = SimpleNamespace(status=True, yRel=0.0, dRel=9.0, vLead=3.0, aLeadK=0.0) + + assert tuning.cap_toyota_sienna_lead_departure_accel(1.9, 0.8, leads=(lead,)) == pytest.approx(1.9) + + def test_toyota_sienna_target_filter_bypasses_stop_and_urgent_braking(): CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN) tuning = vehicle_tunes.LongControlVehicleTuning(CP)