From a7ad72489ba0f14c1baeb24f5f7f433b5d342918 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sat, 27 Jun 2026 23:45:56 -0500 Subject: [PATCH] pri pri --- opendbc_repo/opendbc/car/toyota/carcontroller.py | 9 +++++++-- opendbc_repo/opendbc/car/toyota/tests/test_toyota.py | 12 ++++++++++-- 2 files changed, 17 insertions(+), 4 deletions(-) diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index ea38d890e..a2297e1a4 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -26,6 +26,7 @@ ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_PID_UNWIND = 0.03 * DT_CTRL * 3 # m/s^2 / frame PRIUS_INTEGRAL_MISMATCH_UNWIND = 8.0 PRIUS_POSITIVE_FEEDFORWARD_SCALE = 0.7 +PRIUS_CRUISE_FEEDFORWARD_SCALE = 0.85 MAX_PITCH_COMPENSATION = 1.5 # m/s^2 TOYOTA_COAST_BRAKE_MIN_SPEED = 15.0 # m/s @@ -66,6 +67,11 @@ def get_long_tune(CP, params): rate=1 / (DT_CTRL * 3)) +def get_prius_positive_feedforward_scale(v_ego: float) -> float: + return float(np.interp(v_ego, [0.0, 8.0, 20.0], + [PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_POSITIVE_FEEDFORWARD_SCALE, PRIUS_CRUISE_FEEDFORWARD_SCALE])) + + def update_permit_braking(current: bool, net_acceleration_request_min: float, stopping: bool, long_active: bool, v_ego: float, lead_visible: bool) -> bool: if stopping or not long_active: @@ -435,9 +441,8 @@ class CarController(CarControllerBase): feedforward = pcm_accel_cmd if self.CP.carFingerprint == CAR.TOYOTA_PRIUS: - # Keep Prius positive handoffs softer than the stock tune, while restoring some launch authority. if feedforward > 0.0: - feedforward *= PRIUS_POSITIVE_FEEDFORWARD_SCALE + feedforward *= get_prius_positive_feedforward_scale(CS.out.vEgo) pcm_accel_cmd = self.long_pid.update(error_future, speed=CS.out.vEgo, diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index d71aa912e..2b444a8f9 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -7,8 +7,8 @@ from opendbc.can import CANPacker, CANParser from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict from opendbc.car.toyota import toyotacan -from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, \ - limit_prius_stopping_accel, update_permit_braking +from opendbc.car.toyota.carcontroller import CarController, get_prius_positive_feedforward_scale, limit_interceptor_pcm_accel, \ + limit_interceptor_stopping_accel, limit_prius_stopping_accel, update_permit_braking from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.interface import CarInterface @@ -300,6 +300,14 @@ class TestToyotaCarController: limited = limit_prius_stopping_accel(-3.28, -2.0, True, 0.0, True) assert limited == -3.28 + def test_prius_positive_feedforward_scale_stays_soft_at_launch_speed(self): + assert abs(get_prius_positive_feedforward_scale(0.0) - 0.7) < 1e-6 + assert abs(get_prius_positive_feedforward_scale(8.0) - 0.7) < 1e-6 + + def test_prius_positive_feedforward_scale_restores_cruise_authority(self): + assert get_prius_positive_feedforward_scale(20.0) > get_prius_positive_feedforward_scale(8.0) + assert abs(get_prius_positive_feedforward_scale(20.0) - 0.85) < 1e-6 + def test_sng_hack_clears_existing_standstill_latch(self): controller = self._make_controller(standstill_req=True, last_standstill=True)