pan131419/openpilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Release 260111

Browse Source
This commit is contained in:
Comma Device
2026-01-11 18:23:29 +08:00
commit 3721ecbf8a
2601 changed files with 855070 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
opendbc_repo/opendbc/car/toyota/__init__.py Normal file
View File

299
opendbc_repo/opendbc/car/toyota/carcontroller.py Normal file
View File

@@ -0,0 +1,299 @@
import math
import numpy as np
from opendbc.car import Bus, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
from opendbc.car.can_definitions import CanData
from opendbc.car.carlog import carlog
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.common.pid import PIDController
from opendbc.car.secoc import add_mac, build_sync_mac
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.toyota import toyotacan
from opendbc.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR
from opendbc.can import CANPacker
from openpilot.common.params import Params
Ecu = structs.CarParams.Ecu
LongCtrlState = structs.CarControl.Actuators.LongControlState
SteerControlType = structs.CarParams.SteerControlType
VisualAlert = structs.CarControl.HUDControl.VisualAlert
# The up limit allows the brakes/gas to unwind quickly leaving a stop,
# the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup
ACCEL_WINDUP_LIMIT = 4.0 * DT_CTRL * 3 # m/s^2 / frame
ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3 # m/s^2 / frame
ACCEL_PID_UNWIND = 0.03 * DT_CTRL * 3 # m/s^2 / frame
# LKA limits
# EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
MAX_STEER_RATE = 100 # deg/s
MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut
# EPS allows user torque above threshold for 50 frames before permanently faulting
MAX_USER_TORQUE = 500
def get_long_tune(CP, params):
if CP.carFingerprint in TSS2_CAR:
kiBP = [2., 5.]
kiV = [0.5, 0.25]
else:
kiBP = [0., 5., 35.]
kiV = [3.6, 2.4, 1.5]
return PIDController(0.0, (kiBP, kiV), k_f=1.0,
pos_limit=params.ACCEL_MAX, neg_limit=params.ACCEL_MIN,
rate=1 / (DT_CTRL * 3))
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP):
super().__init__(dbc_names, CP)
self.params = CarControllerParams(self.CP)
self.last_torque = 0
self.last_angle = 0
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.permit_braking = True
self.steer_rate_counter = 0
self.distance_button = 0
# *** start long control state ***
self.long_pid = get_long_tune(self.CP, self.params)
self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
self.accel = 0
self.prev_accel = 0
# *** end long control state ***
self.packer = CANPacker(dbc_names[Bus.pt])
self.secoc_lka_message_counter = 0
self.secoc_lta_message_counter = 0
self.secoc_prev_reset_counter = 0
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
stopping = actuators.longControlState == LongCtrlState.stopping
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
if len(CC.orientationNED) == 3:
self.pitch.update(CC.orientationNED[1])
# *** control msgs ***
can_sends = []
params = Params()
steerMax = params.get_int("CustomSteerMax")
steerDeltaUp = params.get_int("CustomSteerDeltaUp")
steerDeltaDown = params.get_int("CustomSteerDeltaDown")
self.params.STEER_MAX = self.params.STEER_MAX if steerMax <= 0 else steerMax
self.params.STEER_DELTA_UP = self.params.STEER_DELTA_UP if steerDeltaUp <= 0 else steerDeltaUp
self.params.STEER_DELTA_DOWN = self.params.STEER_DELTA_DOWN if steerDeltaDown <= 0 else steerDeltaDown
# *** handle secoc reset counter increase ***
if self.CP.flags & ToyotaFlags.SECOC.value:
if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter:
self.secoc_lka_message_counter = 0
self.secoc_lta_message_counter = 0
self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT']
expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']))
if int(CS.secoc_synchronization['AUTHENTICATOR']) != expected_mac:
carlog.error("SecOC synchronization MAC mismatch, wrong key?")
# *** steer torque ***
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
apply_torque = apply_meas_steer_torque_limits(new_torque, self.last_torque, CS.out.steeringTorqueEps, self.params)
# >100 degree/sec steering fault prevention
self.steer_rate_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
if not lat_active:
apply_torque = 0
# *** steer angle ***
if self.CP.steerControlType == SteerControlType.angle:
# If using LTA control, disable LKA and set steering angle command
apply_torque = 0
apply_steer_req = False
if self.frame % 2 == 0:
# EPS uses the torque sensor angle to control with, offset to compensate
apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
# Angular rate limit based on speed
self.last_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw,
CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
CC.latActive, self.params.ANGLE_LIMITS)
self.last_torque = apply_torque
# toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
# on consecutive messages
steer_command = toyotacan.create_steer_command(self.packer, apply_torque, apply_steer_req)
if self.CP.flags & ToyotaFlags.SECOC.value:
# TODO: check if this slow and needs to be done by the CANPacker
steer_command = add_mac(self.secoc_key,
int(CS.secoc_synchronization['TRIP_CNT']),
int(CS.secoc_synchronization['RESET_CNT']),
self.secoc_lka_message_counter,
steer_command)
self.secoc_lka_message_counter += 1
can_sends.append(steer_command)
# STEERING_LTA does not seem to allow more rate by sending faster, and may wind up easier
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
lta_active = lat_active and self.CP.steerControlType == SteerControlType.angle
# cut steering torque with TORQUE_WIND_DOWN when either EPS torque or driver torque is above
# the threshold, to limit max lateral acceleration and for driver torque blending respectively.
full_torque_condition = (abs(CS.out.steeringTorqueEps) < self.params.STEER_MAX and
abs(CS.out.steeringTorque) < self.params.MAX_LTA_DRIVER_TORQUE_ALLOWANCE)
# TORQUE_WIND_DOWN at 0 ramps down torque at roughly the max down rate of 1500 units/sec
torque_wind_down = 100 if lta_active and full_torque_condition else 0
can_sends.append(toyotacan.create_lta_steer_command(self.packer, self.CP.steerControlType, self.last_angle,
lta_active, self.frame // 2, torque_wind_down))
if self.CP.flags & ToyotaFlags.SECOC.value:
lta_steer_2 = toyotacan.create_lta_steer_command_2(self.packer, self.frame // 2)
lta_steer_2 = add_mac(self.secoc_key,
int(CS.secoc_synchronization['TRIP_CNT']),
int(CS.secoc_synchronization['RESET_CNT']),
self.secoc_lta_message_counter,
lta_steer_2)
self.secoc_lta_message_counter += 1
can_sends.append(lta_steer_2)
# *** gas and brake ***
# on entering standstill, send standstill request
if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR):
self.standstill_req = True
if CS.pcm_acc_status != 8:
# pcm entered standstill or it's disabled
self.standstill_req = False
self.last_standstill = CS.out.standstill
# handle UI messages
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
if self.CP.openpilotLongitudinalControl:
if self.frame % 3 == 0:
# Press distance button until we are at the correct bar length. Only change while enabled to avoid skipping startup popup
if self.frame % 6 == 0 and self.CP.openpilotLongitudinalControl:
desired_distance = 4 - hud_control.leadDistanceBars
if CS.out.cruiseState.enabled and CS.pcm_follow_distance != desired_distance:
self.distance_button = not self.distance_button
else:
self.distance_button = 0
# internal PCM gas command can get stuck unwinding from negative accel so we apply a generous rate limit
pcm_accel_cmd = actuators.accel
if CC.longActive:
pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
self.prev_accel = pcm_accel_cmd
# calculate amount of acceleration PCM should apply to reach target, given pitch.
# clipped to only include downhill angles, avoids erroneously unsetting PERMIT_BRAKING when stopping on uphills
accel_due_to_pitch = math.sin(min(self.pitch.x, 0.0)) * ACCELERATION_DUE_TO_GRAVITY
# TODO: on uphills this sometimes sets PERMIT_BRAKING low not considering the creep force
net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
# GVC does not overshoot ego acceleration when starting from stop, but still has a similar delay
if not self.CP.flags & ToyotaFlags.SECOC.value:
a_ego_blended = float(np.interp(CS.out.vEgo, [1.0, 2.0], [CS.gvc, CS.out.aEgo]))
else:
a_ego_blended = CS.out.aEgo
# wind down integral when approaching target for step changes and smooth ramps to reduce overshoot
prev_aego = self.aego.x
self.aego.update(a_ego_blended)
j_ego = (self.aego.x - prev_aego) / (DT_CTRL * 3)
future_t = float(np.interp(CS.out.vEgo, [2., 5.], [0.25, 0.5]))
a_ego_future = a_ego_blended + j_ego * future_t
if CC.longActive:
# constantly slowly unwind integral to recover from large temporary errors
self.long_pid.i -= ACCEL_PID_UNWIND * float(np.sign(self.long_pid.i))
error_future = pcm_accel_cmd - a_ego_future
pcm_accel_cmd = self.long_pid.update(error_future,
speed=CS.out.vEgo,
feedforward=pcm_accel_cmd,
freeze_integrator=actuators.longControlState != LongCtrlState.pid)
else:
self.long_pid.reset()
# Along with rate limiting positive jerk above, this greatly improves gas response time
# Consider the net acceleration request that the PCM should be applying (pitch included)
net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request)
if net_acceleration_request_min < 0.2 or stopping or not CC.longActive:
self.permit_braking = True
elif net_acceleration_request_min > 0.3:
self.permit_braking = False
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
CS.acc_type, fcw_alert, self.distance_button))
self.accel = pcm_accel_cmd
else:
# we can spam can to cancel the system even if we are using lat only control
if pcm_cancel_cmd:
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
else:
can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, True, False, lead, CS.acc_type, False, self.distance_button))
# *** hud ui ***
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
# ui mesg is at 1Hz but we send asap if:
# - there is something to display
# - there is something to stop displaying
send_ui = False
if ((fcw_alert or steer_alert) and not self.alert_active) or \
(not (fcw_alert or steer_alert) and self.alert_active):
send_ui = True
self.alert_active = not self.alert_active
elif pcm_cancel_cmd:
# forcing the pcm to disengage causes a bad fault sound so play a good sound instead
send_ui = True
if self.frame % 20 == 0 or send_ui:
can_sends.append(toyotacan.create_ui_command(self.packer, steer_alert, pcm_cancel_cmd, hud_control.leftLaneVisible,
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, CC.enabled, CS.lkas_hud))
if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))
# *** static msgs ***
for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars:
can_sends.append(CanData(addr, vl, bus))
# keep radar disabled
if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
can_sends.append(make_tester_present_msg(0x750, 0, 0xF))
new_actuators = actuators.as_builder()
new_actuators.torque = apply_torque / self.params.STEER_MAX
new_actuators.torqueOutputCan = apply_torque
new_actuators.steeringAngleDeg = self.last_angle
new_actuators.accel = self.accel
self.frame += 1
return new_actuators, can_sends

