Toyota: adaptive ACCEL_NET for new long tune (#1494)

* learn

* allow all

* ohhhh

* revert

* revert

* curious if this works

* -2 to 1 is reasonable, want that to be half a second

* this isn't part of this PR, but test to see if we can do this now

* no ki

* fix that

* fix for corolla creep case, and matches better on hybrids (TODO: VERIFY)

* woo lots going on

* two rcs

* only for testing

* oops forgot -- it's amazing

* fix

* testing

* Revert "testing"

This reverts commit ec410f5f68.

* revert unrelated stuff

* it doesn't really mean anything disengaged, but should start from it

* clean up

* clean up

* clean up

* comment from other pr

* yayy no ki

opendbc/car/toyota/carcontroller.py

@@ -12,6 +12,7 @@ from opendbc.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, T
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
+Ecu = structs.CarParams.Ecu
 LongCtrlState = structs.CarControl.Actuators.LongControlState
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
@@ -44,11 +45,21 @@ class CarController(CarControllerBase):
     self.alert_active = False
     self.last_standstill = False
     self.standstill_req = False
+    self.permit_braking = True
     self.steer_rate_counter = 0
     self.distance_button = 0
 
     self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
-    self.permit_braking = True
+
+    # the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop
+    self.pcm_accel_net_offset = FirstOrderFilter(0, 1.0, DT_CTRL * 3)
+
+    # aEgo also often lags behind the PCM request due to physical brake lag which varies by car,
+    # 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)
+    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.packer = CANPacker(dbc_names[Bus.pt])
     self.accel = 0
@@ -186,10 +197,21 @@ class CarController(CarControllerBase):
 
         # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
         if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
+          # filter ACCEL_NET so it more closely matches aEgo delay for error correction
+          self.pcm_accel_net.update(CS.pcm_accel_net)
+
+          # Our model of the PCM's acceleration request isn't perfect, so we learn the offset when moving
+          new_pcm_accel_net = CS.pcm_accel_net
+          if stopping or CS.out.standstill:
+            # TODO: check if maintaining the offset from before stopping is beneficial
+            self.pcm_accel_net_offset.x = 0.0
+          else:
+            new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgo)
+
           # let PCM handle stopping for now
           pcm_accel_compensation = 0.0
           if not stopping:
-            pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request)
+            pcm_accel_compensation = 2.0 * (new_pcm_accel_net - net_acceleration_request)
 
           # prevent compensation windup
           pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX,
@@ -199,6 +221,8 @@ class CarController(CarControllerBase):
 
         else:
           self.pcm_accel_compensation.x = 0.0
+          self.pcm_accel_net_offset.x = 0.0
+          self.pcm_accel_net.x = CS.pcm_accel_net
           self.permit_braking = True
 
         # Along with rate limiting positive jerk above, this greatly improves gas response time

opendbc/car/toyota/interface.py

@@ -141,10 +141,9 @@ class CarInterface(CarInterfaceBase):
       ret.vEgoStarting = 0.25
       ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
 
-      # Since we compensate for imprecise acceleration in carcontroller, we can be less aggressive with tuning
-      # This also prevents unnecessary request windup due to internal car jerk limits
+      # Since we compensate for imprecise acceleration in carcontroller and error correct on aEgo, we can avoid using gains
       if ret.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-        tune.kiV = [0.25]
+        tune.kiV = [0.0]
     else:
       tune.kiBP = [0., 5., 35.]
       tune.kiV = [3.6, 2.4, 1.5]