diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 58a970f788..0346801c0d 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -2,7 +2,8 @@ from cereal import car from common.numpy_fast import clip from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ - create_accel_command, create_acc_cancel_command, create_fcw_command + create_accel_command, create_acc_cancel_command, \ + create_fcw_command from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams from opendbc.can.packer import CANPacker @@ -107,6 +108,11 @@ class CarController(): if Ecu.fwdCamera in self.fake_ecus: can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) + # 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.steerAngle, 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 and Ecu.fwdCamera in self.fake_ecus): lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index a5d1497291..bdd84e7128 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -10,17 +10,19 @@ 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, angle): +def create_lta_steer_command(packer, steer, steer_req, raw_cnt): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { - "COUNTER": raw_cnt, + "COUNTER": raw_cnt + 128, + "SETME_X1": 1, "SETME_X3": 3, - "PERCENTAGE" : 100, + "PERCENTAGE": 100, "SETME_X64": 0x64, - "ANGLE": angle, + "ANGLE": 0, # Rate limit? Lower values seeem to work better, but needs more testing "STEER_ANGLE_CMD": steer, "STEER_REQUEST": steer_req, + "STEER_REQUEST_2": steer_req, "BIT": 0, } return packer.make_can_msg("STEERING_LTA", 0, values)