diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index fa9a42ee16..905251b532 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -24,6 +24,7 @@ MAX_USER_TORQUE = 500 # LTA limits # EPS ignores commands above this angle and causes PCS to fault MAX_STEER_ANGLE = 94.9461 # deg +MAX_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes class CarController: @@ -94,7 +95,10 @@ class CarController: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle - can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2)) + full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and + abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE) + setme_x64 = 100 if lta_active and full_torque_condition else 0 + can_sends.append(create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64)) # *** gas and brake *** if self.CP.enableGasInterceptor and CC.longActive: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 38351139a8..01861c534a 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -9,7 +9,7 @@ def create_steer_command(packer, steer, steer_req): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, steer_angle, steer_req, frame): +def create_lta_steer_command(packer, steer_angle, steer_req, frame, setme_x64): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { @@ -17,7 +17,7 @@ def create_lta_steer_command(packer, steer_angle, steer_req, frame): "SETME_X1": 1, "SETME_X3": 3, "PERCENTAGE": 100, - "SETME_X64": 0, + "SETME_X64": setme_x64, "ANGLE": 0, "STEER_ANGLE_CMD": steer_angle, "STEER_REQUEST": steer_req,