Toyota: prevent lagged gas after heavy braking (#1279)

fix
commaai · Sep 25, 2024 · bda4ad4 · bda4ad4
1 parent 8c17e03
commit bda4ad4
Showing 1 changed file with 6 additions and 1 deletion.
diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py
@@ -14,7 +14,9 @@
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
 
-ACCELERATION_DUE_TO_GRAVITY = 9.81
+ACCELERATION_DUE_TO_GRAVITY = 9.81  # m/s^2
+
+ACCEL_WINDUP_LIMIT = 0.5  # m/s^2 / frame
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -158,6 +160,9 @@ def update(self, CC, CS, now_nanos):
       if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
         can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
+        # internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
+        pcm_accel_cmd = min(pcm_accel_cmd, self.accel + ACCEL_WINDUP_LIMIT) if CC.longActive else 0.0
+
         can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert,
                                                         self.distance_button))
         self.accel = pcm_accel_cmd