from iqdbc.car import Bus, structs, get_safety_config, uds from iqdbc.car.toyota.carstate import CarState from iqdbc.car.toyota.carcontroller import CarController from iqdbc.car.toyota.radar_interface import RadarInterface from iqdbc.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 iqdbc.car.disable_ecu import disable_ecu from iqdbc.car.interfaces import CarInterfaceBase from iqdbc.lvbs.car.toyota.values import ToyotaFlagsIQ, ToyotaSafetyFlagsIQ 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_IQ, current_speed, cruise_speed): return CarControllerParams(CP).ACCEL_MIN, CarControllerParams(CP).ACCEL_MAX @staticmethod def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams: ret.brand = "toyota" ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.toyota)] ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] # BRAKE_MODULE is on a different address for these cars if DBC[candidate][Bus.pt] == "toyota_new_mc_pt_generated": ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.ALT_BRAKE.value if ret.flags & ToyotaFlags.SECOC.value: ret.secOcRequired = True ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.SECOC.value if candidate in ANGLE_CONTROL_CAR: ret.steerControlType = SteerControlType.angle ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LTA.value # LTA control can be more delayed and winds up more often ret.steerActuatorDelay = 0.18 ret.steerLimitTimer = 0.8 else: CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 stop_and_go = candidate in TSS2_CAR # In TSS2 cars, the camera does long control found_ecus = [fw.ecu for fw in car_fw] 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 # Hybrids have much quicker longitudinal actuator response if ret.flags & ToyotaFlags.HYBRID.value: ret.longitudinalActuatorDelay = 0.05 return ret @staticmethod def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams: if candidate in UNSUPPORTED_DSU_CAR: ret.iqSafetyFlags |= ToyotaSafetyFlagsIQ.UNSUPPORTED_DSU if candidate in TSS2_CAR: ret.iqSafetyFlags |= ToyotaSafetyFlagsIQ.LKAS_HUD # 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 |= ToyotaFlagsIQ.SMART_DSU.value if 0x2AA in fingerprint[0] and candidate in NO_DSU_CAR: ret.flags |= ToyotaFlagsIQ.RADAR_CAN_FILTER.value # Detect ZSS, which allows iqpilot 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 |= ToyotaFlagsIQ.ZSS.value if candidate == CAR.TOYOTA_PRIUS: if ret.flags & ToyotaFlagsIQ.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 & ToyotaFlagsIQ.SMART_DSU) stock_cp.minEnableSpeed = -1. if use_sdsu else stock_cp.minEnableSpeed stock_cp.alphaLongitudinalAvailable = use_sdsu or candidate in RADAR_ACC_CAR if use_sdsu: use_sdsu = use_sdsu and alpha_long stock_cp.flags &= ~ToyotaFlags.DISABLE_RADAR.value elif candidate in (RADAR_ACC_CAR | NO_DSU_CAR): if alpha_long and candidate in RADAR_ACC_CAR: stock_cp.flags |= ToyotaFlags.DISABLE_RADAR.value # openpilot longitudinal enabled by default: # - TSS2 cars with camera sending ACC_CONTROL where we can block it # openpilot longitudinal behind experimental long toggle: # - cars w/ smartDSU or CAN filter installed # - TSS2 radar ACC cars w/o smartDSU installed (disables radar) 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.iqSafetyFlags |= ToyotaSafetyFlagsIQ.GAS_INTERCEPTOR stock_cp.minEnableSpeed = -1. if ret.flags & ToyotaFlagsIQ.STOCK_LONGITUDINAL: stock_cp.alphaLongitudinalAvailable = False stock_cp.openpilotLongitudinalControl = False 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_IQ, 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)