diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index 446621de5d..4c241996e2 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -220,14 +220,15 @@ def update(self, CC, CS, now_nanos): j_ego = (self.aego.x - prev_aego) / (DT_CTRL * 3) a_ego_future = a_ego_blended + j_ego * 0.5 - if actuators.longControlState == LongCtrlState.pid: + if CC.longActive: # 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, - feedforward=pcm_accel_cmd) + feedforward=pcm_accel_cmd, + freeze_integrator=actuators.longControlState != LongCtrlState.pid) else: self.long_pid.reset()