brianhansonxyz/oscarpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

openpilot v0.9.6 release

Browse Source 
date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
FrogAi
2024-01-12 22:39:28 -07:00
commit 08e9fb1edc
1881 changed files with 653708 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
selfdrive/car/toyota/__init__.py Normal file
View File

195
selfdrive/car/toyota/carcontroller.py Normal file
View File

@@ -0,0 +1,195 @@
from cereal import car
from openpilot.common.numpy_fast import clip, interp
from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
create_gas_interceptor_command, make_can_msg
from openpilot.selfdrive.car.toyota import toyotacan
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR
from opendbc.can.packer import CANPacker
SteerControlType = car.CarParams.SteerControlType
VisualAlert = car.CarControl.HUDControl.VisualAlert
# 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
# LTA limits
# EPS ignores commands above this angle and causes PCS to fault
MAX_LTA_ANGLE = 94.9461 # deg
MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes
class CarController:
def __init__(self, dbc_name, CP, VM):
self.CP = CP
self.params = CarControllerParams(self.CP)
self.frame = 0
self.last_steer = 0
self.last_angle = 0
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.steer_rate_counter = 0
self.packer = CANPacker(dbc_name)
self.gas = 0
self.accel = 0
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
pcm_cancel_cmd = CC.cruiseControl.cancel
lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE
# *** control msgs ***
can_sends = []
# *** steer torque ***
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, 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, CC.latActive,
self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
if not CC.latActive:
apply_steer = 0
# *** steer angle ***
if self.CP.steerControlType == SteerControlType.angle:
# If using LTA control, disable LKA and set steering angle command
apply_steer = 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
apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw, self.params)
if not lat_active:
apply_angle = CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg
self.last_angle = clip(apply_angle, -MAX_LTA_ANGLE, MAX_LTA_ANGLE)
self.last_steer = apply_steer
# 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
can_sends.append(toyotacan.create_steer_command(self.packer, apply_steer, apply_steer_req))
# 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) < 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))
# *** gas and brake ***
if self.CP.enableGasInterceptor and CC.longActive:
MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal
if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif self.CP.carFingerprint in (CAR.COROLLA,):
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
# offset for creep and windbrake
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
else:
interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
# TODO: probably can delete this. CS.pcm_acc_status uses a different signal
# than CS.cruiseState.enabled. confirm they're not meaningfully different
if not CC.enabled and CS.pcm_acc_status:
pcm_cancel_cmd = 1
# 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 or self.CP.enableGasInterceptor):
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)
# we can spam can to cancel the system even if we are using lat only control
if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
# Lexus IS uses a different cancellation message
if pcm_cancel_cmd and self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
can_sends.append(toyotacan.create_acc_cancel_command(self.packer))
elif self.CP.openpilotLongitudinalControl:
can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, fcw_alert))
self.accel = pcm_accel_cmd
else:
can_sends.append(toyotacan.create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False))
if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
# This prevents unexpected pedal range rescaling
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
self.gas = interceptor_gas_cmd
# *** hud ui ***
if self.CP.carFingerprint != CAR.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(make_can_msg(addr, vl, bus))
# keep radar disabled
if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
can_sends.append([0x750, 0, b"\x0F\x02\x3E\x00\x00\x00\x00\x00", 0])
new_actuators = actuators.copy()
new_actuators.steer = apply_steer / self.params.STEER_MAX
new_actuators.steerOutputCan = apply_steer
new_actuators.steeringAngleDeg = self.last_angle
new_actuators.accel = self.accel
new_actuators.gas = self.gas
self.frame += 1
return new_actuators, can_sends

228
selfdrive/car/toyota/carstate.py Normal file
View File

