diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 9062fea6c1..70a4bb5d86 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -89,7 +89,7 @@ class CarController(): can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg, CS.out.steeringTorque, - apply_steer_req, frame)) + apply_steer_req, CS.steering_lta, frame)) # 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/carstate.py b/selfdrive/car/toyota/carstate.py index 328dbdeef4..147abb08d1 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -59,6 +59,8 @@ class CarState(CarStateBase): ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] self.torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + self.steering_lta = cp.vl["STEERING_LTA"] + # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero if abs(self.torque_sensor_angle_deg) > 1e-3: self.accurate_steer_angle_seen = True @@ -209,12 +211,16 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): signals = [ ("FORCE", "PRE_COLLISION"), + ("SETME_X1", "STEERING_LTA"), + ("SETME_X3", "STEERING_LTA"), + ("SETME_X64", "STEERING_LTA"), ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), ] # use steering message to check if panda is connected to frc checks = [ ("STEERING_LKA", 42), + ("STEERING_LTA", 0), ("PRE_COLLISION", 0), # TODO: figure out why freq is inconsistent ] diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index c32c54ed11..f59367378f 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -12,17 +12,17 @@ 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, apply_steer, steer_angle, driver_torque, steer_req, raw_cnt): +def create_lta_steer_command(packer, apply_steer, steer_angle, driver_torque, steer_req, steering_lta, raw_cnt): """Creates a CAN message for the Toyota LTA Steer Command.""" percentage = interp(abs(driver_torque), [50, 100], [100, 0]) apply_steer = interp(percentage, [-10, 100], [steer_angle, apply_steer]) values = { "COUNTER": raw_cnt, # 0 to 62 - "SETME_X1": 1, - "SETME_X3": 1, # usually 1, but sometimes 3 (??) + "SETME_X1": steering_lta["SETME_X1"], + "SETME_X3": steering_lta["SETME_X3"], # usually 1, but sometimes 3 (??) "PERCENTAGE": percentage, # correlated with driver torque - "SETME_X64": 0x64, # ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1 + "SETME_X64": steering_lta["SETME_X64"], # ramps to 0 smoothly then back on falling edge of STEER_REQUEST if BIT isn't 1 "ANGLE": apply_steer * 1.5, # TODO: need to understand this better, it's always 1.5-2x higher than angle cmd "STEER_ANGLE_CMD": apply_steer, # seems to just be desired angle cmd "STEER_REQUEST": steer_req, # stock system turns off steering after ~20 frames of override, else torque winds up