204
opendbc_repo/opendbc/car/toyota/carstate.py Normal file
View File

@@ -0,0 +1,204 @@
import copy
import numpy as np
from opendbc.can import CANDefine, CANParser
from opendbc.car import Bus, DT_CTRL, create_button_events, structs
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.interfaces import CarStateBase
from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
ButtonType = structs.CarState.ButtonEvent.Type
SteerControlType = structs.CarParams.SteerControlType
# These steering fault definitions seem to be common across LKA (torque) and LTA (angle):
# - high steer rate fault: goes to 21 or 25 for 1 frame, then 9 for 2 seconds
# - lka/lta msg drop out: goes to 9 then 11 for a combined total of 2 seconds, then 3.
# if using the other control command, goes directly to 3 after 1.5 seconds
# - initializing: LTA can report 0 as long as STEER_TORQUE_SENSOR->STEER_ANGLE_INITIALIZING is 1,
# and is a catch-all for LKA
TEMP_STEER_FAULTS = (0, 9, 11, 21, 25)
# - lka/lta msg drop out: 3 (recoverable)
# - prolonged high driver torque: 17 (permanent)
PERM_STEER_FAULTS = (3, 17)
class CarState(CarStateBase):
def __init__(self, CP):
super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100.
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
self.cluster_min_speed = CV.KPH_TO_MS / 2.
if CP.flags & ToyotaFlags.SECOC.value:
self.shifter_values = can_define.dv["GEAR_PACKET_HYBRID"]["GEAR"]
else:
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
# On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# the signal is zeroed to where the steering angle is at start.
# Need to apply an offset as soon as the steering angle measurements are both received
self.accurate_steer_angle_seen = False
self.angle_offset = FirstOrderFilter(None, 60.0, DT_CTRL, initialized=False)
self.distance_button = 0
self.pcm_follow_distance = 0
self.acc_type = 1
self.lkas_hud = {}
self.gvc = 0.0
self.secoc_synchronization = None
def update(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
ret = structs.CarState()
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
if not self.CP.flags & ToyotaFlags.SECOC.value:
self.gvc = cp.vl["VSC1S07"]["GVC"]
ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
ret.parkingBrake = cp.vl["BODY_CONTROL_STATE"]["PARKING_BRAKE"] == 1
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
ret.brakeHoldActive = cp.vl["ESP_CONTROL"]["BRAKE_HOLD_ACTIVE"] == 1
if self.CP.flags & ToyotaFlags.SECOC.value:
self.secoc_synchronization = copy.copy(cp.vl["SECOC_SYNCHRONIZATION"])
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"]
ret.gasPressed = cp.vl["GAS_PEDAL"]["GAS_PEDAL_USER"] > 0
can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"])
else:
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
if self.CP.carFingerprint != CAR.TOYOTA_MIRAI:
ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
ret.wheelSpeeds = self.get_wheel_speeds(
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
)
ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.vEgoCluster = ret.vEgo * 1.015 # minimum of all the cars
ret.standstill = abs(ret.vEgoRaw) < 1e-3
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
# On some cars, the angle measurement is non-zero while initializing
if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
# Offset seems to be invalid for large steering angles and high angle rates
if abs(ret.steeringAngleDeg) < 90 and abs(ret.steeringRateDeg) < 100 and cp.can_valid:
self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg)
if self.angle_offset.initialized:
ret.steeringAngleOffsetDeg = self.angle_offset.x
ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.leftBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 1
ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale
# we could use the override bit from dbc, but it's triggered at too high torque values
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
# Check EPS LKA/LTA fault status
ret.steerFaultTemporary = cp.vl["EPS_STATUS"]["LKA_STATE"] in TEMP_STEER_FAULTS
ret.steerFaultPermanent = cp.vl["EPS_STATUS"]["LKA_STATE"] in PERM_STEER_FAULTS
if self.CP.steerControlType == SteerControlType.angle:
ret.steerFaultTemporary = ret.steerFaultTemporary or cp.vl["EPS_STATUS"]["LTA_STATE"] in TEMP_STEER_FAULTS
ret.steerFaultPermanent = ret.steerFaultPermanent or cp.vl["EPS_STATUS"]["LTA_STATE"] in PERM_STEER_FAULTS
# Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
# the more accurate angle sensor signal is initialized
ret.vehicleSensorsInvalid = not self.accurate_steer_angle_seen
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
# TODO: find the bit likely in DSU_CRUISE that describes an ACC fault. one may also exist in CLUTCH
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
cluster_set_speed = cp.vl["PCM_CRUISE_ALT"]["UI_SET_SPEED"]
else:
ret.accFaulted = cp.vl["PCM_CRUISE_2"]["ACC_FAULTED"] != 0
ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]["SET_SPEED"] * CV.KPH_TO_MS
cluster_set_speed = cp.vl["PCM_CRUISE_SM"]["UI_SET_SPEED"]
# UI_SET_SPEED is always non-zero when main is on, hide until first enable
if ret.cruiseState.speed != 0:
is_metric = cp.vl["BODY_CONTROL_STATE_2"]["UNITS"] in (1, 2)
conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
# these cars are identified by an ACC_TYPE value of 2.
# TODO: it is possible to avoid the lockout and gain stop and go if you
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
if self.CP.openpilotLongitudinalControl:
ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
# ignore standstill state in certain vehicles, since pcm allows to restart with just an acceleration request
ret.cruiseState.standstill = self.pcm_acc_status == 7
ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"])
ret.cruiseState.nonAdaptive = self.pcm_acc_status in (1, 2, 3, 4, 5, 6)
ret.genericToggle = bool(cp.vl["LIGHT_STALK"]["AUTO_HIGH_BEAM"])
ret.espDisabled = cp.vl["ESP_CONTROL"]["TC_DISABLED"] != 0
if self.CP.enableBsm:
ret.leftBlindspot = (cp.vl["BSM"]["L_ADJACENT"] == 1) or (cp.vl["BSM"]["L_APPROACHING"] == 1)
ret.rightBlindspot = (cp.vl["BSM"]["R_ADJACENT"] == 1) or (cp.vl["BSM"]["R_APPROACHING"] == 1)
if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
if self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR:
self.pcm_follow_distance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
# distance button is wired to the ACC module (camera or radar)
prev_distance_button = self.distance_button
self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
ret.buttonEvents = create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise})
return ret
@staticmethod
def get_can_parsers(CP):
pt_messages = [
("BLINKERS_STATE", float('nan')),
]
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
}

1803
opendbc_repo/opendbc/car/toyota/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

160
opendbc_repo/opendbc/car/toyota/interface.py Normal file
View File

@@ -0,0 +1,160 @@
from opendbc.car import Bus, structs, get_safety_config, uds
from opendbc.car.toyota.carstate import CarState
from opendbc.car.toyota.carcontroller import CarController
from opendbc.car.toyota.radar_interface import RadarInterface
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
ToyotaSafetyFlags
from opendbc.car.disable_ecu import disable_ecu
from opendbc.car.interfaces import CarInterfaceBase
SteerControlType = structs.CarParams.SteerControlType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "toyota"
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.toyota)]
ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate]
# BRAKE_MODULE is on a different address for these cars
if DBC[candidate][Bus.pt] == "toyota_new_mc_pt_generated":
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.ALT_BRAKE.value
if ret.flags & ToyotaFlags.SECOC.value:
ret.secOcRequired = True
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.SECOC.value
if candidate in ANGLE_CONTROL_CAR:
ret.steerControlType = SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LTA.value
# LTA control can be more delayed and winds up more often
ret.steerActuatorDelay = 0.18
ret.steerLimitTimer = 0.8
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4
stop_and_go = candidate in TSS2_CAR
# In TSS2 cars, the camera does long control
found_ecus = [fw.ecu for fw in car_fw]
ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR)
if Ecu.hybrid in found_ecus:
ret.flags |= ToyotaFlags.HYBRID.value
if candidate == CAR.TOYOTA_PRIUS:
stop_and_go = True
# Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
stop_and_go = True
ret.wheelSpeedFactor = 1.035
elif candidate in (CAR.TOYOTA_AVALON, CAR.TOYOTA_AVALON_2019, CAR.TOYOTA_AVALON_TSS2):
# starting from 2019, all Avalon variants have stop and go
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
stop_and_go = candidate != CAR.TOYOTA_AVALON
elif candidate in (CAR.TOYOTA_RAV4_TSS2, CAR.TOYOTA_RAV4_TSS2_2022, CAR.TOYOTA_RAV4_TSS2_2023, CAR.TOYOTA_RAV4_PRIME, CAR.TOYOTA_SIENNA_4TH_GEN):
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP = [0.0]
ret.lateralTuning.pid.kpBP = [0.0]
ret.lateralTuning.pid.kpV = [0.6]
ret.lateralTuning.pid.kiV = [0.1]
ret.lateralTuning.pid.kf = 0.00007818594
# 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary.
# See https://github.com/commaai/openpilot/pull/21429#issuecomment-873652891
for fw in car_fw:
if fw.ecu == "eps" and (fw.fwVersion.startswith(b'\x02') or fw.fwVersion in [b'8965B42181\x00\x00\x00\x00\x00\x00']):
ret.lateralTuning.pid.kpV = [0.15]
ret.lateralTuning.pid.kiV = [0.05]
ret.lateralTuning.pid.kf = 0.00004
break
elif candidate in (CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX):
# TODO: Some of these platforms are not advertised to have full range ACC, are they similar to SNG_WITHOUT_DSU cars?
stop_and_go = True
# TODO: these models can do stop and go, but unclear if it requires sDSU or unplugging DSU.
# For now, don't list stop and go functionality in the docs
if ret.flags & ToyotaFlags.SNG_WITHOUT_DSU:
stop_and_go = stop_and_go or (ret.enableDsu and not docs)
ret.centerToFront = ret.wheelbase * 0.44
# TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction.
# Detect flipped signals and enable for C-HR and others
ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
# No radar dbc for cars without DSU which are not TSS 2.0
# TODO: make an adas dbc file for dsu-less models
ret.radarUnavailable = Bus.radar not in DBC[candidate] or candidate in (NO_DSU_CAR - TSS2_CAR)
# since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle
if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
ret.alphaLongitudinalAvailable = candidate in RADAR_ACC_CAR
# Disabling radar is only supported on TSS2 radar-ACC cars
if alpha_long and candidate in RADAR_ACC_CAR:
ret.flags |= ToyotaFlags.DISABLE_RADAR.value
# openpilot longitudinal enabled by default:
# - cars w/ DSU disconnected
# - TSS2 cars with camera sending ACC_CONTROL where we can block it
# openpilot longitudinal behind experimental long toggle:
# - TSS2 radar ACC cars (disables radar)
if ret.flags & ToyotaFlags.SECOC.value:
ret.openpilotLongitudinalControl = False
else:
ret.openpilotLongitudinalControl = ret.enableDsu or \
candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
if not ret.openpilotLongitudinalControl:
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
# min speed to enable ACC. if car can do stop and go, then set enabling speed
# to a negative value, so it won't matter.
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
if candidate in TSS2_CAR:
ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
# Hybrids have much quicker longitudinal actuator response
if ret.flags & ToyotaFlags.HYBRID.value:
ret.longitudinalActuatorDelay = 0.05
return ret
@staticmethod
def init(CP, can_recv, can_send):
# disable radar if alpha longitudinal toggled on radar-ACC car
if CP.flags & ToyotaFlags.DISABLE_RADAR.value:
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
disable_ecu(can_recv, can_send, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)

93
opendbc_repo/opendbc/car/toyota/radar_interface.py Executable file
View File

@@ -0,0 +1,93 @@
#!/usr/bin/env python3
from opendbc.can import CANParser
from opendbc.car import Bus
from opendbc.car.structs import RadarData
from opendbc.car.toyota.values import DBC, TSS2_CAR
from opendbc.car.interfaces import RadarInterfaceBase
def _create_radar_can_parser(car_fingerprint):
if car_fingerprint in TSS2_CAR:
RADAR_A_MSGS = list(range(0x180, 0x190))
RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
RADAR_A_MSGS = list(range(0x210, 0x220))
RADAR_B_MSGS = list(range(0x220, 0x230))
msg_a_n = len(RADAR_A_MSGS)
msg_b_n = len(RADAR_B_MSGS)
messages = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n), strict=True))
return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.track_id = 0
if CP.carFingerprint in TSS2_CAR:
self.RADAR_A_MSGS = list(range(0x180, 0x190))
self.RADAR_B_MSGS = list(range(0x190, 0x1a0))
else:
self.RADAR_A_MSGS = list(range(0x210, 0x220))
self.RADAR_B_MSGS = list(range(0x220, 0x230))
self.valid_cnt = {key: 0 for key in self.RADAR_A_MSGS}
self.rcp = None if CP.radarUnavailable else _create_radar_can_parser(CP.carFingerprint)
self.trigger_msg = self.RADAR_B_MSGS[-1]
self.updated_messages = set()
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
rr = self._update(self.updated_messages)
self.updated_messages.clear()
return rr
def _update(self, updated_messages):
ret = RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
for ii in sorted(updated_messages):
if ii in self.RADAR_A_MSGS:
cpt = self.rcp.vl[ii]
if cpt['LONG_DIST'] >= 255 or cpt['NEW_TRACK']:
self.valid_cnt[ii] = 0 # reset counter
if cpt['VALID'] and cpt['LONG_DIST'] < 255:
self.valid_cnt[ii] += 1
else:
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
score = self.rcp.vl[ii+16]['SCORE']
# print ii, self.valid_cnt[ii], score, cpt['VALID'], cpt['LONG_DIST'], cpt['LAT_DIST']
# radar point only valid if it's a valid measurement and score is above 50
if cpt['VALID'] or (score > 50 and cpt['LONG_DIST'] < 255 and self.valid_cnt[ii] > 0):
if ii not in self.pts or cpt['NEW_TRACK']:
self.pts[ii] = RadarData.RadarPoint()
self.pts[ii].trackId = self.track_id
self.track_id += 1
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
self.pts[ii].vRel = cpt['REL_SPEED']
self.pts[ii].vLead = self.pts[ii].vRel + self.v_ego
self.pts[ii].aRel = float('nan')
self.pts[ii].yvRel = 0 #float('nan')
self.pts[ii].measured = bool(cpt['VALID'])
else:
if ii in self.pts:
del self.pts[ii]
ret.points = list(self.pts.values())
return ret

0
opendbc_repo/opendbc/car/toyota/tests/__init__.py Normal file
View File

31
opendbc_repo/opendbc/car/toyota/tests/print_platform_codes.py Executable file
View File

@@ -0,0 +1,31 @@
#!/usr/bin/env python3
from collections import defaultdict
from opendbc.car.toyota.values import PLATFORM_CODE_ECUS, get_platform_codes
from opendbc.car.toyota.fingerprints import FW_VERSIONS
if __name__ == "__main__":
parts_for_ecu: dict = defaultdict(set)
cars_for_code: dict = defaultdict(lambda: defaultdict(set))
for car_model, ecus in FW_VERSIONS.items():
print()
print(car_model)
for ecu in sorted(ecus):
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
platform_codes = get_platform_codes(ecus[ecu])
parts_for_ecu[ecu] |= {code.split(b'-')[0] for code in platform_codes if code.count(b'-') > 1}
for code in platform_codes:
cars_for_code[ecu][b'-'.join(code.split(b'-')[:2])] |= {car_model}
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
print(f' Codes: {platform_codes}')
print('\nECU parts:')
for ecu, parts in parts_for_ecu.items():
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}): {parts}')
print('\nCar models vs. platform codes (no major versions):')
for ecu, codes in cars_for_code.items():
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
for code, cars in codes.items():
print(f' {code!r}: {sorted(cars)}')

167
opendbc_repo/opendbc/car/toyota/tests/test_toyota.py Normal file
View File

@@ -0,0 +1,167 @@
from hypothesis import given, settings, strategies as st
from opendbc.car import Bus
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict
from opendbc.car.toyota.fingerprints import FW_VERSIONS
from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \
get_platform_codes
Ecu = CarParams.Ecu
def check_fw_version(fw_version: bytes) -> bool:
# TODO: just use the FW patterns, need to support all chunks
return b'?' not in fw_version and b'!' not in fw_version
class TestToyotaInterfaces:
def test_car_sets(self):
assert len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0
assert len(RADAR_ACC_CAR - TSS2_CAR) == 0
def test_lta_platforms(self):
# At this time, only RAV4 2023 is expected to use LTA/angle control
assert ANGLE_CONTROL_CAR == {CAR.TOYOTA_RAV4_TSS2_2023}
def test_tss2_dbc(self):
# We make some assumptions about TSS2 platforms,
# like looking up certain signals only in this DBC
for car_model, dbc in DBC.items():
if car_model in TSS2_CAR and car_model not in SECOC_CAR:
assert dbc[Bus.pt] == "toyota_nodsu_pt_generated"
def test_essential_ecus(self, subtests):
# Asserts standard ECUs exist for each platform
common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera}
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
present_ecus = {ecu[0] for ecu in ecus}
missing_ecus = common_ecus - present_ecus
assert len(missing_ecus) == 0
# Some exceptions for other common ECUs
if car_model not in (CAR.TOYOTA_ALPHARD_TSS2,):
assert Ecu.abs in present_ecus
if car_model not in (CAR.TOYOTA_MIRAI,):
assert Ecu.engine in present_ecus
if car_model not in (CAR.TOYOTA_PRIUS_V, CAR.LEXUS_CTH):
assert Ecu.eps in present_ecus
class TestToyotaFingerprint:
def test_non_essential_ecus(self, subtests):
# Ensures only the cars that have multiple engine ECUs are in the engine non-essential ECU list
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
engine_ecus = {ecu for ecu in ecus if ecu[0] == Ecu.engine}
assert (len(engine_ecus) > 1) == (car_model in FW_QUERY_CONFIG.non_essential_ecus[Ecu.engine]), \
f"Car model unexpectedly {'not ' if len(engine_ecus) > 1 else ''}in non-essential list"
def test_valid_fw_versions(self, subtests):
# Asserts all FW versions are valid
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for fws in ecus.values():
for fw in fws:
assert check_fw_version(fw), fw
# Tests for part numbers, platform codes, and sub-versions which Toyota will use to fuzzy
# fingerprint in the absence of full FW matches:
@settings(max_examples=100)
@given(data=st.data())
def test_platform_codes_fuzzy_fw(self, data):
fw_strategy = st.lists(st.binary())
fws = data.draw(fw_strategy)
get_platform_codes(fws)
def test_platform_code_ecus_available(self, subtests):
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for platform_code_ecu in PLATFORM_CODE_ECUS:
if platform_code_ecu == Ecu.eps and car_model in (CAR.TOYOTA_PRIUS_V, CAR.LEXUS_CTH,):
continue
if platform_code_ecu == Ecu.abs and car_model in (CAR.TOYOTA_ALPHARD_TSS2,):
continue
assert platform_code_ecu in [e[0] for e in ecus]
def test_fw_format(self, subtests):
# Asserts:
# - every supported ECU FW version returns one platform code
# - every supported ECU FW version has a part number
# - expected parsing of ECU sub-versions
for car_model, ecus in FW_VERSIONS.items():
with subtests.test(car_model=car_model.value):
for ecu, fws in ecus.items():
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
codes = dict()
for fw in fws:
result = get_platform_codes([fw])
# Check only one platform code and sub-version
assert 1 == len(result), f"Unable to parse FW: {fw}"
assert 1 == len(list(result.values())[0]), f"Unable to parse FW: {fw}"
codes |= result
# Toyota places the ECU part number in their FW versions, assert all parsable
# Note that there is only one unique part number per ECU across the fleet, so this
# is not important for identification, just a sanity check.
assert all(code.count(b"-") > 1 for code in codes), f"FW does not have part number: {fw} {codes}"
def test_platform_codes_spot_check(self):
# Asserts basic platform code parsing behavior for a few cases
results = get_platform_codes([
b"F152607140\x00\x00\x00\x00\x00\x00",
b"F152607171\x00\x00\x00\x00\x00\x00",
b"F152607110\x00\x00\x00\x00\x00\x00",
b"F152607180\x00\x00\x00\x00\x00\x00",
])
assert results == {b"F1526-07-1": {b"10", b"40", b"71", b"80"}}
results = get_platform_codes([
b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00",
b"\x028646F4104100\x00\x00\x00\x008646G3304000\x00\x00\x00\x00",
])
assert results == {b"8646F-41-04": {b"100"}}
# Short version has no part number
results = get_platform_codes([
b"\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00",
b"\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00",
])
assert results == {b"58-70": {b"000"}, b"58-83": {b"000"}}
results = get_platform_codes([
b"F152607110\x00\x00\x00\x00\x00\x00",
b"F152607140\x00\x00\x00\x00\x00\x00",
b"\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00",
b"\x0235879000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00",
])
assert results == {b"F1526-07-1": {b"10", b"40"}, b"8646F-41-04": {b"100"}, b"58-79": {b"000"}}
def test_fuzzy_excluded_platforms(self):
# Asserts a list of platforms that will not fuzzy fingerprint with platform codes due to them being shared.
platforms_with_shared_codes = set()
for platform, fw_by_addr in FW_VERSIONS.items():
car_fw = []
for ecu, fw_versions in fw_by_addr.items():
ecu_name, addr, sub_addr = ecu
for fw in fw_versions:
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr,
subAddress=0 if sub_addr is None else sub_addr))
CP = CarParams(carFw=car_fw)
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS)
if len(matches) == 1:
assert list(matches)[0] == platform
else:
# If a platform has multiple matches, add it and its matches
platforms_with_shared_codes |= {str(platform), *matches}
assert platforms_with_shared_codes == FUZZY_EXCLUDED_PLATFORMS, (len(platforms_with_shared_codes), len(FW_VERSIONS))

159
opendbc_repo/opendbc/car/toyota/toyotacan.py Normal file
View File

@@ -0,0 +1,159 @@
from opendbc.car.structs import CarParams
SteerControlType = CarParams.SteerControlType
def create_steer_command(packer, steer, steer_req):
"""Creates a CAN message for the Toyota Steer Command."""
values = {
"STEER_REQUEST": steer_req,
"STEER_TORQUE_CMD": steer,
"SET_ME_1": 1,
}
return packer.make_can_msg("STEERING_LKA", 0, values)
def create_lta_steer_command(packer, steer_control_type, steer_angle, steer_req, frame, torque_wind_down):
"""Creates a CAN message for the Toyota LTA Steer Command."""
values = {
"COUNTER": frame + 128,
"SETME_X1": 1, # suspected LTA feature availability
# 1 for TSS 2.5 cars, 3 for TSS 2.0. Send based on whether we're using LTA for lateral control
"SETME_X3": 1 if steer_control_type == SteerControlType.angle else 3,
"PERCENTAGE": 100,
"TORQUE_WIND_DOWN": torque_wind_down,
"ANGLE": 0,
"STEER_ANGLE_CMD": steer_angle,
"STEER_REQUEST": steer_req,
"STEER_REQUEST_2": steer_req,
"CLEAR_HOLD_STEERING_ALERT": 0,
}
return packer.make_can_msg("STEERING_LTA", 0, values)
def create_lta_steer_command_2(packer, frame):
values = {
"COUNTER": frame + 128,
}
return packer.make_can_msg("STEERING_LTA_2", 0, values)
def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_req, lead, acc_type, fcw_alert, distance):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
"ACC_TYPE": acc_type,
"DISTANCE": distance,
"MINI_CAR": lead,
"PERMIT_BRAKING": permit_braking,
"RELEASE_STANDSTILL": not standstill_req,
"CANCEL_REQ": pcm_cancel,
"ALLOW_LONG_PRESS": 2, # 1,
"ACC_CUT_IN": fcw_alert, # only shown when ACC enabled
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
def create_pcs_commands(packer, accel, active, mass):
values1 = {
"COUNTER": 0,
"FORCE": round(min(accel, 0) * mass * 2),
"STATE": 3 if active else 0,
"BRAKE_STATUS": 0,
"PRECOLLISION_ACTIVE": 1 if active else 0,
}
msg1 = packer.make_can_msg("PRE_COLLISION", 0, values1)
values2 = {
"DSS1GDRV": min(accel, 0), # accel
"PCSALM": 1 if active else 0, # goes high same time as PRECOLLISION_ACTIVE
"IBTRGR": 1 if active else 0, # unknown
"PBATRGR": 1 if active else 0, # noisy actuation bit?
"PREFILL": 1 if active else 0, # goes on and off before DSS1GDRV
"AVSTRGR": 1 if active else 0,
}
msg2 = packer.make_can_msg("PRE_COLLISION_2", 0, values2)
return [msg1, msg2]
def create_acc_cancel_command(packer):
values = {
"GAS_RELEASED": 0,
"CRUISE_ACTIVE": 0,
"ACC_BRAKING": 0,
"ACCEL_NET": 0,
"CRUISE_STATE": 0,
"CANCEL_REQ": 1,
}
return packer.make_can_msg("PCM_CRUISE", 0, values)
def create_fcw_command(packer, fcw):
values = {
"PCS_INDICATOR": 1, # PCS turned off
"FCW": fcw,
"SET_ME_X20": 0x20,
"SET_ME_X10": 0x10,
"PCS_OFF": 1,
"PCS_SENSITIVITY": 0,
}
return packer.make_can_msg("PCS_HUD", 0, values)
def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled, stock_lkas_hud):
values = {
"TWO_BEEPS": chime,
"LDA_ALERT": steer,
"RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2,
"LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2,
"BARRIERS": 1 if enabled else 0,
# static signals
"SET_ME_X02": 2,
"SET_ME_X01": 1,
"LKAS_STATUS": 1,
"REPEATED_BEEPS": 0,
"LANE_SWAY_FLD": 7,
"LANE_SWAY_BUZZER": 0,
"LANE_SWAY_WARNING": 0,
"LDA_FRONT_CAMERA_BLOCKED": 0,
"TAKE_CONTROL": 0,
"LANE_SWAY_SENSITIVITY": 2,
"LANE_SWAY_TOGGLE": 1,
"LDA_ON_MESSAGE": 0,
"LDA_MESSAGES": 0,
"LDA_SA_TOGGLE": 1,
"LDA_SENSITIVITY": 2,
"LDA_UNAVAILABLE": 0,
"LDA_MALFUNCTION": 0,
"LDA_UNAVAILABLE_QUIET": 0,
"ADJUSTING_CAMERA": 0,
"LDW_EXIST": 1,
}
# lane sway functionality
# not all cars have LKAS_HUD — update with camera values if available
if len(stock_lkas_hud):
values.update({s: stock_lkas_hud[s] for s in [
"LANE_SWAY_FLD",
"LANE_SWAY_BUZZER",
"LANE_SWAY_WARNING",
"LANE_SWAY_SENSITIVITY",
"LANE_SWAY_TOGGLE",
]})
return packer.make_can_msg("LKAS_HUD", 0, values)
def toyota_checksum(address: int, sig, d: bytearray) -> int:
s = len(d)
addr = address
while addr:
s += addr & 0xFF
addr >>= 8
for i in range(len(d) - 1):
s += d[i]
return s & 0xFF

632
opendbc_repo/opendbc/car/toyota/values.py Normal file
View File

