From c848b5f1b450af7fce93f5b487880c288a9791f8 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Thu, 21 May 2026 10:03:12 -0500
Subject: [PATCH] toyota

---
 .../opendbc/car/toyota/carcontroller.py       | 49 +++++++++++--------
 1 file changed, 28 insertions(+), 21 deletions(-)

diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py
index f54e33b5c..66b58fc87 100644
--- a/opendbc_repo/opendbc/car/toyota/carcontroller.py
+++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py
@@ -347,31 +347,38 @@ class CarController(CarControllerBase):
         a_ego_future = a_ego_blended + j_ego * future_t
 
         if CC.longActive:
-          # constantly slowly unwind integral to recover from large temporary errors
-          unwind_rate = ACCEL_PID_UNWIND
-          if self.CP.carFingerprint == CAR.TOYOTA_PRIUS and pcm_accel_cmd * self.long_pid.i < 0.0:
-            unwind_rate *= PRIUS_INTEGRAL_MISMATCH_UNWIND
-          self.long_pid.i -= unwind_rate * float(np.sign(self.long_pid.i))
+          if self.CP.enableGasInterceptorDEPRECATED:
+            # Pedal/SDSU Toyotas have shown better behavior when we trust the planner
+            # target directly instead of letting the Toyota longitudinal PID swing it
+            # around. Keep the shared rate limits above, but bypass the extra
+            # correction layer that causes rev/release oscillation.
+            self.long_pid.reset()
+          else:
+            # constantly slowly unwind integral to recover from large temporary errors
+            unwind_rate = ACCEL_PID_UNWIND
+            if self.CP.carFingerprint == CAR.TOYOTA_PRIUS and pcm_accel_cmd * self.long_pid.i < 0.0:
+              unwind_rate *= PRIUS_INTEGRAL_MISMATCH_UNWIND
+            self.long_pid.i -= unwind_rate * float(np.sign(self.long_pid.i))
 
-          error_future = pcm_accel_cmd - a_ego_future
+            error_future = pcm_accel_cmd - a_ego_future
 
-          if not stopping:
-            # Toyota's PCM slowly responds to changes in pitch. On change, we amplify our
-            # acceleration request to compensate for the undershoot and following overshoot
-            pitch_compensation = float(np.clip(math.sin(self.pitch_hp.x) * ACCELERATION_DUE_TO_GRAVITY,
-                                               -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
-            pcm_accel_cmd += pitch_compensation
+            if not stopping:
+              # Toyota's PCM slowly responds to changes in pitch. On change, we amplify our
+              # acceleration request to compensate for the undershoot and following overshoot
+              pitch_compensation = float(np.clip(math.sin(self.pitch_hp.x) * ACCELERATION_DUE_TO_GRAVITY,
+                                                 -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
+              pcm_accel_cmd += pitch_compensation
 
-          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 = 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
 
-          pcm_accel_cmd = self.long_pid.update(error_future,
-                                               speed=CS.out.vEgo,
-                                               feedforward=feedforward,
-                                               freeze_integrator=actuators.longControlState != LongCtrlState.pid)
+            pcm_accel_cmd = self.long_pid.update(error_future,
+                                                 speed=CS.out.vEgo,
+                                                 feedforward=feedforward,
+                                                 freeze_integrator=actuators.longControlState != LongCtrlState.pid)
         else:
           self.long_pid.reset()