use car's SETME's

selfdrive/car/toyota/carcontroller.py

@@ -89,7 +89,7 @@ class CarController():
     can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
                                               CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
                                               CS.out.steeringTorque,
-                                              apply_steer_req, frame))
+                                              apply_steer_req, CS.steering_lta, frame))
 
     # 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

@@ -59,6 +59,8 @@ class CarState(CarStateBase):
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
     self.torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
 
+    self.steering_lta = cp.vl["STEERING_LTA"]
+
     # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero
     if abs(self.torque_sensor_angle_deg) > 1e-3:
       self.accurate_steer_angle_seen = True
@@ -209,12 +211,16 @@ class CarState(CarStateBase):
   def get_cam_can_parser(CP):
     signals = [
       ("FORCE", "PRE_COLLISION"),
+      ("SETME_X1", "STEERING_LTA"),
+      ("SETME_X3", "STEERING_LTA"),
+      ("SETME_X64", "STEERING_LTA"),
       ("PRECOLLISION_ACTIVE", "PRE_COLLISION"),
     ]
 
     # use steering message to check if panda is connected to frc
     checks = [
       ("STEERING_LKA", 42),
+      ("STEERING_LTA", 0),
       ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent
     ]
 

selfdrive/car/toyota/toyotacan.py

@@ -12,17 +12,17 @@ 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, apply_steer, steer_angle, driver_torque, steer_req, raw_cnt):
+def create_lta_steer_command(packer, apply_steer, steer_angle, driver_torque, steer_req, steering_lta, raw_cnt):
   """Creates a CAN message for the Toyota LTA Steer Command."""
 
   percentage = interp(abs(driver_torque), [50, 100], [100, 0])
   apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer])
   values = {
     "COUNTER": raw_cnt,  # 0 to 62
-    "SETME_X1": 1,
-    "SETME_X3": 1,  # usually 1, but sometimes 3 (??)
+    "SETME_X1": steering_lta["SETME_X1"],
+    "SETME_X3": steering_lta["SETME_X3"],  # usually 1, but sometimes 3 (??)
     "PERCENTAGE": percentage,  # correlated with driver torque
-    "SETME_X64": 0x64,  # ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1
+    "SETME_X64": steering_lta["SETME_X64"],  # ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1
     "ANGLE": apply_steer * 1.5,  # TODO: need to understand this better, it's always 1.5-2x higher than angle cmd
     "STEER_ANGLE_CMD": apply_steer,  # seems to just be desired angle cmd
     "STEER_REQUEST": steer_req,  # stock system turns off steering after ~20 frames of override, else torque winds up