test out lta message

selfdrive/car/toyota/carcontroller.py

@@ -78,14 +78,14 @@ 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, frame))
-    if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
-      can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
+    # can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame))
+    # if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
+    #   can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2))
 
     # 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.steeringAngleDeg, apply_steer_req, frame // 2))
+    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, self.torque_sensor_angle_deg, actuators.steeringAngleDeg, 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:

selfdrive/car/toyota/carstate.py

@@ -25,6 +25,7 @@ class CarState(CarStateBase):
 
     self.low_speed_lockout = False
     self.acc_type = 1
+    self.torque_sensor_angle_deg = 0
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -56,20 +57,20 @@ class CarState(CarStateBase):
     ret.standstill = ret.vEgoRaw < 0.001
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
-    torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
+    self.torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
     # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
-    if abs(torque_sensor_angle_deg) > 1e-3:
+    if abs(self.torque_sensor_angle_deg) > 1e-3:
       self.accurate_steer_angle_seen = True
 
     if self.accurate_steer_angle_seen:
       # Offset seems to be invalid for large steering angles
       if abs(ret.steeringAngleDeg) < 90 and cp.can_valid:
-        self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
+        self.angle_offset.update(self.torque_sensor_angle_deg - ret.steeringAngleDeg)
 
       if self.angle_offset.initialized:
         ret.steeringAngleOffsetDeg = self.angle_offset.x
-        ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
+        ret.steeringAngleDeg = self.torque_sensor_angle_deg - self.angle_offset.x
 
     ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
 

selfdrive/car/toyota/interface.py

@@ -130,7 +130,8 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
       ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG
-      set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+      ret.steerControlType = car.CarParams.SteerControlType.angle
+      # set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
 
     elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH):
       stop_and_go = True

selfdrive/car/toyota/toyotacan.py

@@ -10,7 +10,7 @@ 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):
+def create_lta_steer_command(packer, torque_steer, steer, steer_req, raw_cnt):
   """Creates a CAN message for the Toyota LTA Steer Command."""
 
   values = {
@@ -19,8 +19,8 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
     "SETME_X3": 3,
     "PERCENTAGE": 100,
     "SETME_X64": 0x64,
-    "ANGLE": 0,  # Rate limit? Lower values seeem to work better, but needs more testing
-    "STEER_ANGLE_CMD": steer,
+    "ANGLE": steer,  # Rate limit? Lower values seeem to work better, but needs more testing
+    "STEER_ANGLE_CMD": torque_steer,  # actually a copy of ["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]?
     "STEER_REQUEST": steer_req,
     "STEER_REQUEST_2": steer_req,
     "BIT": 0,