diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index b3806dc1ef..c41f6f8dec 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -22,6 +22,7 @@ # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup ACCEL_WINDUP_LIMIT = 4.0 * DT_CTRL * 3 # m/s^2 / frame ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame +ACCEL_PID_UNWIND = 0.03 * 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 @@ -220,6 +221,9 @@ def update(self, CC, CS, now_nanos): a_ego_future = a_ego_blended + j_ego * 0.5 if actuators.longControlState == LongCtrlState.pid: + # constantly slowly unwind integral to recover from large temporary errors + self.long_pid.i -= ACCEL_PID_UNWIND * float(np.sign(self.long_pid.i)) + error_future = pcm_accel_cmd - a_ego_future pcm_accel_cmd = self.long_pid.update(error_future, speed=CS.out.vEgo,