@@ -0,0 +1,228 @@
import copy
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.common.numpy_fast import mean
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_CTRL
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from openpilot.selfdrive.car.interfaces import CarStateBase
from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
SteerControlType = car.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]["pt"])
self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"]
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.
# 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.low_speed_lockout = False
self.acc_type = 1
self.lkas_hud = {}
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
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.enableGasInterceptor:
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
ret.gasPressed = ret.gas > 805
else:
# TODO: find a common gas pedal percentage signal
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
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 = 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 = ret.vEgoRaw == 0
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
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
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
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
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not (self.CP.flags & ToyotaFlags.SMART_DSU.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):
self.low_speed_lockout = 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 = cp.vl["PCM_CRUISE"]["CRUISE_STATE"] 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 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.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.PRIUS_V:
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
return ret
@staticmethod
def get_can_parser(CP):
messages = [
("GEAR_PACKET", 1),
("LIGHT_STALK", 1),
("BLINKERS_STATE", 0.15),
("BODY_CONTROL_STATE", 3),
("BODY_CONTROL_STATE_2", 2),
("ESP_CONTROL", 3),
("EPS_STATUS", 25),
("BRAKE_MODULE", 40),
("WHEEL_SPEEDS", 80),
("STEER_ANGLE_SENSOR", 80),
("PCM_CRUISE", 33),
("PCM_CRUISE_SM", 1),
("STEER_TORQUE_SENSOR", 50),
]
if CP.carFingerprint in UNSUPPORTED_DSU_CAR:
messages.append(("DSU_CRUISE", 5))
messages.append(("PCM_CRUISE_ALT", 1))
else:
messages.append(("PCM_CRUISE_2", 33))
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
messages.append(("GAS_SENSOR", 50))
if CP.enableBsm:
messages.append(("BSM", 1))
if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not CP.flags & ToyotaFlags.SMART_DSU.value:
messages += [
("ACC_CONTROL", 33),
]
messages += [
("PCS_HUD", 1),
]
if CP.carFingerprint not in (TSS2_CAR - RADAR_ACC_CAR) and not CP.enableDsu and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
messages += [
("PRE_COLLISION", 33),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0)
@staticmethod
def get_cam_can_parser(CP):
messages = []
if CP.carFingerprint != CAR.PRIUS_V:
messages += [
("LKAS_HUD", 1),
]
if CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR):
messages += [
("PRE_COLLISION", 33),
("ACC_CONTROL", 33),
("PCS_HUD", 1),
]
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)

1679
selfdrive/car/toyota/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

319
selfdrive/car/toyota/interface.py Normal file
View File

