diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index d75573aec7..c79a471465 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -85,7 +85,7 @@ class CarController(): # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda if frame % 2 == 0: can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) - can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) + can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, CS.out.steeringTorque, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 49ad84d898..45f1264785 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,3 +1,5 @@ +from common.numpy_fast import interp + def create_steer_command(packer, steer, steer_req, raw_cnt): """Creates a CAN message for the Toyota Steer Command.""" @@ -10,14 +12,15 @@ def create_steer_command(packer, steer, steer_req, raw_cnt): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, steer, steer_req, raw_cnt): +def create_lta_steer_command(packer, steer, driver_torque, steer_req, raw_cnt): """Creates a CAN message for the Toyota LTA Steer Command.""" + percentage = interp(abs(driver_torque), [40, 100], [100, 0]) values = { "COUNTER": raw_cnt + 128, "SETME_X1": 1, "SETME_X3": 3, - "PERCENTAGE": 100, + "PERCENTAGE": percentage, "SETME_X64": 0x64, "ANGLE": steer, # Rate limit? Lower values seeem to work better, but needs more testing "STEER_ANGLE_CMD": steer,