diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2a02a57d86..0fff0fcf6b 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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: diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 9cf86d69ab..ea28d18302 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index ed0237c1be..9c0b0643d6 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -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, diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 4a1011982c..10702269d6 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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])