From 9112df2b5eac6c10fcc8197356c068c050821e61 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Tue, 18 Jul 2023 01:58:21 -0700
Subject: [PATCH] Toyota LTA: limit max angle with applied torque (#28848)

* draft

* clean up

* there was a bug, need to subtract from last_angle essentially

* Revert "there was a bug, need to subtract from last_angle essentially"

This reverts commit a595e4af64523665519fd450dbeb2fefd293ce2b.

* maybe

* Revert "maybe"

This reverts commit cc96523cc28b82e74018011e5a476be2e6f68d92.

* or can do this

* this seems simpler and actually would work

* update comment

* fix that case

* joystick testing

* revert testing changes

* comment should be more clear

* can have full thing

* bump panda to lta

* revert cc

* this seems to work well

* constants

* try setme_x64

* 99 just winds up torque?!

* try less torque

* add corolla

* clean up

* clean up

* clean up

* clean up

* better name

* -which

* fix

* fix typo

* use params
old-commit-hash: c01ba907c029ee76ba4c7084a7ae598b6c9fbaae
---
 selfdrive/car/toyota/carcontroller.py | 6 +++++-
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 7 insertions(+), 3 deletions(-)

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,