diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 4ae800eb6b..9062fea6c1 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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: