INDI: replace internal rate limit by actuatorsOutput (#23243)

* INDI: replace internal rate limit by actuatorsOutput

* update ref

selfdrive/controls/lib/latcontrol_indi.py

@@ -5,8 +5,6 @@ from cereal import log
 from common.filter_simple import FirstOrderFilter
 from common.numpy_fast import clip, interp
 from common.realtime import DT_CTRL
-from selfdrive.car import apply_toyota_steer_torque_limits
-from selfdrive.car.toyota.values import CarControllerParams
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
 
@@ -37,8 +35,6 @@ class LatControlINDI(LatControl):
     self.A_K = A - np.dot(K, C)
     self.x = np.array([[0.], [0.], [0.]])
 
-    self.enforce_rate_limit = CP.carName == "toyota"
-
     self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV)
     self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV)
     self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
@@ -67,7 +63,6 @@ class LatControlINDI(LatControl):
   def reset(self):
     super().reset()
     self.steer_filter.x = 0.
-    self.output_steer = 0.
     self.speed = 0.
 
   def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
@@ -90,12 +85,12 @@ class LatControlINDI(LatControl):
 
     if CS.vEgo < MIN_STEER_SPEED or not active:
       indi_log.active = False
-      self.output_steer = 0.0
       self.steer_filter.x = 0.0
+      output_steer = 0
     else:
       # Expected actuator value
       self.steer_filter.update_alpha(self.RC)
-      self.steer_filter.update(self.output_steer)
+      self.steer_filter.update(last_actuators.steer)
 
       # Compute acceleration error
       rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
@@ -107,21 +102,13 @@ class LatControlINDI(LatControl):
       delta_u = g_inv * accel_error
 
       # If steering pressed, only allow wind down
-      if CS.steeringPressed and (delta_u * self.output_steer > 0):
+      if CS.steeringPressed and (delta_u * last_actuators.steer > 0):
         delta_u = 0
 
-      # Enforce rate limit
+      output_steer = self.steer_filter.x + delta_u
-      if self.enforce_rate_limit:
-        steer_max = float(CarControllerParams.STEER_MAX)
-        new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u)
-        prev_output_steer_cmd = steer_max * self.output_steer
-        new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
-        self.output_steer = new_output_steer_cmd / steer_max
-      else:
-        self.output_steer = self.steer_filter.x + delta_u
 
       steers_max = get_steer_max(CP, CS.vEgo)
-      self.output_steer = clip(self.output_steer, -steers_max, steers_max)
+      output_steer = clip(output_steer, -steers_max, steers_max)
 
       indi_log.active = True
       indi_log.rateSetPoint = float(rate_sp)
@@ -129,7 +116,7 @@ class LatControlINDI(LatControl):
       indi_log.accelError = float(accel_error)
       indi_log.delayedOutput = float(self.steer_filter.x)
       indi_log.delta = float(delta_u)
-      indi_log.output = float(self.output_steer)
+      indi_log.output = float(output_steer)
-      indi_log.saturated = self._check_saturation(steers_max - abs(self.output_steer) < 1e-3, CS)
+      indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS)
 
-    return float(self.output_steer), float(steers_des), indi_log
+    return float(output_steer), float(steers_des), indi_log

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-2cd40ceff3c326a45c6ddf5f23e7ae2883bf55a2
+6e6f7cbfc2ccf51fb493482db2aba34eec9d2e5a