mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-02-18 13:03:52 +08:00
b39eb5ab91
* Toyota: move Enforce Factory Longitudinal Control checks to sunnypilot interface * hmm * back
219 lines
10 KiB
Python
219 lines
10 KiB
Python
from opendbc.car import Bus, structs, get_safety_config, uds
|
|
from opendbc.car.toyota.carstate import CarState
|
|
from opendbc.car.toyota.carcontroller import CarController
|
|
from opendbc.car.toyota.radar_interface import RadarInterface
|
|
from opendbc.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
|
|
MIN_ACC_SPEED, EPS_SCALE, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, \
|
|
ToyotaSafetyFlags, UNSUPPORTED_DSU_CAR
|
|
from opendbc.car.disable_ecu import disable_ecu
|
|
from opendbc.car.interfaces import CarInterfaceBase
|
|
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP, ToyotaSafetyFlagsSP
|
|
|
|
SteerControlType = structs.CarParams.SteerControlType
|
|
|
|
|
|
class CarInterface(CarInterfaceBase):
|
|
CarState = CarState
|
|
CarController = CarController
|
|
RadarInterface = RadarInterface
|
|
|
|
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport,)
|
|
|
|
@staticmethod
|
|
def get_pid_accel_limits(CP, CP_SP, current_speed, cruise_speed):
|
|
return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX
|
|
|
|
@staticmethod
|
|
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
|
ret.brand = "toyota"
|
|
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.toyota)]
|
|
ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate]
|
|
|
|
# BRAKE_MODULE is on a different address for these cars
|
|
if DBC[candidate][Bus.pt] == "toyota_new_mc_pt_generated":
|
|
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.ALT_BRAKE.value
|
|
|
|
if ret.flags & ToyotaFlags.SECOC.value:
|
|
ret.secOcRequired = True
|
|
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.SECOC.value
|
|
ret.dashcamOnly = is_release
|
|
|
|
if candidate in ANGLE_CONTROL_CAR:
|
|
ret.steerControlType = SteerControlType.angle
|
|
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LTA.value
|
|
|
|
# LTA control can be more delayed and winds up more often
|
|
ret.steerActuatorDelay = 0.18
|
|
ret.steerLimitTimer = 0.8
|
|
else:
|
|
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
|
|
|
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
|
|
ret.steerLimitTimer = 0.4
|
|
|
|
stop_and_go = candidate in TSS2_CAR
|
|
|
|
# In TSS2 cars, the camera does long control
|
|
found_ecus = [fw.ecu for fw in car_fw]
|
|
|
|
if Ecu.hybrid in found_ecus:
|
|
ret.flags |= ToyotaFlags.HYBRID.value
|
|
|
|
if candidate == CAR.TOYOTA_PRIUS:
|
|
stop_and_go = True
|
|
# Only give steer angle deadzone to for bad angle sensor prius
|
|
for fw in car_fw:
|
|
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
|
|
ret.steerActuatorDelay = 0.25
|
|
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
|
|
|
|
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
|
|
stop_and_go = True
|
|
ret.wheelSpeedFactor = 1.035
|
|
|
|
elif candidate in (CAR.TOYOTA_AVALON, CAR.TOYOTA_AVALON_2019, CAR.TOYOTA_AVALON_TSS2):
|
|
# starting from 2019, all Avalon variants have stop and go
|
|
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
|
|
stop_and_go = candidate != CAR.TOYOTA_AVALON
|
|
|
|
elif candidate in (CAR.TOYOTA_CHR, CAR.TOYOTA_CAMRY, CAR.TOYOTA_SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_LS, CAR.LEXUS_NX):
|
|
# TODO: Some of these platforms are not advertised to have full range ACC, do they really all have sng?
|
|
stop_and_go = True
|
|
|
|
ret.centerToFront = ret.wheelbase * 0.44
|
|
|
|
# TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction.
|
|
# Detect flipped signals and enable for C-HR and others
|
|
ret.enableBsm = 0x3F6 in fingerprint[0] and candidate in TSS2_CAR
|
|
|
|
# No radar dbc for cars without DSU which are not TSS 2.0
|
|
# TODO: make an adas dbc file for dsu-less models
|
|
ret.radarUnavailable = Bus.radar not in DBC[candidate] or candidate in (NO_DSU_CAR - TSS2_CAR)
|
|
|
|
# since we don't yet parse radar on TSS2/TSS-P radar-based ACC cars, gate longitudinal behind experimental toggle
|
|
if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
|
|
ret.alphaLongitudinalAvailable = candidate in RADAR_ACC_CAR
|
|
|
|
# Disabling radar is only supported on TSS2 radar-ACC cars
|
|
if alpha_long and candidate in RADAR_ACC_CAR:
|
|
ret.flags |= ToyotaFlags.DISABLE_RADAR.value
|
|
|
|
# openpilot longitudinal enabled by default:
|
|
# - cars w/ DSU disconnected
|
|
# - TSS2 cars with camera sending ACC_CONTROL where we can block it
|
|
# openpilot longitudinal behind experimental long toggle:
|
|
# - TSS2 radar ACC cars (disables radar)
|
|
|
|
ret.openpilotLongitudinalControl = (candidate in (TSS2_CAR - RADAR_ACC_CAR) or
|
|
bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value))
|
|
|
|
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
|
|
|
|
if not ret.openpilotLongitudinalControl:
|
|
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
|
|
|
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
|
# to a negative value, so it won't matter.
|
|
ret.minEnableSpeed = -1. if stop_and_go else MIN_ACC_SPEED
|
|
|
|
if candidate in TSS2_CAR:
|
|
ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
|
|
|
|
ret.vEgoStopping = 0.25
|
|
ret.vEgoStarting = 0.25
|
|
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
|
|
|
|
# Hybrids have much quicker longitudinal actuator response
|
|
if ret.flags & ToyotaFlags.HYBRID.value:
|
|
ret.longitudinalActuatorDelay = 0.05
|
|
|
|
return ret
|
|
|
|
@staticmethod
|
|
def _get_params_sp(stock_cp: structs.CarParams, ret: structs.CarParamsSP, candidate, fingerprint: dict[int, dict[int, int]],
|
|
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_sp: bool, docs: bool) -> structs.CarParamsSP:
|
|
if candidate in UNSUPPORTED_DSU_CAR:
|
|
ret.safetyParam |= ToyotaSafetyFlagsSP.UNSUPPORTED_DSU
|
|
|
|
# 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 |= ToyotaFlagsSP.SMART_DSU.value
|
|
|
|
if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR:
|
|
ret.flags |= ToyotaFlagsSP.RADAR_CAN_FILTER.value
|
|
|
|
# Detect ZSS, which allows sunnypilot to utilize an improved angle sensor for some Toyota vehicles
|
|
# https://github.com/zorrobyte/betterToyotaAngleSensorForOP
|
|
if 0x23 in fingerprint[0] and not stock_cp.flags & ToyotaFlags.SECOC:
|
|
ret.flags |= ToyotaFlagsSP.ZSS.value
|
|
|
|
if candidate == CAR.TOYOTA_PRIUS:
|
|
if ret.flags & ToyotaFlagsSP.ZSS:
|
|
stock_cp.steerRatio = 15.0
|
|
stock_cp.mass = 3370.
|
|
|
|
# reuse logic from _get_params
|
|
# 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':
|
|
stock_cp.steerActuatorDelay = 0.25
|
|
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning, steering_angle_deadzone_deg=0.0)
|
|
|
|
use_sdsu = bool(ret.flags & ToyotaFlagsSP.SMART_DSU)
|
|
|
|
stock_cp.minEnableSpeed = -1. if use_sdsu else stock_cp.minEnableSpeed
|
|
|
|
# reuse logic from _get_params
|
|
# 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
|
|
if candidate in (RADAR_ACC_CAR | NO_DSU_CAR):
|
|
stock_cp.alphaLongitudinalAvailable = use_sdsu or candidate in RADAR_ACC_CAR
|
|
|
|
if not use_sdsu:
|
|
# Disabling radar is only supported on TSS2 radar-ACC cars
|
|
if alpha_long and candidate in RADAR_ACC_CAR:
|
|
stock_cp.flags |= ToyotaFlags.DISABLE_RADAR.value
|
|
else:
|
|
use_sdsu = use_sdsu and alpha_long
|
|
stock_cp.flags &= ~ToyotaFlags.DISABLE_RADAR.value
|
|
|
|
# openpilot longitudinal enabled by default:
|
|
# - non-(TSS2 radar ACC cars) w/ smartDSU installed
|
|
# - 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)
|
|
stock_cp.openpilotLongitudinalControl = use_sdsu or \
|
|
candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
|
|
bool(stock_cp.flags & ToyotaFlags.DISABLE_RADAR)
|
|
|
|
ret.enableGasInterceptor = 0x201 in fingerprint[0] and stock_cp.openpilotLongitudinalControl and \
|
|
not stock_cp.flags & ToyotaFlags.SECOC
|
|
|
|
if ret.enableGasInterceptor:
|
|
ret.safetyParam |= ToyotaSafetyFlagsSP.GAS_INTERCEPTOR
|
|
stock_cp.minEnableSpeed = -1.
|
|
|
|
if not stock_cp.openpilotLongitudinalControl:
|
|
stock_cp.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
|
else:
|
|
stock_cp.safetyConfigs[0].safetyParam &= ~ToyotaSafetyFlags.STOCK_LONGITUDINAL.value
|
|
|
|
return ret
|
|
|
|
@staticmethod
|
|
def init(CP, CP_SP, can_recv, can_send, communication_control=None):
|
|
# disable radar if alpha longitudinal toggled on radar-ACC car without CAN filter/smartDSU
|
|
if CP.flags & ToyotaFlags.DISABLE_RADAR.value:
|
|
if communication_control is None:
|
|
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
|
disable_ecu(can_recv, can_send, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)
|
|
|
|
@staticmethod
|
|
def deinit(CP, can_recv, can_send):
|
|
# re-enable radar if alpha longitudinal toggled on radar-ACC car
|
|
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX, uds.MESSAGE_TYPE.NORMAL])
|
|
CarInterface.init(CP, can_recv, can_send, communication_control)
|