diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d0068e06b0..618359c3ba 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -76,9 +76,9 @@ class CarController: apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg torque_sensor_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg - # driver torque blending - percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0]) - apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle]) + # driver torque blending - NOW HANDLED IN TOYOTACAN + # percentage = interp(abs(CS.out.steeringTorque), [40, 100], [100, 0]) + # apply_angle = interp(percentage, [-10, 100], [torque_sensor_angle, apply_angle]) # limit max angle error between cmd and actual to reduce EPS integral windup # TODO: can we configure this with a signal? @@ -133,7 +133,8 @@ class CarController: angle_control = self.CP.steerControlType == SteerControlType.angle can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req and not angle_control)) if TSS2_CAR: - can_sends.append(create_lta_steer_command(self.packer, apply_angle, apply_steer_req and angle_control, self.op_params)) + limit_torque = CS.out.steeringPressed + can_sends.append(create_lta_steer_command(self.packer, apply_angle, apply_steer_req and angle_control, limit_torque, self.op_params)) self.last_angle = apply_angle self.last_steer = apply_steer diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 1e16f70b67..e28e2b1d71 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -12,7 +12,7 @@ def create_steer_command(packer, steer, steer_req): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, apply_steer, steer_req, op_params): +def create_lta_steer_command(packer, apply_steer, steer_req, limit_torque, op_params): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { @@ -32,9 +32,9 @@ def create_lta_steer_command(packer, apply_steer, steer_req, op_params): "PERCENTAGE": op_params.get("PERCENTAGE"), # ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1 - # but on 2023 RAV4, it was constant 100 on falling edge and BIT was 0 - # TODO: figure out if important. torque wind down? maybe user torque blending? - "SETME_X64": op_params.get("SETME_X64"), + # stock system sometimes uses this signal to wind down torque + # TODO: figure out why 99 is so much less torque than 100 + "SETME_X64": 99 if limit_torque else 100, # TODO: need to understand this better, it's always 1.5-2x higher than angle cmd # TODO: revisit on 2023 RAV4