From f9abb9e473b0e82511d107ffac4b747aa2099b6b Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 13 Nov 2024 17:23:17 -0600 Subject: [PATCH] Toyota: add accel winddown limit (#1481) * add rate limits * add * oops * fix * fix --- opendbc/car/toyota/carcontroller.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 366873c1..8dd5654c 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ import math from opendbc.car import carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \ - make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY + make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL from opendbc.car.can_definitions import CanData from opendbc.car.common.numpy_fast import clip from opendbc.car.secoc import add_mac, build_sync_mac @@ -15,7 +15,10 @@ LongCtrlState = structs.CarControl.Actuators.LongControlState SteerControlType = structs.CarParams.SteerControlType VisualAlert = structs.CarControl.HUDControl.VisualAlert +# The up limit allows the brakes/gas to unwind quickly leaving a stop, +# the down limit matches the rate of ACCEL_NET, reducing PCM compensation windup ACCEL_WINDUP_LIMIT = 0.5 # m/s^2 / frame +ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3 # m/s^2 / frame # LKA limits # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long @@ -171,7 +174,9 @@ class CarController(CarControllerBase): self.distance_button = 0 # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit - pcm_accel_cmd = min(actuators.accel, self.prev_accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0 + pcm_accel_cmd = actuators.accel + if CC.longActive: + pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT) # calculate amount of acceleration PCM should apply to reach target, given pitch accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY if len(CC.orientationNED) == 3 else 0.0