@@ -0,0 +1,319 @@
from cereal import car
from openpilot.common.conversions import Conversions as CV
from panda import Panda
from panda.python import uds
from openpilot.selfdrive.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
from openpilot.selfdrive.car import get_safety_config
from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
EventName = car.CarEvent.EventName
SteerControlType = car.CarParams.SteerControlType
class CarInterface(CarInterfaceBase):
@staticmethod
def get_pid_accel_limits(CP, current_speed, cruise_speed):
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
@staticmethod
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
ret.carName = "toyota"
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)]
ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate]
# BRAKE_MODULE is on a different address for these cars
if DBC[candidate]["pt"] == "toyota_new_mc_pt_generated":
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_ALT_BRAKE
if candidate in ANGLE_CONTROL_CAR:
ret.steerControlType = SteerControlType.angle
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_LTA
# 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
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = candidate in TSS2_CAR
if candidate == CAR.PRIUS:
stop_and_go = True
ret.wheelbase = 2.70
ret.steerRatio = 15.74 # unknown end-to-end spec
ret.tireStiffnessFactor = 0.6371 # hand-tune
ret.mass = 3045. * CV.LB_TO_KG
# 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 == CAR.PRIUS_V:
stop_and_go = True
ret.wheelbase = 2.78
ret.steerRatio = 17.4
ret.tireStiffnessFactor = 0.5533
ret.mass = 3340. * CV.LB_TO_KG
elif candidate in (CAR.RAV4, CAR.RAV4H):
stop_and_go = True if (candidate in CAR.RAV4H) else False
ret.wheelbase = 2.65
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
ret.tireStiffnessFactor = 0.5533
ret.mass = 3650. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate == CAR.COROLLA:
ret.wheelbase = 2.70
ret.steerRatio = 18.27
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 2860. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2):
stop_and_go = True
ret.wheelbase = 2.79
ret.steerRatio = 16. # 14.8 is spec end-to-end
ret.wheelSpeedFactor = 1.035
ret.tireStiffnessFactor = 0.5533
ret.mass = 4481. * CV.LB_TO_KG # mean between min and max
elif candidate in (CAR.CHR, CAR.CHR_TSS2):
stop_and_go = True
ret.wheelbase = 2.63906
ret.steerRatio = 13.6
ret.tireStiffnessFactor = 0.7933
ret.mass = 3300. * CV.LB_TO_KG
elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2):
stop_and_go = True
ret.wheelbase = 2.82448
ret.steerRatio = 13.7
ret.tireStiffnessFactor = 0.7933
ret.mass = 3400. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.HIGHLANDER_TSS2):
stop_and_go = True
ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in
ret.steerRatio = 16.0
ret.tireStiffnessFactor = 0.8
ret.mass = 4516. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.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.AVALON
ret.wheelbase = 2.82
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
ret.tireStiffnessFactor = 0.7983
ret.mass = 3505. * CV.LB_TO_KG # mean between normal and hybrid
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023):
ret.wheelbase = 2.68986
ret.steerRatio = 14.3
ret.tireStiffnessFactor = 0.7933
ret.mass = 3585. * CV.LB_TO_KG # Average between ICE and Hybrid
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 == CAR.COROLLA_TSS2:
ret.wheelbase = 2.67 # Average between 2.70 for sedan and 2.64 for hatchback
ret.steerRatio = 13.9
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 3060. * CV.LB_TO_KG
elif candidate in (CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_ES_TSS2):
if candidate not in (CAR.LEXUS_ES,): # TODO: LEXUS_ES may have sng
stop_and_go = True
ret.wheelbase = 2.8702
ret.steerRatio = 16.0 # not optimized
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 3677. * CV.LB_TO_KG # mean between min and max
elif candidate == CAR.SIENNA:
stop_and_go = True
ret.wheelbase = 3.03
ret.steerRatio = 15.5
ret.tireStiffnessFactor = 0.444
ret.mass = 4590. * CV.LB_TO_KG
elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_IS_TSS2, CAR.LEXUS_RC):
ret.wheelbase = 2.79908
ret.steerRatio = 13.3
ret.tireStiffnessFactor = 0.444
ret.mass = 3736.8 * CV.LB_TO_KG
elif candidate == CAR.LEXUS_GS_F:
ret.wheelbase = 2.84988
ret.steerRatio = 13.3
ret.tireStiffnessFactor = 0.444
ret.mass = 4034. * CV.LB_TO_KG
elif candidate == CAR.LEXUS_CTH:
stop_and_go = True
ret.wheelbase = 2.60
ret.steerRatio = 18.6
ret.tireStiffnessFactor = 0.517
ret.mass = 3108 * CV.LB_TO_KG # mean between min and max
elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2):
stop_and_go = True
ret.wheelbase = 2.66
ret.steerRatio = 14.7
ret.tireStiffnessFactor = 0.444 # not optimized yet
ret.mass = 4070 * CV.LB_TO_KG
elif candidate == CAR.PRIUS_TSS2:
ret.wheelbase = 2.70002 # from toyota online sepc.
ret.steerRatio = 13.4 # True steerRatio from older prius
ret.tireStiffnessFactor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG
elif candidate == CAR.MIRAI:
stop_and_go = True
ret.wheelbase = 2.91
ret.steerRatio = 14.8
ret.tireStiffnessFactor = 0.8
ret.mass = 4300. * CV.LB_TO_KG
elif candidate == CAR.ALPHARD_TSS2:
ret.wheelbase = 3.00
ret.steerRatio = 14.2
ret.tireStiffnessFactor = 0.444
ret.mass = 4305. * CV.LB_TO_KG
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
# Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
# 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs
if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
ret.flags |= ToyotaFlags.SMART_DSU.value
# 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 = DBC[candidate]['radar'] is None or candidate in (NO_DSU_CAR - 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) \
and not (ret.flags & ToyotaFlags.SMART_DSU)
ret.enableGasInterceptor = 0x201 in fingerprint[0]
if ret.enableGasInterceptor:
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR
# if the smartDSU is detected, openpilot can send ACC_CONTROL and the smartDSU will block it from the DSU or radar.
# since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle
use_sdsu = bool(ret.flags & ToyotaFlags.SMART_DSU)
if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
ret.experimentalLongitudinalAvailable = use_sdsu
if not use_sdsu:
# Disabling radar is only supported on TSS2 radar-ACC cars
if experimental_long and candidate in RADAR_ACC_CAR and False: # TODO: disabling radar isn't supported yet
ret.flags |= ToyotaFlags.DISABLE_RADAR.value
else:
use_sdsu = use_sdsu and experimental_long
# openpilot longitudinal enabled by default:
# - non-(TSS2 radar ACC cars) w/ smartDSU installed
# - 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 w/ smartDSU installed
# - TSS2 radar ACC cars w/o smartDSU installed (disables radar)
# - TSS-P DSU-less cars w/ CAN filter installed (no radar parser yet)
ret.openpilotLongitudinalControl = use_sdsu or 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 |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL
# 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 or ret.enableGasInterceptor) else MIN_ACC_SPEED
tune = ret.longitudinalTuning
tune.deadzoneBP = [0., 9.]
tune.deadzoneV = [.0, .15]
if candidate in TSS2_CAR or ret.enableGasInterceptor:
tune.kpBP = [0., 5., 20.]
tune.kpV = [1.3, 1.0, 0.7]
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
if candidate in TSS2_CAR:
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
else:
tune.kpBP = [0., 5., 35.]
tune.kiBP = [0., 35.]
tune.kpV = [3.6, 2.4, 1.5]
tune.kiV = [0.54, 0.36]
return ret
@staticmethod
def init(CP, logcan, sendcan):
# disable radar if alpha longitudinal toggled on radar-ACC car without CAN filter/smartDSU
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(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)
# returns a car.CarState
def _update(self, c):
ret = self.CS.update(self.cp, self.cp_cam)
# events
events = self.create_common_events(ret)
# Lane Tracing Assist control is unavailable (EPS_STATUS->LTA_STATE=0) until
# the more accurate angle sensor signal is initialized
if self.CP.steerControlType == SteerControlType.angle and not self.CS.accurate_steer_angle_seen:
events.add(EventName.vehicleSensorsInvalid)
if self.CP.openpilotLongitudinalControl:
if ret.cruiseState.standstill and not ret.brakePressed and not self.CP.enableGasInterceptor:
events.add(EventName.resumeRequired)
if self.CS.low_speed_lockout:
events.add(EventName.lowSpeedLockout)
if ret.vEgo < self.CP.minEnableSpeed:
events.add(EventName.belowEngageSpeed)
if c.actuators.accel > 0.3:
# some margin on the actuator to not false trigger cancellation while stopping
events.add(EventName.speedTooLow)
if ret.vEgo < 0.001:
# while in standstill, send a user alert
events.add(EventName.manualRestart)
ret.events = events.to_msg()
return ret
# pass in a car.CarControl
# to be called @ 100hz
def apply(self, c, now_nanos):
return self.CC.update(c, self.CS, now_nanos)

