[experiment] apply some rate limiting and anti-windup

panda

@@ -1 +1 @@
-Subproject commit b0d9d0ec96a1d414c16584d018f8a729914e9c0a
+Subproject commit 8f946ce3576a23e57bf3e8933e92d8f32f6acd8e

selfdrive/car/toyota/carcontroller.py

@@ -10,6 +10,7 @@ from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR,
 from opendbc.can.packer import CANPacker
 from common.op_params import opParams
 
+SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -24,9 +25,10 @@ class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.op_params = opParams()
     self.CP = CP
-    self.torque_rate_limits = CarControllerParams(self.CP)
+    self.params = CarControllerParams(self.CP)
     self.frame = 0
     self.last_steer = 0
+    self.last_angle = 0
     self.alert_active = False
     self.last_standstill = False
     self.standstill_req = False
@@ -58,11 +60,23 @@ class CarController:
       interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
     else:
       interceptor_gas_cmd = 0.
-    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
-
+
-    # steer torque
+    # - steer torque
-    new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
+    new_steer = int(round(actuators.steer * self.params.STEER_MAX))
-    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
+    apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params)
+
+    # - steer angle
+    # 1a. angle command is in terms of the angle signal in the torque sensor message (may or may not have an offset)
+    #  1b. clip to max angle limits (EPS ignores messages outside of these bounds and causes PCS faults)
+    # 2. rate limit angle command
+    # 3. limit max angle error between cmd and actual to reduce EPS integral windup  # TODO: can we configure this with a signal?
+    torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
+    apply_angle = clip(actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, -94.9461, 94.9461)
+    apply_angle = clip(apply_angle, self.last_angle - self.params.ANGLE_RATE_MAX, self.last_angle + self.params.ANGLE_RATE_MAX)
+    apply_steer = clip(apply_steer,
+                       torque_sensor_angle - self.params.ANGLE_DELTA_MAX,
+                       torque_sensor_angle + self.params.ANGLE_DELTA_MAX)
 
     # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault
     if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE:
@@ -72,12 +86,18 @@ class CarController:
 
     apply_steer_req = 1
     if not lat_active:
+      apply_angle = torque_sensor_angle
       apply_steer = 0
       apply_steer_req = 0
     elif self.steer_rate_counter > MAX_STEER_RATE_FRAMES:
       apply_steer_req = 0
       self.steer_rate_counter = 0
 
+    if self.CP.steerControlType == SteerControlType.angle:
+      apply_steer = 0
+    else:
+      apply_angle = 0
+
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
     if not CC.enabled and CS.pcm_acc_status:
@@ -90,9 +110,6 @@ class CarController:
       # pcm entered standstill or it's disabled
       self.standstill_req = False
 
-    self.last_steer = apply_steer
-    self.last_standstill = CS.out.standstill
-
     can_sends = []
 
     # *** control msgs ***
@@ -101,20 +118,26 @@ class CarController:
     # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
     # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
     # on consecutive messages
-    """
+
-    can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
+    if self.CP.steerControlType == SteerControlType.angle:
-    if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
+      apply_steer = 0
-      can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2))
+      # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
-    """
+      can_sends.append(create_steer_command(self.packer, 0, 0))
-
+      # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
-    # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
+      # at 0 degrees, so we need to offset the commanded angle as well.
-    can_sends.append(create_steer_command(self.packer, 0, 0))
+      can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
-    # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
+                                                CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
-    # at 0 degrees, so we need to offset the commanded angle as well.
+                                                CS.out.steeringTorque,
-    can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
+                                                apply_steer_req, self.op_params))
-                                              CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
+    else:
-                                              CS.out.steeringTorque,
+      apply_angle = 0
-                                              apply_steer_req, self.op_params))
+      can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req))
+      if TSS2_CAR:
+        can_sends.append(create_lta_steer_command(self.packer, 0, 0, 0, False, self.op_params))
+
+    self.last_angle = apply_angle
+    self.last_steer = apply_steer
+    self.last_standstill = CS.out.standstill
 
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
@@ -165,7 +188,7 @@ class CarController:
         can_sends.append(make_can_msg(addr, vl, bus))
 
     new_actuators = actuators.copy()
-    new_actuators.steer = apply_steer / CarControllerParams.STEER_MAX
+    new_actuators.steer = apply_steer / self.params.STEER_MAX
     new_actuators.accel = self.accel
     new_actuators.gas = self.gas
 

selfdrive/car/toyota/toyotacan.py

@@ -15,8 +15,8 @@ def create_steer_command(packer, steer, steer_req):
 def create_lta_steer_command(packer, apply_steer, steer_angle, driver_torque, steer_req, op_params):
   """Creates a CAN message for the Toyota LTA Steer Command."""
 
-  percentage = interp(abs(driver_torque), [50, 100], [100, 0])
+  # percentage = interp(abs(driver_torque), [40, 100], [100, 0])
-  apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer])
+  # apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer])
   values = {
     # seems to actually be 1. Even 1 on 2023 RAV4 2023 (TODO: check from data)
     "SETME_X1": op_params.get("SETME_X1"),

selfdrive/car/toyota/values.py

@@ -21,6 +21,12 @@ class CarControllerParams:
   STEER_MAX = 1500
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 
+  # stock LTA is 0 to 0.05 going straight
+  # and 0.1 to 0.4 when turning (max seen is 0.6303)
+  ANGLE_RATE_MAX = 0.5
+  # needs to be within +-3 degrees of current angle to avoid windup
+  ANGLE_DELTA_MAX = 3.0
+
   def __init__(self, CP):
     if CP.lateralTuning.which == 'torque':
       self.STEER_DELTA_UP = 15       # 1.0s time to peak torque