@@ -0,0 +1,632 @@
import re
from collections import defaultdict
from dataclasses import dataclass, field
from enum import Enum, IntFlag
from opendbc.car import Bus, CarSpecs, PlatformConfig, Platforms, AngleSteeringLimits
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarDocs, Column, CarParts, CarHarness
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
class CarControllerParams:
STEER_STEP = 1
STEER_MAX = 1500
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
# Lane Tracing Assist (LTA) control limits
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
# EPS ignores commands above this angle and causes PCS to fault
94.9461, # deg
# Assuming a steering ratio of 13.7:
# Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
# Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
# however the EPS has its own internal limits at all speeds which are less than that:
# Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA
([5, 25], [0.3, 0.15]),
([5, 25], [0.36, 0.26]),
)
MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes
def __init__(self, CP):
if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
self.ACCEL_MAX = 2.0
else:
self.ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s^2 for tuning reasons
self.ACCEL_MIN = -3.5 # m/s2
if CP.lateralTuning.which() == 'torque':
self.STEER_DELTA_UP = 15 # 1.0s time to peak torque
self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
else:
self.STEER_DELTA_UP = 10 # 1.5s time to peak torque
self.STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
class ToyotaSafetyFlags(IntFlag):
# first byte is for EPS scaling factor
ALT_BRAKE = (1 << 8)
STOCK_LONGITUDINAL = (2 << 8)
LTA = (4 << 8)
SECOC = (8 << 8)
class ToyotaFlags(IntFlag):
# Detected flags
HYBRID = 1
DISABLE_RADAR = 4
# Static flags
TSS2 = 8
NO_DSU = 16
UNSUPPORTED_DSU = 32
RADAR_ACC = 64
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL = 128
NO_STOP_TIMER = 256
# these cars are speculated to allow stop and go when the DSU is unplugged
SNG_WITHOUT_DSU = 512
# these cars can utilize 2.0 m/s^2
RAISED_ACCEL_LIMIT = 1024
SECOC = 2048
class Footnote(Enum):
CAMRY = CarFootnote(
"openpilot operates above 28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.",
Column.FSR_LONGITUDINAL)
@dataclass
class ToyotaCarDocs(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota_a]))
def dbc_dict(pt, radar):
return {Bus.pt: pt, Bus.radar: radar}
@dataclass
class ToyotaTSS2PlatformConfig(PlatformConfig):
dbc_dict: dict = field(default_factory=lambda: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'))
def init(self):
self.flags |= ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU
if self.flags & ToyotaFlags.RADAR_ACC:
self.dbc_dict = {Bus.pt: 'toyota_nodsu_pt_generated'}
@dataclass
class ToyotaSecOCPlatformConfig(PlatformConfig):
dbc_dict: dict = field(default_factory=lambda: dbc_dict('toyota_secoc_pt_generated', 'toyota_tss2_adas'))
def init(self):
# don't expose car docs until SecOC cars can be suppressed from the comma website
self.car_docs = []
self.flags |= ToyotaFlags.TSS2 | ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.NO_DSU | ToyotaFlags.SECOC
if self.flags & ToyotaFlags.RADAR_ACC:
self.dbc_dict = {Bus.pt: 'toyota_secoc_pt_generated'}
class CAR(Platforms):
# Toyota
TOYOTA_ALPHARD_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota Alphard 2019-20"),
ToyotaCarDocs("Toyota Alphard Hybrid 2021"),
],
CarSpecs(mass=4305. * CV.LB_TO_KG, wheelbase=3.0, steerRatio=14.2, tireStiffnessFactor=0.444),
)
TOYOTA_AVALON = PlatformConfig(
[
ToyotaCarDocs("Toyota Avalon 2016", "Toyota Safety Sense P"),
ToyotaCarDocs("Toyota Avalon 2017-18"),
],
CarSpecs(mass=3505. * CV.LB_TO_KG, wheelbase=2.82, steerRatio=14.8, tireStiffnessFactor=0.7983),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
)
TOYOTA_AVALON_2019 = PlatformConfig(
[
ToyotaCarDocs("Toyota Avalon 2019-21"),
ToyotaCarDocs("Toyota Avalon Hybrid 2019-21"),
],
TOYOTA_AVALON.specs,
dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
)
TOYOTA_AVALON_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5
[
ToyotaCarDocs("Toyota Avalon 2022"),
ToyotaCarDocs("Toyota Avalon Hybrid 2022"),
],
TOYOTA_AVALON.specs,
)
TOYOTA_CAMRY = PlatformConfig(
[
ToyotaCarDocs("Toyota Camry 2018-20", video="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
ToyotaCarDocs("Toyota Camry Hybrid 2018-20", video="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
],
CarSpecs(mass=3400. * CV.LB_TO_KG, wheelbase=2.82448, steerRatio=13.7, tireStiffnessFactor=0.7933),
dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.NO_DSU,
)
TOYOTA_CAMRY_TSS2 = ToyotaTSS2PlatformConfig( # TSS 2.5
[
ToyotaCarDocs("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]),
ToyotaCarDocs("Toyota Camry Hybrid 2021-24"),
],
TOYOTA_CAMRY.specs,
)
TOYOTA_CHR = PlatformConfig(
[
ToyotaCarDocs("Toyota C-HR 2017-20"),
ToyotaCarDocs("Toyota C-HR Hybrid 2017-20"),
],
CarSpecs(mass=3300. * CV.LB_TO_KG, wheelbase=2.63906, steerRatio=13.6, tireStiffnessFactor=0.7933),
dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.NO_DSU,
)
TOYOTA_CHR_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota C-HR 2021"),
ToyotaCarDocs("Toyota C-HR Hybrid 2021-22"),
],
TOYOTA_CHR.specs,
flags=ToyotaFlags.RADAR_ACC,
)
TOYOTA_COROLLA = PlatformConfig(
[ToyotaCarDocs("Toyota Corolla 2017-19")],
CarSpecs(mass=2860. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=18.27, tireStiffnessFactor=0.444),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
)
# LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
TOYOTA_COROLLA_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota Corolla 2020-22", video="https://www.youtube.com/watch?v=_66pXk0CBYA"),
ToyotaCarDocs("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5),
ToyotaCarDocs("Toyota Corolla Hatchback 2019-22", video="https://www.youtube.com/watch?v=_66pXk0CBYA"),
# Hybrid platforms
ToyotaCarDocs("Toyota Corolla Hybrid 2020-22"),
ToyotaCarDocs("Toyota Corolla Hybrid (South America only) 2020-23", min_enable_speed=7.5),
ToyotaCarDocs("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5),
ToyotaCarDocs("Lexus UX Hybrid 2019-24"),
],
CarSpecs(mass=3060. * CV.LB_TO_KG, wheelbase=2.67, steerRatio=13.9, tireStiffnessFactor=0.444),
)
TOYOTA_HIGHLANDER = PlatformConfig(
[
ToyotaCarDocs("Toyota Highlander 2017-19", video="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
ToyotaCarDocs("Toyota Highlander Hybrid 2017-19"),
],
CarSpecs(mass=4516. * CV.LB_TO_KG, wheelbase=2.8194, steerRatio=16.0, tireStiffnessFactor=0.8),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU,
)
TOYOTA_HIGHLANDER_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota Highlander 2020-23"),
ToyotaCarDocs("Toyota Highlander Hybrid 2020-23"),
],
TOYOTA_HIGHLANDER.specs,
)
TOYOTA_PRIUS = PlatformConfig(
[
ToyotaCarDocs("Toyota Prius 2016", "Toyota Safety Sense P", video="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
ToyotaCarDocs("Toyota Prius 2017-20", video="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
ToyotaCarDocs("Toyota Prius Prime 2017-20", video="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
],
CarSpecs(mass=3045. * CV.LB_TO_KG, wheelbase=2.7, steerRatio=15.74, tireStiffnessFactor=0.6371),
dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
)
TOYOTA_PRIUS_V = PlatformConfig(
[ToyotaCarDocs("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=3340. * CV.LB_TO_KG, wheelbase=2.78, steerRatio=17.4, tireStiffnessFactor=0.5533),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU,
)
TOYOTA_PRIUS_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota Prius 2021-22", video="https://www.youtube.com/watch?v=J58TvCpUd4U"),
ToyotaCarDocs("Toyota Prius Prime 2021-22", video="https://www.youtube.com/watch?v=J58TvCpUd4U"),
],
CarSpecs(mass=3115. * CV.LB_TO_KG, wheelbase=2.70002, steerRatio=13.4, tireStiffnessFactor=0.6371),
)
TOYOTA_RAV4 = PlatformConfig(
[
ToyotaCarDocs("Toyota RAV4 2016", "Toyota Safety Sense P"),
ToyotaCarDocs("Toyota RAV4 2017-18")
],
CarSpecs(mass=3650. * CV.LB_TO_KG, wheelbase=2.65, steerRatio=16.88, tireStiffnessFactor=0.5533),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
)
TOYOTA_RAV4H = PlatformConfig(
[
ToyotaCarDocs("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video="https://youtu.be/LhT5VzJVfNI?t=26"),
ToyotaCarDocs("Toyota RAV4 Hybrid 2017-18", video="https://youtu.be/LhT5VzJVfNI?t=26")
],
TOYOTA_RAV4.specs,
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
# Note that the ICE RAV4 does not respect positive acceleration commands under 19 mph
flags=ToyotaFlags.NO_STOP_TIMER | ToyotaFlags.SNG_WITHOUT_DSU,
)
TOYOTA_RAV4_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota RAV4 2019-21", video="https://www.youtube.com/watch?v=wJxjDd42gGA"),
ToyotaCarDocs("Toyota RAV4 Hybrid 2019-21"),
],
CarSpecs(mass=3585. * CV.LB_TO_KG, wheelbase=2.68986, steerRatio=14.3, tireStiffnessFactor=0.7933),
)
TOYOTA_RAV4_TSS2_2022 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota RAV4 2022"),
ToyotaCarDocs("Toyota RAV4 Hybrid 2022", video="https://youtu.be/U0nH9cnrFB0"),
],
TOYOTA_RAV4_TSS2.specs,
flags=ToyotaFlags.RADAR_ACC,
)
TOYOTA_RAV4_TSS2_2023 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Toyota RAV4 2023-25"),
ToyotaCarDocs("Toyota RAV4 Hybrid 2023-25", video="https://youtu.be/4eIsEq4L4Ng"),
],
TOYOTA_RAV4_TSS2.specs,
flags=ToyotaFlags.RADAR_ACC | ToyotaFlags.ANGLE_CONTROL,
)
TOYOTA_RAV4_PRIME = ToyotaSecOCPlatformConfig(
[ToyotaCarDocs("Toyota RAV4 Prime 2021-23", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=4372. * CV.LB_TO_KG, wheelbase=2.68, steerRatio=16.88, tireStiffnessFactor=0.5533),
)
TOYOTA_YARIS = ToyotaSecOCPlatformConfig(
[ToyotaCarDocs("Toyota Yaris 2023 (Non-US only)", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=1170, wheelbase=2.55, steerRatio=14.80, tireStiffnessFactor=0.5533),
flags=ToyotaFlags.RADAR_ACC,
)
TOYOTA_MIRAI = ToyotaTSS2PlatformConfig( # TSS 2.5
[ToyotaCarDocs("Toyota Mirai 2021")],
CarSpecs(mass=4300. * CV.LB_TO_KG, wheelbase=2.91, steerRatio=14.8, tireStiffnessFactor=0.8),
)
TOYOTA_SIENNA = PlatformConfig(
[ToyotaCarDocs("Toyota Sienna 2018-20", video="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=4590. * CV.LB_TO_KG, wheelbase=3.03, steerRatio=15.5, tireStiffnessFactor=0.444),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.NO_STOP_TIMER,
)
TOYOTA_SIENNA_4TH_GEN = ToyotaSecOCPlatformConfig(
[ToyotaCarDocs("Toyota Sienna 2021-23", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=4625. * CV.LB_TO_KG, wheelbase=3.06, steerRatio=17.8, tireStiffnessFactor=0.444),
)
# Lexus
LEXUS_CTH = PlatformConfig(
[ToyotaCarDocs("Lexus CT Hybrid 2017-18", "Lexus Safety System+")],
CarSpecs(mass=3108. * CV.LB_TO_KG, wheelbase=2.6, steerRatio=18.6, tireStiffnessFactor=0.517),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
)
LEXUS_ES = PlatformConfig(
[
ToyotaCarDocs("Lexus ES 2017-18"),
ToyotaCarDocs("Lexus ES Hybrid 2017-18"),
],
CarSpecs(mass=3677. * CV.LB_TO_KG, wheelbase=2.8702, steerRatio=16.0, tireStiffnessFactor=0.444),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
)
LEXUS_ES_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Lexus ES 2019-24"),
ToyotaCarDocs("Lexus ES Hybrid 2019-25", video="https://youtu.be/BZ29osRVJeg?t=12"),
],
LEXUS_ES.specs,
)
LEXUS_IS = PlatformConfig(
[ToyotaCarDocs("Lexus IS 2017-19")],
CarSpecs(mass=3736.8 * CV.LB_TO_KG, wheelbase=2.79908, steerRatio=13.3, tireStiffnessFactor=0.444),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.UNSUPPORTED_DSU,
)
LEXUS_IS_TSS2 = ToyotaTSS2PlatformConfig(
[ToyotaCarDocs("Lexus IS 2022-23")],
LEXUS_IS.specs,
)
LEXUS_NX = PlatformConfig(
[
ToyotaCarDocs("Lexus NX 2018-19"),
ToyotaCarDocs("Lexus NX Hybrid 2018-19"),
],
CarSpecs(mass=4070. * CV.LB_TO_KG, wheelbase=2.66, steerRatio=14.7, tireStiffnessFactor=0.444),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
)
LEXUS_NX_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Lexus NX 2020-21"),
ToyotaCarDocs("Lexus NX Hybrid 2020-21"),
],
LEXUS_NX.specs,
)
LEXUS_LC_TSS2 = ToyotaTSS2PlatformConfig(
[ToyotaCarDocs("Lexus LC 2024")],
CarSpecs(mass=4500. * CV.LB_TO_KG, wheelbase=2.87, steerRatio=13.0, tireStiffnessFactor=0.444),
)
LEXUS_RC = PlatformConfig(
[ToyotaCarDocs("Lexus RC 2018-20")],
LEXUS_IS.specs,
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.UNSUPPORTED_DSU,
)
LEXUS_RX = PlatformConfig(
[
ToyotaCarDocs("Lexus RX 2016", "Lexus Safety System+"),
ToyotaCarDocs("Lexus RX 2017-19"),
# Hybrid platforms
ToyotaCarDocs("Lexus RX Hybrid 2016", "Lexus Safety System+"),
ToyotaCarDocs("Lexus RX Hybrid 2017-19"),
],
CarSpecs(mass=4481. * CV.LB_TO_KG, wheelbase=2.79, steerRatio=16., tireStiffnessFactor=0.5533),
dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
)
LEXUS_RX_TSS2 = ToyotaTSS2PlatformConfig(
[
ToyotaCarDocs("Lexus RX 2020-22"),
ToyotaCarDocs("Lexus RX Hybrid 2020-22"),
],
LEXUS_RX.specs,
)
LEXUS_GS_F = PlatformConfig(
[ToyotaCarDocs("Lexus GS F 2016")],
CarSpecs(mass=4034. * CV.LB_TO_KG, wheelbase=2.84988, steerRatio=13.3, tireStiffnessFactor=0.444),
dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
flags=ToyotaFlags.UNSUPPORTED_DSU,
)
# (addr, cars, bus, 1/freq*100, vl)
STATIC_DSU_MSGS = [
(0x128, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON), \
1, 3, b'\xf4\x01\x90\x83\x00\x37'),
(0x128, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
(0x141, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON,
CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 2, b'\x00\x00\x00\x46'),
(0x160, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON,
CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_PRIUS_V),
1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
(0X161, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
(0x283, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON,
CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
(0x2E6, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
(0x2E7, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
(0x33E, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
(0x344, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON,
CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x365, (CAR.TOYOTA_PRIUS, CAR.LEXUS_NX, CAR.TOYOTA_HIGHLANDER), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
(0x365, (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_RX,
CAR.TOYOTA_PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_HIGHLANDER), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
(0x366, (CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_AVALON, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V),
0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
(0x470, (CAR.TOYOTA_PRIUS, CAR.LEXUS_RX), 1, 100, b'\x00\x00\x02\x7a'),
(0x470, (CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_RAV4H, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 1, 100, b'\x00\x00\x01\x79'),
(0x4CB, (CAR.TOYOTA_PRIUS, CAR.TOYOTA_RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_AVALON,
CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.TOYOTA_PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
]
def get_platform_codes(fw_versions: list[bytes]) -> dict[bytes, set[bytes]]:
# Returns sub versions in a dict so comparisons can be made within part-platform-major_version combos
codes = defaultdict(set) # Optional[part]-platform-major_version: set of sub_version
for fw in fw_versions:
# FW versions returned from UDS queries can return multiple fields/chunks of data (different ECU calibrations, different data?)
# and are prefixed with a byte that describes how many chunks of data there are.
# But FW returned from KWP requires querying of each sub-data id and does not have a length prefix.
length_code = 1
length_code_match = FW_LEN_CODE.search(fw)
if length_code_match is not None:
length_code = length_code_match.group()[0]
fw = fw[1:]
# fw length should be multiple of 16 bytes (per chunk, even if no length code), skip parsing if unexpected length
if length_code * FW_CHUNK_LEN != len(fw):
continue
chunks = [fw[FW_CHUNK_LEN * i:FW_CHUNK_LEN * i + FW_CHUNK_LEN].strip(b'\x00 ') for i in range(length_code)]
# only first is considered for now since second is commonly shared (TODO: understand that)
first_chunk = chunks[0]
if len(first_chunk) == 8:
# TODO: no part number, but some short chunks have it in subsequent chunks
fw_match = SHORT_FW_PATTERN.search(first_chunk)
if fw_match is not None:
platform, major_version, sub_version = fw_match.groups()
codes[b'-'.join((platform, major_version))].add(sub_version)
elif len(first_chunk) == 10:
fw_match = MEDIUM_FW_PATTERN.search(first_chunk)
if fw_match is not None:
part, platform, major_version, sub_version = fw_match.groups()
codes[b'-'.join((part, platform, major_version))].add(sub_version)
elif len(first_chunk) == 12:
fw_match = LONG_FW_PATTERN.search(first_chunk)
if fw_match is not None:
part, platform, major_version, sub_version = fw_match.groups()
codes[b'-'.join((part, platform, major_version))].add(sub_version)
return dict(codes)
def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
candidates = set()
for candidate, fws in offline_fw_versions.items():
# Keep track of ECUs which pass all checks (platform codes, within sub-version range)
valid_found_ecus = set()
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
for ecu, expected_versions in fws.items():
addr = ecu[1:]
# Only check ECUs expected to have platform codes
if ecu[0] not in PLATFORM_CODE_ECUS:
continue
# Expected platform codes & versions
expected_platform_codes = get_platform_codes(expected_versions)
# Found platform codes & versions
found_platform_codes = get_platform_codes(live_fw_versions.get(addr, set()))
# Check part number + platform code + major version matches for any found versions
# Platform codes and major versions change for different physical parts, generation, API, etc.
# Sub-versions are incremented for minor recalls, do not need to be checked.
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
break
valid_found_ecus.add(addr)
# If all live ECUs pass all checks for candidate, add it as a match
if valid_expected_ecus.issubset(valid_found_ecus):
candidates.add(candidate)
return {str(c) for c in (candidates - FUZZY_EXCLUDED_PLATFORMS)}
# Regex patterns for parsing more general platform-specific identifiers from FW versions.
# - Part number: Toyota part number (usually last character needs to be ignored to find a match).
# Each ECU address has just one part number.
# - Platform: usually multiple codes per an openpilot platform, however this is the least variable and
# is usually shared across ECUs and model years signifying this describes something about the specific platform.
# This describes more generational changes (TSS-P vs TSS2), or manufacture region.
# - Major version: second least variable part of the FW version. Seen splitting cars by model year/API such as
# RAV4 2022/2023 and Avalon. Used to differentiate cars where API has changed slightly, but is not a generational change.
# It is important to note that these aren't always consecutive, for example:
# Avalon 2016-18's fwdCamera has these major versions: 01, 03 while 2019 has: 02
# - Sub version: exclusive to major version, but shared with other cars. Should only be used for further filtering.
# Seen bumped in TSB FW updates, and describes other minor differences.
SHORT_FW_PATTERN = re.compile(b'[A-Z0-9](?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{2})(?P<sub_version>[A-Z0-9]{3})')
MEDIUM_FW_PATTERN = re.compile(b'(?P<part>[A-Z0-9]{5})(?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{1})(?P<sub_version>[A-Z0-9]{2})')
LONG_FW_PATTERN = re.compile(b'(?P<part>[A-Z0-9]{5})(?P<platform>[A-Z0-9]{2})(?P<major_version>[A-Z0-9]{2})(?P<sub_version>[A-Z0-9]{3})')
FW_LEN_CODE = re.compile(b'^[\x01-\x03]') # highest seen is 3 chunks, 16 bytes each
FW_CHUNK_LEN = 16
# List of ECUs that are most unique across openpilot platforms
# - fwdCamera: describes actual features related to ADAS. For example, on the Avalon it describes
# when TSS-P became standard, whether the car supports stop and go, and whether it's TSS2.
# On the RAV4, it describes the move to the radar doing ACC, and the use of LTA for lane keeping.
# Note that the platform codes & major versions do not describe features in plain text, only with
# matching against other seen FW versions in the database they can describe features.
# - fwdRadar: sanity check against fwdCamera, commonly shares a platform code.
# For example the RAV4 2022's new radar architecture is shown for both with platform code.
# - abs: differentiates hybrid/ICE on most cars (Corolla TSS2 is an exception, not used due to hybrid platform combination)
# - eps: describes lateral API changes for the EPS, such as using LTA for lane keeping and rejecting LKA messages
PLATFORM_CODE_ECUS = (Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps)
# These platforms have at least one platform code for all ECUs shared with another platform.
FUZZY_EXCLUDED_PLATFORMS: set[CAR] = set()
# Some ECUs that use KWP2000 have their FW versions on non-standard data identifiers.
# Toyota diagnostic software first gets the supported data ids, then queries them one by one.
# For example, sends: 0x1a8800, receives: 0x1a8800010203, queries: 0x1a8801, 0x1a8802, 0x1a8803
TOYOTA_VERSION_REQUEST_KWP = b'\x1a\x88\x01'
TOYOTA_VERSION_RESPONSE_KWP = b'\x5a\x88\x01'
FW_QUERY_CONFIG = FwQueryConfig(
# TODO: look at data to whitelist new ECUs effectively
requests=[
Request(
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, TOYOTA_VERSION_REQUEST_KWP],
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, TOYOTA_VERSION_RESPONSE_KWP],
whitelist_ecus=[Ecu.fwdCamera, Ecu.fwdRadar, Ecu.dsu, Ecu.abs, Ecu.eps, Ecu.srs, Ecu.transmission, Ecu.hvac],
bus=0,
),
Request(
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, StdQueries.OBD_VERSION_REQUEST],
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, StdQueries.OBD_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.hybrid, Ecu.srs, Ecu.transmission, Ecu.hvac],
bus=0,
),
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.DEFAULT_DIAGNOSTIC_REQUEST, StdQueries.EXTENDED_DIAGNOSTIC_REQUEST, StdQueries.UDS_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.DEFAULT_DIAGNOSTIC_RESPONSE, StdQueries.EXTENDED_DIAGNOSTIC_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.abs, Ecu.eps,
Ecu.hybrid, Ecu.srs, Ecu.transmission, Ecu.hvac],
bus=0,
),
],
non_essential_ecus={
# FIXME: On some models, abs can sometimes be missing
Ecu.abs: [CAR.TOYOTA_RAV4, CAR.TOYOTA_COROLLA, CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_SIENNA, CAR.LEXUS_IS, CAR.TOYOTA_ALPHARD_TSS2],
# On some models, the engine can show on two different addresses
Ecu.engine: [CAR.TOYOTA_HIGHLANDER, CAR.TOYOTA_CAMRY, CAR.TOYOTA_COROLLA_TSS2, CAR.TOYOTA_CHR, CAR.TOYOTA_CHR_TSS2, CAR.LEXUS_IS,
CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2],
},
extra_ecus=[
# All known ECUs on a late-model Toyota vehicle not queried here:
# Responds to UDS:
# - Combination Meter (0x7c0)
# - HV Battery (0x713, 0x747)
# - Motor Generator (0x716, 0x724)
# - 2nd ABS "Brake/EPB" (0x730)
# - Electronic Parking Brake ((0x750, 0x2c))
# - Telematics ((0x750, 0xc7))
# Responds to KWP (0x1a8801):
# - Steering Angle Sensor (0x7b3)
# - EPS/EMPS (0x7a0, 0x7a1)
# - 2nd SRS Airbag (0x784)
# - Central Gateway ((0x750, 0x5f))
# - Telematics ((0x750, 0xc7))
# Responds to KWP (0x1a8881):
# - Body Control Module ((0x750, 0x40))
# - Telematics ((0x750, 0xc7))
# Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform
(Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer
(Ecu.hybrid, 0x7d2, None), # Hybrid Control Assembly & Computer
(Ecu.srs, 0x780, None), # SRS Airbag
# Transmission is combined with engine on some platforms, such as TSS-P RAV4
(Ecu.transmission, 0x701, None),
# A few platforms have a tester present response on this address, add to log
(Ecu.transmission, 0x7e1, None),
(Ecu.hvac, 0x7c4, None),
],
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
STEER_THRESHOLD = 100
# These cars have non-standard EPS torque scale factors. All others are 73
EPS_SCALE = defaultdict(lambda: 73,
{CAR.TOYOTA_PRIUS: 66, CAR.TOYOTA_COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.TOYOTA_PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = CAR.with_flags(ToyotaFlags.TSS2)
NO_DSU_CAR = CAR.with_flags(ToyotaFlags.NO_DSU)
# the DSU uses the AEB message for longitudinal on these cars
UNSUPPORTED_DSU_CAR = CAR.with_flags(ToyotaFlags.UNSUPPORTED_DSU)
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = CAR.with_flags(ToyotaFlags.RADAR_ACC)
ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL)
SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC)
# no resume button press required
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
DBC = CAR.create_dbc_map()
if __name__ == "__main__":
cars = []
for platform in CAR:
for doc in platform.config.car_docs:
cars.append(doc.name)
cars.sort()
for c in cars:
print(c)