94
selfdrive/car/toyota/radar_interface.py Executable file
View File

@@ -0,0 +1,94 @@
#!/usr/bin/env python3
from opendbc.can.parser import CANParser
from cereal import car
from openpilot.selfdrive.car.toyota.values import DBC, TSS2_CAR
from openpilot.selfdrive.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]['radar'], messages, 1)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
self.track_id = 0
self.radar_ts = CP.radarTimeStep
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_strings(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 = car.RadarData.new_message()
errors = []
if not self.rcp.can_valid:
errors.append("canError")
ret.errors = errors
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] = car.RadarData.RadarPoint.new_message()
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].aRel = float('nan')
self.pts[ii].yvRel = 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

118
selfdrive/car/toyota/toyotacan.py Normal file
View File

@@ -0,0 +1,118 @@
from cereal import car
SteerControlType = car.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_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, fcw_alert):
# TODO: find the exact canceling bit that does not create a chime
values = {
"ACCEL_CMD": accel,
"ACC_TYPE": acc_type,
"DISTANCE": 0,
"MINI_CAR": lead,
"PERMIT_BRAKING": 1,
"RELEASE_STANDSTILL": not standstill_req,
"CANCEL_REQ": pcm_cancel,
"ALLOW_LONG_PRESS": 1,
"ACC_CUT_IN": fcw_alert, # only shown when ACC enabled
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
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)

498
selfdrive/car/toyota/values.py Normal file
View File

