long

selfdrive/controls/lib/longcontrol.py

@@ -271,6 +271,7 @@ class LongControl:
 
     else:  # LongCtrlState.pid
       a_target = self.vehicle_tuning.shape_gm_truck_accel_target(a_target, CS.vEgo, should_stop)
+      a_target = self.vehicle_tuning.shape_toyota_sienna_accel_target(a_target, CS.vEgo, should_stop)
       error = a_target - CS.aEgo
       self.update_mpc_mode(self.experimental_mode)
       self.vehicle_tuning.shape_volt_test_tune_integrator(self.pid, error, CS.vEgo)

selfdrive/controls/lib/longcontrol_vehicle_tunes.py

@@ -1,6 +1,7 @@
 import numpy as np
 
 from opendbc.car.gm.values import CAR, GMFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 from openpilot.common.realtime import DT_CTRL
 from openpilot.starpilot.common.testing_grounds import testing_ground
 
@@ -22,6 +23,11 @@ GM_TRUCK_TARGET_FILTER_UP_TAU = 0.10
 GM_TRUCK_TARGET_FILTER_DOWN_TAU = 0.06
 GM_TRUCK_TARGET_FILTER_BRAKE_BYPASS = -0.65
 GM_TRUCK_TARGET_FILTER_DROP_BYPASS = 0.45
+TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED = 12.0
+TOYOTA_SIENNA_TARGET_FILTER_UP_TAU = 0.18
+TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU = 0.12
+TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS = -0.75
+TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS = 0.65
 
 
 def get_bolt_acc_pedal_friction_bias(output_accel, a_target, v_ego):
@@ -86,6 +92,10 @@ class LongControlVehicleTuning:
       getattr(CP, "carFingerprint", None) in (CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_SILVERADO_CC) and
       not CP.enableGasInterceptorDEPRECATED
     )
+    self.is_toyota_sienna_4g = bool(
+      CP.brand == "toyota" and
+      getattr(CP, "carFingerprint", None) == TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN
+    )
     self.is_bolt_acc_pedal_friction_car = bool(
       CP.brand == "gm" and
       CP.enableGasInterceptorDEPRECATED and
@@ -99,6 +109,8 @@ class LongControlVehicleTuning:
     self.integrator_hold_frames = 0
     self.gm_truck_filtered_a_target = 0.0
     self.gm_truck_target_filter_initialized = False
+    self.toyota_sienna_filtered_a_target = 0.0
+    self.toyota_sienna_target_filter_initialized = False
     self.bolt_start_handoff_frames = 0
 
   def apply_bolt_start_handoff_floor(self, output_accel, last_output_accel, a_target, v_ego,
@@ -147,6 +159,29 @@ class LongControlVehicleTuning:
     self.gm_truck_filtered_a_target += alpha * (float(a_target) - self.gm_truck_filtered_a_target)
     return self.gm_truck_filtered_a_target
 
+  def shape_toyota_sienna_accel_target(self, a_target, v_ego, should_stop):
+    """Dampen ordinary high-speed lead handoffs without delaying safety braking."""
+    if not self.is_toyota_sienna_4g or v_ego < TOYOTA_SIENNA_TARGET_FILTER_MIN_SPEED or should_stop:
+      self.toyota_sienna_target_filter_initialized = False
+      return a_target
+
+    bypass_filter = (
+      a_target <= TOYOTA_SIENNA_TARGET_FILTER_BRAKE_BYPASS or
+      (self.toyota_sienna_target_filter_initialized and
+       a_target < self.toyota_sienna_filtered_a_target - TOYOTA_SIENNA_TARGET_FILTER_DROP_BYPASS)
+    )
+    if not self.toyota_sienna_target_filter_initialized or bypass_filter:
+      self.toyota_sienna_filtered_a_target = float(a_target)
+      self.toyota_sienna_target_filter_initialized = True
+      return float(a_target)
+
+    tau = (TOYOTA_SIENNA_TARGET_FILTER_DOWN_TAU
+           if a_target < self.toyota_sienna_filtered_a_target
+           else TOYOTA_SIENNA_TARGET_FILTER_UP_TAU)
+    alpha = DT_CTRL / (tau + DT_CTRL)
+    self.toyota_sienna_filtered_a_target += alpha * (float(a_target) - self.toyota_sienna_filtered_a_target)
+    return self.toyota_sienna_filtered_a_target
+
   def get_integrator_freeze(self, last_output_accel, a_target, error, v_ego, accel_limits):
     volt_test_tune_handoff = self.is_volt and testing_ground.use_2
 

selfdrive/controls/tests/test_longcontrol.py

@@ -6,6 +6,7 @@ import pytest
 import openpilot.selfdrive.controls.lib.longcontrol as longcontrol
 import openpilot.selfdrive.controls.lib.longcontrol_vehicle_tunes as vehicle_tunes
 from opendbc.car.gm.values import CAR, GMFlags
+from opendbc.car.toyota.values import CAR as TOYOTA_CAR
 from openpilot.selfdrive.controls.lib.longcontrol import (
   LongControl,
   LongCtrlState,
@@ -963,6 +964,32 @@ def test_gm_stock_truck_target_filter_bypasses_low_speed_and_other_cars():
   assert bolt_tuning.shape_gm_truck_accel_target(-0.10, 20.0, False) == pytest.approx(-0.10)
 
 
+def test_toyota_sienna_target_filter_smooths_mild_high_speed_handoffs():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_SIENNA_4TH_GEN)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+
+  assert tuning.shape_toyota_sienna_accel_target(0.30, 20.0, False) == pytest.approx(0.30)
+  filtered = tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, False)
+
+  assert -0.20 < filtered < 0.30
+
+
+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)
+
+  tuning.shape_toyota_sienna_accel_target(0.30, 20.0, False)
+  assert tuning.shape_toyota_sienna_accel_target(-0.80, 20.0, False) == pytest.approx(-0.80)
+  assert tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, True) == pytest.approx(-0.20)
+
+
+def test_toyota_sienna_target_filter_does_not_change_other_vehicles():
+  CP = make_longcontrol_cp(brand="toyota", carFingerprint=TOYOTA_CAR.TOYOTA_CAMRY)
+  tuning = vehicle_tunes.LongControlVehicleTuning(CP)
+
+  assert tuning.shape_toyota_sienna_accel_target(-0.20, 20.0, False) == pytest.approx(-0.20)
+
+
 def test_gm_stock_truck_positive_i_bleeds_during_light_highway_accel_request():
   CP = car.CarParams.new_message()
   CP.brand = "gm"