Toyota: unwind longitudinal integral (#1652)

* fix undershoot

* clean up

* maybe fine

* unwind instead

* revert

* up here

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,