@@ -0,0 +1,498 @@
import re
from collections import defaultdict
from dataclasses import dataclass, field
from enum import Enum, IntFlag, StrEnum
from typing import Dict, List, Set, Union
from cereal import car
from openpilot.common.conversions import Conversions as CV
from openpilot.selfdrive.car import AngleRateLimit, dbc_dict
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarInfo, Column, CarParts, CarHarness
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = car.CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
class CarControllerParams:
ACCEL_MAX = 1.5 # m/s2, lower than allowed 2.0 m/s2 for tuning reasons
ACCEL_MIN = -3.5 # m/s2
STEER_STEP = 1
STEER_MAX = 1500
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
# Lane Tracing Assist (LTA) control limits
# 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
ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15])
ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])
def __init__(self, CP):
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 ToyotaFlags(IntFlag):
HYBRID = 1
SMART_DSU = 2
DISABLE_RADAR = 4
class CAR(StrEnum):
# Toyota
ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
AVALON = "TOYOTA AVALON 2016"
AVALON_2019 = "TOYOTA AVALON 2019"
AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5
CAMRY = "TOYOTA CAMRY 2018"
CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5
CHR = "TOYOTA C-HR 2018"
CHR_TSS2 = "TOYOTA C-HR 2021"
COROLLA = "TOYOTA COROLLA 2017"
# LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
HIGHLANDER = "TOYOTA HIGHLANDER 2017"
HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020"
HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
PRIUS = "TOYOTA PRIUS 2017"
PRIUS_V = "TOYOTA PRIUS v 2017"
PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
RAV4 = "TOYOTA RAV4 2017"
RAV4H = "TOYOTA RAV4 HYBRID 2017"
RAV4_TSS2 = "TOYOTA RAV4 2019"
RAV4_TSS2_2022 = "TOYOTA RAV4 2022"
RAV4_TSS2_2023 = "TOYOTA RAV4 2023"
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
SIENNA = "TOYOTA SIENNA 2018"
# Lexus
LEXUS_CTH = "LEXUS CT HYBRID 2018"
LEXUS_ES = "LEXUS ES 2018"
LEXUS_ESH = "LEXUS ES HYBRID 2018"
LEXUS_ES_TSS2 = "LEXUS ES 2019"
LEXUS_IS = "LEXUS IS 2018"
LEXUS_IS_TSS2 = "LEXUS IS 2023"
LEXUS_NX = "LEXUS NX 2018"
LEXUS_NX_TSS2 = "LEXUS NX 2020"
LEXUS_RC = "LEXUS RC 2020"
LEXUS_RX = "LEXUS RX 2016"
LEXUS_RXH = "LEXUS RX HYBRID 2017"
LEXUS_RX_TSS2 = "LEXUS RX 2020"
LEXUS_GS_F = "LEXUS GS F 2016"
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 ToyotaCarInfo(CarInfo):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.toyota_a]))
CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
# Toyota
CAR.ALPHARD_TSS2: [
ToyotaCarInfo("Toyota Alphard 2019-20"),
ToyotaCarInfo("Toyota Alphard Hybrid 2021"),
],
CAR.AVALON: [
ToyotaCarInfo("Toyota Avalon 2016", "Toyota Safety Sense P"),
ToyotaCarInfo("Toyota Avalon 2017-18"),
],
CAR.AVALON_2019: [
ToyotaCarInfo("Toyota Avalon 2019-21"),
ToyotaCarInfo("Toyota Avalon Hybrid 2019-21"),
],
CAR.AVALON_TSS2: [
ToyotaCarInfo("Toyota Avalon 2022"),
ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
],
CAR.CAMRY: [
ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk"),
],
CAR.CAMRY_TSS2: [
ToyotaCarInfo("Toyota Camry 2021-24", footnotes=[Footnote.CAMRY]),
ToyotaCarInfo("Toyota Camry Hybrid 2021-24"),
],
CAR.CHR: [
ToyotaCarInfo("Toyota C-HR 2017-20"),
ToyotaCarInfo("Toyota C-HR Hybrid 2017-20"),
],
CAR.CHR_TSS2: [
ToyotaCarInfo("Toyota C-HR 2021"),
ToyotaCarInfo("Toyota C-HR Hybrid 2021-22"),
],
CAR.COROLLA: ToyotaCarInfo("Toyota Corolla 2017-19"),
CAR.COROLLA_TSS2: [
ToyotaCarInfo("Toyota Corolla 2020-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
ToyotaCarInfo("Toyota Corolla Cross (Non-US only) 2020-23", min_enable_speed=7.5),
ToyotaCarInfo("Toyota Corolla Hatchback 2019-22", video_link="https://www.youtube.com/watch?v=_66pXk0CBYA"),
# Hybrid platforms
ToyotaCarInfo("Toyota Corolla Hybrid 2020-22"),
ToyotaCarInfo("Toyota Corolla Hybrid (Non-US only) 2020-23", min_enable_speed=7.5),
ToyotaCarInfo("Toyota Corolla Cross Hybrid (Non-US only) 2020-22", min_enable_speed=7.5),
ToyotaCarInfo("Lexus UX Hybrid 2019-23"),
],
CAR.HIGHLANDER: ToyotaCarInfo("Toyota Highlander 2017-19", video_link="https://www.youtube.com/watch?v=0wS0wXSLzoo"),
CAR.HIGHLANDER_TSS2: [
ToyotaCarInfo("Toyota Highlander 2020-23"),
ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"),
],
CAR.HIGHLANDERH: ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"),
CAR.PRIUS: [
ToyotaCarInfo("Toyota Prius 2016", "Toyota Safety Sense P", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
ToyotaCarInfo("Toyota Prius 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
ToyotaCarInfo("Toyota Prius Prime 2017-20", video_link="https://www.youtube.com/watch?v=8zopPJI8XQ0"),
],
CAR.PRIUS_V: ToyotaCarInfo("Toyota Prius v 2017", "Toyota Safety Sense P", min_enable_speed=MIN_ACC_SPEED),
CAR.PRIUS_TSS2: [
ToyotaCarInfo("Toyota Prius 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"),
ToyotaCarInfo("Toyota Prius Prime 2021-22", video_link="https://www.youtube.com/watch?v=J58TvCpUd4U"),
],
CAR.RAV4: [
ToyotaCarInfo("Toyota RAV4 2016", "Toyota Safety Sense P"),
ToyotaCarInfo("Toyota RAV4 2017-18")
],
CAR.RAV4H: [
ToyotaCarInfo("Toyota RAV4 Hybrid 2016", "Toyota Safety Sense P", video_link="https://youtu.be/LhT5VzJVfNI?t=26"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2017-18", video_link="https://youtu.be/LhT5VzJVfNI?t=26")
],
CAR.RAV4_TSS2: [
ToyotaCarInfo("Toyota RAV4 2019-21", video_link="https://www.youtube.com/watch?v=wJxjDd42gGA"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2019-21"),
],
CAR.RAV4_TSS2_2022: [
ToyotaCarInfo("Toyota RAV4 2022"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2022", video_link="https://youtu.be/U0nH9cnrFB0"),
],
CAR.RAV4_TSS2_2023: [
ToyotaCarInfo("Toyota RAV4 2023-24"),
ToyotaCarInfo("Toyota RAV4 Hybrid 2023-24"),
],
CAR.MIRAI: ToyotaCarInfo("Toyota Mirai 2021"),
CAR.SIENNA: ToyotaCarInfo("Toyota Sienna 2018-20", video_link="https://www.youtube.com/watch?v=q1UPOo4Sh68", min_enable_speed=MIN_ACC_SPEED),
# Lexus
CAR.LEXUS_CTH: ToyotaCarInfo("Lexus CT Hybrid 2017-18", "Lexus Safety System+"),
CAR.LEXUS_ES: ToyotaCarInfo("Lexus ES 2017-18"),
CAR.LEXUS_ESH: ToyotaCarInfo("Lexus ES Hybrid 2017-18"),
CAR.LEXUS_ES_TSS2: [
ToyotaCarInfo("Lexus ES 2019-24"),
ToyotaCarInfo("Lexus ES Hybrid 2019-23", video_link="https://youtu.be/BZ29osRVJeg?t=12"),
],
CAR.LEXUS_IS: ToyotaCarInfo("Lexus IS 2017-19"),
CAR.LEXUS_IS_TSS2: ToyotaCarInfo("Lexus IS 2022-23"),
CAR.LEXUS_GS_F: ToyotaCarInfo("Lexus GS F 2016"),
CAR.LEXUS_NX: [
ToyotaCarInfo("Lexus NX 2018-19"),
ToyotaCarInfo("Lexus NX Hybrid 2018-19"),
],
CAR.LEXUS_NX_TSS2: [
ToyotaCarInfo("Lexus NX 2020-21"),
ToyotaCarInfo("Lexus NX Hybrid 2020-21"),
],
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"),
CAR.LEXUS_RX: [
ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"),
ToyotaCarInfo("Lexus RX 2017-19"),
],
CAR.LEXUS_RXH: [
ToyotaCarInfo("Lexus RX Hybrid 2016", "Lexus Safety System+"),
ToyotaCarInfo("Lexus RX Hybrid 2017-19"),
],
CAR.LEXUS_RX_TSS2: [
ToyotaCarInfo("Lexus RX 2020-22"),
ToyotaCarInfo("Lexus RX Hybrid 2020-22"),
],
}
# (addr, cars, bus, 1/freq*100, vl)
STATIC_DSU_MSGS = [
(0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
(0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
(0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'),
(0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'),
(0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V, CAR.LEXUS_ES),
1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'),
(0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'),
(0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'),
(0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'),
(0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'),
(0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'),
(0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'),
(0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'),
(0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX,
CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'),
(0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'),
(0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V),
0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'),
(0x366, (CAR.LEXUS_ES,), 0, 20, b'\x00\x95\x07\xfe\x08\x05\x00'),
(0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'),
(0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'),
(0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.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, 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
# TODO: use hybrid ECU, splits similar ICE and hybrid variants
# - 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.
# - abs: differentiates hybrid/ICE on most cars (Corolla TSS2 is an exception)
# - 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.abs, 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.epb, Ecu.telematics,
Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, 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.epb, Ecu.telematics, Ecu.hybrid, Ecu.srs, Ecu.combinationMeter, Ecu.transmission,
Ecu.gateway, 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.epb, Ecu.telematics,
Ecu.hybrid, Ecu.srs, Ecu.combinationMeter, Ecu.transmission, Ecu.gateway, Ecu.hvac],
bus=0,
),
],
non_essential_ecus={
# FIXME: On some models, abs can sometimes be missing
Ecu.abs: [CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_IS, CAR.ALPHARD_TSS2],
# On some models, the engine can show on two different addresses
Ecu.engine: [CAR.CAMRY, CAR.COROLLA_TSS2, CAR.CHR, CAR.CHR_TSS2, CAR.LEXUS_IS, CAR.LEXUS_RC,
CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2, CAR.LEXUS_RX_TSS2],
},
extra_ecus=[
# All known ECUs on a late-model Toyota vehicle not queried here:
# Responds to UDS:
# - HV Battery (0x713, 0x747)
# - Motor Generator (0x716, 0x724)
# - 2nd ABS "Brake/EPB" (0x730)
# Responds to KWP (0x1a8801):
# - Steering Angle Sensor (0x7b3)
# - EPS/EMPS (0x7a0, 0x7a1)
# Responds to KWP (0x1a8881):
# - Body Control Module ((0x750, 0x40))
# Hybrid control computer can be on 0x7e2 (KWP) or 0x7d2 (UDS) depending on platform
(Ecu.hybrid, 0x7e2, None), # Hybrid Control Assembly & Computer
# TODO: if these duplicate ECUs always exist together, remove one
(Ecu.srs, 0x780, None), # SRS Airbag
(Ecu.srs, 0x784, None), # SRS Airbag 2
# Likely only exists on cars where EPB isn't standard (e.g. Camry, Avalon (/Hybrid))
# On some cars, EPB is controlled by the ABS module
(Ecu.epb, 0x750, 0x2c), # Electronic Parking Brake
# This isn't accessible on all cars
(Ecu.gateway, 0x750, 0x5f),
# On some cars, this only responds to b'\x1a\x88\x81', which is reflected by the b'\x1a\x88\x00' query
(Ecu.telematics, 0x750, 0xc7),
# 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),
# On some cars, this only responds to b'\x1a\x88\x80'
(Ecu.combinationMeter, 0x7c0, None),
(Ecu.hvac, 0x7c4, None),
],
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
STEER_THRESHOLD = 100
DBC = {
CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CHR_TSS2: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.RAV4_TSS2_2022: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.RAV4_TSS2_2023: dbc_dict('toyota_nodsu_pt_generated', None),
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ES: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_IS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'),
CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
CAR.LEXUS_GS_F: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'),
}
# These cars have non-standard EPS torque scale factors. All others are 73
EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100})
# Toyota/Lexus Safety Sense 2.0 and 2.5
TSS2_CAR = {CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.COROLLA_TSS2, CAR.LEXUS_ES_TSS2,
CAR.LEXUS_RX_TSS2, CAR.HIGHLANDER_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.LEXUS_IS_TSS2,
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.CHR_TSS2}
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CAMRY}
# the DSU uses the AEB message for longitudinal on these cars
UNSUPPORTED_DSU_CAR = {CAR.LEXUS_IS, CAR.LEXUS_RC, CAR.LEXUS_GS_F}
# these cars have a radar which sends ACC messages instead of the camera
RADAR_ACC_CAR = {CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023, CAR.CHR_TSS2}
# these cars use the Lane Tracing Assist (LTA) message for lateral control
ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023}
# no resume button press required
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH}