Add code to steer using TSS2 LTA message (#1507)
* try again with toyota LTA * Fix dbc and send at 50Hz * Add commented code * Remove unused import * Remove thatalbatross
parent
7b35533ebd
commit
49dd37b81b
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Reference in New Issue