toy

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:

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

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,
       )

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:

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

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)