From 8de5f5197163ffebd0728d1e49a946e80cef8e6f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 3 Aug 2026 00:59:12 -0500 Subject: [PATCH] long --- selfdrive/controls/lib/longcontrol.py | 1 + .../controls/lib/longcontrol_vehicle_tunes.py | 35 +++++++++++++++++++ selfdrive/controls/tests/test_longcontrol.py | 27 ++++++++++++++ 3 files changed, 63 insertions(+) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 0dcbbae35..999a690ed 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/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) diff --git a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py b/selfdrive/controls/lib/longcontrol_vehicle_tunes.py index 20ac156a4..b4c215f65 100644 --- a/selfdrive/controls/lib/longcontrol_vehicle_tunes.py +++ b/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 diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 86327634b..32e58dd4a 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/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"