Shane Smiskol
1 year ago
parent
0052d6c587
commit
b02971659c
14 changed files with 3337 additions and 0 deletions
@ -0,0 +1,196 @@ |
||||
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.interfaces import CarControllerBase |
||||
from openpilot.selfdrive.car.toyota import toyotacan |
||||
from openpilot.selfdrive.car.toyota_old.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(CarControllerBase): |
||||
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, lat_active, |
||||
self.steer_rate_counter, MAX_STEER_RATE_FRAMES) |
||||
|
||||
if not lat_active: |
||||
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): |
||||
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 |
||||
@ -0,0 +1,234 @@ |
||||
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_old.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 = 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 |
||||
|
||||
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 |
||||
|
||||
if self.CP.carFingerprint != CAR.MIRAI: |
||||
ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"] |
||||
|
||||
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 != CAR.MIRAI: |
||||
messages.append(("ENGINE_RPM", 42)) |
||||
|
||||
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) |
||||
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,324 @@ |
||||
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_old.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_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.HIGHLANDER_TSS2): |
||||
# TODO: TSS-P models can do stop and go, but unclear if it requires sDSU or unplugging DSU |
||||
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_old.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_old.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_ES_TSS2): |
||||
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.LEXUS_LC_TSS2: |
||||
ret.wheelbase = 2.87 |
||||
ret.steerRatio = 13.0 |
||||
ret.tireStiffnessFactor = 0.444 # not optimized yet |
||||
ret.mass = 4500 * 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) |
||||
|
||||
# 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 or candidate in RADAR_ACC_CAR |
||||
|
||||
if not use_sdsu: |
||||
# Disabling radar is only supported on TSS2 radar-ACC cars |
||||
if experimental_long and candidate in RADAR_ACC_CAR: |
||||
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 |
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0] and ret.openpilotLongitudinalControl |
||||
|
||||
if not ret.openpilotLongitudinalControl: |
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_STOCK_LONGITUDINAL |
||||
|
||||
if ret.enableGasInterceptor: |
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_TOYOTA_GAS_INTERCEPTOR |
||||
|
||||
# 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) |
||||
@ -0,0 +1,94 @@ |
||||
#!/usr/bin/env python3 |
||||
from opendbc.can.parser import CANParser |
||||
from cereal import car |
||||
from openpilot.selfdrive.car.toyota_old.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 |
||||
@ -0,0 +1,35 @@ |
||||
#!/usr/bin/env python3 |
||||
from collections import defaultdict |
||||
from cereal import car |
||||
from openpilot.selfdrive.car.toyota_old.values import PLATFORM_CODE_ECUS, get_platform_codes |
||||
from openpilot.selfdrive.car.toyota_old.fingerprints import FW_VERSIONS |
||||
|
||||
Ecu = car.CarParams.Ecu |
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} |
||||
|
||||
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, key=lambda x: int(x[0])): |
||||
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_NAME[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_NAME[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_NAME[ecu[0]]}, {hex(ecu[1])}, {ecu[2]}):') |
||||
for code, cars in codes.items(): |
||||
print(f' {code!r}: {sorted(cars)}') |
||||
@ -0,0 +1,162 @@ |
||||
#!/usr/bin/env python3 |
||||
from hypothesis import given, settings, strategies as st |
||||
import unittest |
||||
|
||||
from cereal import car |
||||
from openpilot.selfdrive.car.fw_versions import build_fw_dict |
||||
from openpilot.selfdrive.car.toyota_old.fingerprints import FW_VERSIONS |
||||
from openpilot.selfdrive.car.toyota_old.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, \ |
||||
FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ |
||||
get_platform_codes |
||||
|
||||
Ecu = car.CarParams.Ecu |
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} |
||||
|
||||
|
||||
class TestToyotaInterfaces(unittest.TestCase): |
||||
def test_car_sets(self): |
||||
self.assertTrue(len(ANGLE_CONTROL_CAR - TSS2_CAR) == 0) |
||||
self.assertTrue(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 |
||||
self.assertEqual(ANGLE_CONTROL_CAR, {CAR.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: |
||||
self.assertEqual(dbc["pt"], "toyota_nodsu_pt_generated") |
||||
|
||||
def test_essential_ecus(self): |
||||
# Asserts standard ECUs exist for each platform |
||||
common_ecus = {Ecu.fwdRadar, Ecu.fwdCamera} |
||||
for car_model, ecus in FW_VERSIONS.items(): |
||||
with self.subTest(car_model=car_model.value): |
||||
present_ecus = {ecu[0] for ecu in ecus} |
||||
missing_ecus = common_ecus - present_ecus |
||||
self.assertEqual(len(missing_ecus), 0) |
||||
|
||||
# Some exceptions for other common ECUs |
||||
if car_model not in (CAR.ALPHARD_TSS2,): |
||||
self.assertIn(Ecu.abs, present_ecus) |
||||
|
||||
if car_model not in (CAR.MIRAI,): |
||||
self.assertIn(Ecu.engine, present_ecus) |
||||
|
||||
if car_model not in (CAR.PRIUS_V, CAR.LEXUS_CTH): |
||||
self.assertIn(Ecu.eps, present_ecus) |
||||
|
||||
|
||||
class TestToyotaFingerprint(unittest.TestCase): |
||||
def test_non_essential_ecus(self): |
||||
# 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 self.subTest(car_model=car_model.value): |
||||
engine_ecus = {ecu for ecu in ecus if ecu[0] == Ecu.engine} |
||||
self.assertEqual(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") |
||||
|
||||
# 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): |
||||
# Asserts ECU keys essential for fuzzy fingerprinting are available on all platforms |
||||
for car_model, ecus in FW_VERSIONS.items(): |
||||
with self.subTest(car_model=car_model.value): |
||||
for platform_code_ecu in PLATFORM_CODE_ECUS: |
||||
if platform_code_ecu == Ecu.eps and car_model in (CAR.PRIUS_V, CAR.LEXUS_CTH,): |
||||
continue |
||||
if platform_code_ecu == Ecu.abs and car_model in (CAR.ALPHARD_TSS2,): |
||||
continue |
||||
self.assertIn(platform_code_ecu, [e[0] for e in ecus]) |
||||
|
||||
def test_fw_format(self): |
||||
# 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 self.subTest(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 |
||||
self.assertEqual(1, len(result), f"Unable to parse FW: {fw}") |
||||
self.assertEqual(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. |
||||
self.assertTrue(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", |
||||
]) |
||||
self.assertEqual(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", |
||||
]) |
||||
self.assertEqual(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", |
||||
]) |
||||
self.assertEqual(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", |
||||
]) |
||||
self.assertEqual(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({"ecu": ecu_name, "fwVersion": fw, "address": addr, |
||||
"subAddress": 0 if sub_addr is None else sub_addr}) |
||||
|
||||
CP = car.CarParams.new_message(carFw=car_fw) |
||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), FW_VERSIONS) |
||||
if len(matches) == 1: |
||||
self.assertEqual(list(matches)[0], platform) |
||||
else: |
||||
# If a platform has multiple matches, add it and its matches |
||||
platforms_with_shared_codes |= {str(platform), *matches} |
||||
|
||||
self.assertEqual(platforms_with_shared_codes, FUZZY_EXCLUDED_PLATFORMS, (len(platforms_with_shared_codes), len(FW_VERSIONS))) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -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) |
||||
@ -0,0 +1,500 @@ |
||||
import re |
||||
from collections import defaultdict |
||||
from dataclasses import dataclass, field |
||||
from enum import Enum, IntFlag, StrEnum |
||||
|
||||
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_OLD ALPHARD 2020" |
||||
AVALON = "TOYOTA_OLD AVALON 2016" |
||||
AVALON_2019 = "TOYOTA_OLD AVALON 2019" |
||||
AVALON_TSS2 = "TOYOTA_OLD AVALON 2022" # TSS 2.5 |
||||
CAMRY = "TOYOTA_OLD CAMRY 2018" |
||||
CAMRY_TSS2 = "TOYOTA_OLD CAMRY 2021" # TSS 2.5 |
||||
CHR = "TOYOTA_OLD C-HR 2018" |
||||
CHR_TSS2 = "TOYOTA_OLD C-HR 2021" |
||||
COROLLA = "TOYOTA_OLD COROLLA 2017" |
||||
# LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid |
||||
COROLLA_TSS2 = "TOYOTA_OLD COROLLA TSS2 2019" |
||||
HIGHLANDER = "TOYOTA_OLD HIGHLANDER 2017" |
||||
HIGHLANDER_TSS2 = "TOYOTA_OLD HIGHLANDER 2020" |
||||
PRIUS = "TOYOTA_OLD PRIUS 2017" |
||||
PRIUS_V = "TOYOTA_OLD PRIUS v 2017" |
||||
PRIUS_TSS2 = "TOYOTA_OLD PRIUS TSS2 2021" |
||||
RAV4 = "TOYOTA_OLD RAV4 2017" |
||||
RAV4H = "TOYOTA_OLD RAV4 HYBRID 2017" |
||||
RAV4_TSS2 = "TOYOTA_OLD RAV4 2019" |
||||
RAV4_TSS2_2022 = "TOYOTA_OLD RAV4 2022" |
||||
RAV4_TSS2_2023 = "TOYOTA_OLD RAV4 2023" |
||||
MIRAI = "TOYOTA_OLD MIRAI 2021" # TSS 2.5 |
||||
SIENNA = "TOYOTA_OLD SIENNA 2018" |
||||
|
||||
# Lexus |
||||
LEXUS_CTH = "LEXUS_OLD CT HYBRID 2018" |
||||
LEXUS_ES = "LEXUS_OLD ES 2018" |
||||
LEXUS_ES_TSS2 = "LEXUS_OLD ES 2019" |
||||
LEXUS_IS = "LEXUS_OLD IS 2018" |
||||
LEXUS_IS_TSS2 = "LEXUS_OLD IS 2023" |
||||
LEXUS_NX = "LEXUS_OLD NX 2018" |
||||
LEXUS_NX_TSS2 = "LEXUS_OLD NX 2020" |
||||
LEXUS_LC_TSS2 = "LEXUS_OLD LC 2024" |
||||
LEXUS_RC = "LEXUS_OLD RC 2020" |
||||
LEXUS_RX = "LEXUS_OLD RX 2016" |
||||
LEXUS_RX_TSS2 = "LEXUS_OLD RX 2020" |
||||
LEXUS_GS_F = "LEXUS_OLD 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, 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"), |
||||
ToyotaCarInfo("Toyota Highlander Hybrid 2017-19"), |
||||
], |
||||
CAR.HIGHLANDER_TSS2: [ |
||||
ToyotaCarInfo("Toyota Highlander 2020-23"), |
||||
ToyotaCarInfo("Toyota Highlander Hybrid 2020-23"), |
||||
], |
||||
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"), |
||||
ToyotaCarInfo("Lexus ES Hybrid 2017-18"), |
||||
], |
||||
CAR.LEXUS_ES_TSS2: [ |
||||
ToyotaCarInfo("Lexus ES 2019-24"), |
||||
ToyotaCarInfo("Lexus ES Hybrid 2019-24", 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_LC_TSS2: ToyotaCarInfo("Lexus LC 2024"), |
||||
CAR.LEXUS_RC: ToyotaCarInfo("Lexus RC 2018-20"), |
||||
CAR.LEXUS_RX: [ |
||||
ToyotaCarInfo("Lexus RX 2016", "Lexus Safety System+"), |
||||
ToyotaCarInfo("Lexus RX 2017-19"), |
||||
# Hybrid platforms |
||||
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_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), |
||||
(0x128, (CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 3, b'\x03\x00\x20\x00\x00\x52'), |
||||
(0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, |
||||
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), |
||||
(0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, |
||||
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), |
||||
(0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.PRIUS_V), |
||||
1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), |
||||
(0X161, (CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), |
||||
(0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, |
||||
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), |
||||
(0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), |
||||
(0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), |
||||
(0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), |
||||
(0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, |
||||
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), |
||||
(0x365, (CAR.PRIUS, CAR.LEXUS_NX, CAR.HIGHLANDER), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), |
||||
(0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.LEXUS_RX, |
||||
CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), |
||||
(0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.HIGHLANDER), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), |
||||
(0x366, (CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), |
||||
0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), |
||||
(0x470, (CAR.PRIUS, CAR.LEXUS_RX), 1, 100, b'\x00\x00\x02\x7a'), |
||||
(0x470, (CAR.HIGHLANDER, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), |
||||
(0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RX, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, |
||||
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ES, 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 |
||||
# - 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.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.HIGHLANDER, 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, 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_LC_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_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_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.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.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.LEXUS_LC_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.HIGHLANDER, CAR.SIENNA} |
||||
Loading…
Reference in new issue