Update

selfdrive/car/toyota/carcontroller.py

@@ -12,6 +12,7 @@
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
                                         UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR, Ecu
 from openpilot.selfdrive.controls.lib.drive_helpers import CRUISE_LONG_PRESS
+from openpilot.selfdrive.controls.lib.pid import PIDController
 from opendbc.can.packer import CANPacker
 
 from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_acceleration import get_max_allowed_accel
@@ -24,7 +25,7 @@
 
 # The up limit allows the brakes/gas to unwind quickly leaving a stop,
 # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup
-ACCEL_WINDUP_LIMIT = 0.5  # m/s^2 / frame
+ACCEL_WINDUP_LIMIT = 6.0 * DT_CTRL * 3  # m/s^2 / frame
 ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3  # m/s^2 / frame
 
 # LKA limits
@@ -62,8 +63,8 @@ def __init__(self, dbc_name, CP, VM):
     self.distance_button = 0
 
     self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
-    self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
 
+    self.accel_pid = PIDController(2.0, 0.5, 1 / (DT_CTRL * 3))
     self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
 
     # the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop
@@ -73,8 +74,10 @@ def __init__(self, dbc_name, CP, VM):
     # so we error correct on the filtered PCM acceleration request using the actuator delay.
     # TODO: move the delay into the interface
     self.pcm_accel_net = FirstOrderFilter(0, self.CP.longitudinalActuatorDelay, DT_CTRL * 3)
+    self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
     if not any(fw.ecu == Ecu.hybrid for fw in self.CP.carFw):
       self.pcm_accel_net.update_alpha(self.CP.longitudinalActuatorDelay + 0.2)
+      self.net_acceleration_request.update_alpha(self.CP.longitudinalActuatorDelay + 0.2)
 
     self.packer = CANPacker(dbc_name)
     self.accel = 0
@@ -253,15 +256,15 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
           # let PCM handle stopping for now, error correct on a delayed acceleration request
           pcm_accel_compensation = 0.0
           if not stopping:
-            pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x)
-
-          # prevent compensation windup
-          if frogpilot_toggles.sport_plus:
-            pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - get_max_allowed_accel(CS.out.vEgo),
-                                          pcm_accel_cmd - self.params.ACCEL_MIN)
+            # prevent compensation windup
+            if frogpilot_toggles.sport_plus:
+              self.accel_pid.neg_limit = pcm_accel_cmd - get_max_allowed_accel(CS.out.vEgo)
+            else:
+              self.accel_pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX
+            self.accel_pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN
+            pcm_accel_compensation = self.accel_pid.update(new_pcm_accel_net - self.net_acceleration_request.x)
           else:
-            pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX,
-                                          pcm_accel_cmd - self.params.ACCEL_MIN)
+            self.accel_pid.reset()
 
           pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation)
 
@@ -270,6 +273,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
           self.pcm_accel_net_offset.x = 0.0
           self.net_acceleration_request.x = 0.0
           self.pcm_accel_net.x = CS.pcm_accel_net
+          self.accel_pid.reset()
           self.permit_braking = True
 
         # Along with rate limiting positive jerk above, this greatly improves gas response time

selfdrive/car/toyota/interface.py

@@ -62,7 +62,7 @@ def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, exp
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR) \
                                         and not (ret.flags & FrogPilotToyotaFlags.SMART_DSU)
 
-    if candidate == CAR.LEXUS_ES_TSS2 and Ecu.hybrid not in found_ecus:
+    if candidate in (CAR.LEXUS_ES_TSS2, CAR.TOYOTA_COROLLA_TSS2) and Ecu.hybrid not in found_ecus:
       ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
 
     if candidate == CAR.TOYOTA_PRIUS:

selfdrive/controls/lib/pid.py

@@ -1,7 +1,7 @@
 import numpy as np
 from numbers import Number
 
-from openpilot.common.numpy_fast import clip, interp
+from opendbc.car.common.numpy_fast import clip, interp
 
 
 class PIDController:
@@ -49,10 +49,10 @@ def reset(self):
     self.f = 0.0
     self.control = 0
 
-  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False, frogpilot_toggles=None):
+  def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
     self.speed = speed
 
-    self.p = float(error) * (frogpilot_toggles.steer_kp if frogpilot_toggles and frogpilot_toggles.use_custom_kp else self.k_p)
+    self.p = float(error) * self.k_p
     self.f = feedforward * self.k_f
     self.d = error_rate * self.k_d