Toyota: LTA cleanup (#30579)

* have been running this for a few weeks: good

* set setme_x3 closer to stock

* add comment to values about additional EPS torque rate safety

* rename some variables

* should use vEgoRaw here to match panda!

* switch

* more notes

* specify

* smaller

* for

* oof
old-commit-hash: 54517c0638

selfdrive/car/toyota/carcontroller.py

@@ -21,8 +21,8 @@ MAX_USER_TORQUE = 500
 
 # LTA limits
 # EPS ignores commands above this angle and causes PCS to fault
-MAX_STEER_ANGLE = 94.9461  # deg
-MAX_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows some resistance when changing lanes
+MAX_LTA_ANGLE = 94.9461  # deg
+MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150  # slightly above steering pressed allows some resistance when changing lanes
 
 
 class CarController:
@@ -71,25 +71,32 @@ class CarController:
         apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
 
         # Angular rate limit based on speed
-        apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params)
+        apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw, self.params)
 
         if not lat_active:
           apply_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
 
-        self.last_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE)
+        self.last_angle = clip(apply_angle, -MAX_LTA_ANGLE, MAX_LTA_ANGLE)
 
     self.last_steer = apply_steer
 
-    # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
+    # toyota can trace shows STEERING_LKA 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(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
+
+    # STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier
     if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
       lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
+      # cut steering torque with SETME_X64 when either EPS torque or driver torque is above
+      # the threshold, to limit max lateral acceleration and for driver torque blending respectively.
       full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
-                               abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE)
+                               abs(CS.out.steeringTorque) < MAX_LTA_DRIVER_TORQUE_ALLOWANCE)
+
+      # SETME_X64 at 0 ramps down torque at roughly the max down rate of 1500 units/sec
       setme_x64 = 100 if lta_active and full_torque_condition else 0
-      can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64))
+      can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle,
+                                                          lta_active, self.frame // 2, setme_x64))
 
     # *** gas and brake ***
     if self.CP.enableGasInterceptor and CC.longActive:

selfdrive/car/toyota/interface.py

@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
       ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
 
       # LTA control can be more delayed and winds up more often
-      ret.steerActuatorDelay = 0.25
+      ret.steerActuatorDelay = 0.18
       ret.steerLimitTimer = 0.8
     else:
       CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)

selfdrive/car/toyota/toyotacan.py

@@ -1,3 +1,8 @@
+from cereal import car
+
+SteerControlType = car.CarParams.SteerControlType
+
+
 def create_steer_command(packer, steer, steer_req):
   """Creates a CAN message for the Toyota Steer Command."""
 
@@ -9,13 +14,14 @@ def create_steer_command(packer, steer, steer_req):
   return packer.make_can_msg("STEERING_LKA", 0, values)
 
 
-def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64):
+def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, setme_x64):
   """Creates a CAN message for the Toyota LTA Steer Command."""
 
   values = {
     "COUNTER": frame + 128,
-    "SETME_X1": 1,
-    "SETME_X3": 3,
+    "SETME_X1": 1,  # suspected LTA feature availability
+    # 1 for TSS 2.5 cars, 3 for TSS 2.0. Send based on whether we're using LTA for lateral control
+    "SETME_X3": 1 if steer_control_type == SteerControlType.angle else 3,
     "PERCENTAGE": 100,
     "SETME_X64": setme_x64,
     "ANGLE": 0,

selfdrive/car/toyota/values.py

@@ -27,7 +27,8 @@ class CarControllerParams:
   # Assuming a steering ratio of 13.7:
   # Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
   # Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
-  # however the EPS has its own internal limits at all speeds which are less than that
+  # however the EPS has its own internal limits at all speeds which are less than that:
+  # Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA
   ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15])
   ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])