Offset commanded torque correctly

selfdrive/car/toyota/carcontroller.py

@@ -84,7 +84,12 @@ class CarController():
 
     # 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, frame))
-    can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, CS.out.steeringAngleDeg, CS.out.steeringTorque, apply_steer_req, frame))
+    # On TSS2 cars, the LTA and STEER_TORQUE_SENSOR messages use relative steering angle signals that start
+    # at 0 degrees, so we need to offset the commanded angle as well.
+    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))
 
     # 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: