Add code to steer using TSS2 LTA message (#1507)

* try again with toyota LTA

* Fix dbc and send at 50Hz

* Add commented code

* Remove unused import

* Remove that
old-commit-hash: 49dd37b81b

selfdrive/car/toyota/carcontroller.py

@@ -2,7 +2,8 @@ from cereal import car
 from common.numpy_fast import clip
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
-                                           create_accel_command, create_acc_cancel_command, create_fcw_command
+                                           create_accel_command, create_acc_cancel_command, \
+                                           create_fcw_command
 from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams
 from opendbc.can.packer import CANPacker
 
@@ -107,6 +108,11 @@ class CarController():
     if Ecu.fwdCamera in self.fake_ecus:
       can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
 
+      # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
+      # if frame % 2 == 0:
+      #   can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
+      #   can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))
+
     # we can spam can to cancel the system even if we are using lat only control
     if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):
       lead = lead or CS.out.vEgo < 12.    # at low speed we always assume the lead is present do ACC can be engaged

selfdrive/car/toyota/toyotacan.py

@@ -10,17 +10,19 @@ def create_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LKA", 0, values)
 
 
-def create_lta_steer_command(packer, steer, steer_req, raw_cnt, angle):
+def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   """Creates a CAN message for the Toyota LTA Steer Command."""
 
   values = {
-    "COUNTER": raw_cnt,
+    "COUNTER": raw_cnt + 128,
+    "SETME_X1": 1,
     "SETME_X3": 3,
-    "PERCENTAGE" : 100,
+    "PERCENTAGE": 100,
     "SETME_X64": 0x64,
-    "ANGLE": angle,
+    "ANGLE": 0,  # Rate limit? Lower values seeem to work better, but needs more testing
     "STEER_ANGLE_CMD": steer,
     "STEER_REQUEST": steer_req,
+    "STEER_REQUEST_2": steer_req,
     "BIT": 0,
   }
   return packer.make_can_msg("STEERING_LTA", 0, values)