mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-10 20:02:13 +08:00
Code Issues Packages Projects Releases Wiki Activity

Toyota: LTA cleanup (#30579)

Browse Source 
* have been running this for a few weeks: good

* set setme_x3 closer to stock

* add comment to values about additional EPS torque rate safety

* rename some variables

* should use vEgoRaw here to match panda!

* switch

* more notes

* specify

* smaller

* for

* oof
old-commit-hash: 54517c0638c99b7a40b146376def1ac324d07074
This commit is contained in:
Shane Smiskol
2023-12-02 00:27:29 -06:00
committed by GitHub
parent d062a97a26
commit ad5ae03be1
4 changed files with 26 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/toyota/carcontroller.py
+14 -7
View File
@@ -21,8 +21,8 @@ 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
MAX_LTA_ANGLE = 94.9461 # deg
MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes
class CarController:
@@ -71,25 +71,32 @@ class CarController:
apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
# Angular rate limit based on speed
apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params)
apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw, self.params)
if not lat_active:
apply_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
self.last_angle = clip(apply_angle, -MAX_STEER_ANGLE, MAX_STEER_ANGLE)
self.last_angle = clip(apply_angle, -MAX_LTA_ANGLE, MAX_LTA_ANGLE)
self.last_steer = apply_steer
# toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2;
# toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
# STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
# cut steering torque with SETME_X64 when either EPS torque or driver torque is above
# the threshold, to limit max lateral acceleration and for driver torque blending respectively.
full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
abs(CS.out.steeringTorque) < MAX_DRIVER_TORQUE_ALLOWANCE)
abs(CS.out.steeringTorque) < MAX_LTA_DRIVER_TORQUE_ALLOWANCE)
# SETME_X64 at 0 ramps down torque at roughly the max down rate of 1500 units/sec
setme_x64 = 100 if lta_active and full_torque_condition else 0
can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.last_angle, lta_active, self.frame // 2, setme_x64))
can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle,
lta_active, self.frame // 2, setme_x64))
# *** gas and brake ***
if self.CP.enableGasInterceptor and CC.longActive:
selfdrive/car/toyota/interface.py
+1 -1
View File
@@ -33,7 +33,7 @@ class CarInterface(CarInterfaceBase):
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
# LTA control can be more delayed and winds up more often
ret.steerActuatorDelay = 0.25
ret.steerActuatorDelay = 0.18
ret.steerLimitTimer = 0.8
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
selfdrive/car/toyota/toyotacan.py
+9 -3
View File
@@ -1,3 +1,8 @@
from cereal import car
SteerControlType = car.CarParams.SteerControlType
def create_steer_command(packer, steer, steer_req):
"""Creates a CAN message for the Toyota Steer Command."""
@@ -9,13 +14,14 @@ 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, setme_x64):
def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, setme_x64):
"""Creates a CAN message for the Toyota LTA Steer Command."""
values = {
"COUNTER": frame + 128,
"SETME_X1": 1,
"SETME_X3": 3,
"SETME_X1": 1, # suspected LTA feature availability
# 1 for TSS 2.5 cars, 3 for TSS 2.0. Send based on whether we're using LTA for lateral control
"SETME_X3": 1 if steer_control_type == SteerControlType.angle else 3,
"PERCENTAGE": 100,
"SETME_X64": setme_x64,
"ANGLE": 0,
selfdrive/car/toyota/values.py
+2 -1
View File
@@ -27,7 +27,8 @@ class CarControllerParams:
# Assuming a steering ratio of 13.7:
# Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
# Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
# however the EPS has its own internal limits at all speeds which are less than that
# however the EPS has its own internal limits at all speeds which are less than that:
# Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])