diff --git a/opendbc/car/tesla/carcontroller.py b/opendbc/car/tesla/carcontroller.py index 7724a22c1f3..976501b7298 100644 --- a/opendbc/car/tesla/carcontroller.py +++ b/opendbc/car/tesla/carcontroller.py @@ -8,6 +8,7 @@ from opendbc.car.tesla.values import CarControllerParams, CANBUS, LEGACY_CARS, CAR from opendbc.car.vehicle_model import VehicleModel from opendbc.car.tesla.preap.carcontroller import PreAPLongController, init_preap_can +from opendbc.car.tesla.preap.nap_conf import nap_conf from opendbc.car.tesla.preap.stock_cc_spoofer import StockCCSpoofer @@ -38,6 +39,7 @@ def __init__(self, dbc_names, CP): self.preap_long = PreAPLongController() self.stock_cc = StockCCSpoofer() self.tesla_can = init_preap_can(dbc_names, self.packers) + self.radar_vin_idx = 0 else: self.tesla_can = TeslaCANRaven(self.packers) @@ -121,6 +123,17 @@ def _update_preap(self, CC, CS, now_nanos): if self.stock_cc.pcc_event: CS.pccEvent = self.stock_cc.pcc_event + # Tinkla 0.6.6 donor contract: stream VIN/position/EPAS on 0x560 + # when radar is on. Empty VIN is 17 spaces (this-car passthrough); + # position and EPAS still apply. Panda stays silent until all three + # fragments arrive, so 10 Hz keeps that pause around 300 ms. + if nap_conf.radar_enabled and self.frame % 10 == 0: + can_sends.append(self.tesla_can.create_radar_vin_msg( + self.radar_vin_idx, nap_conf.radar_donor_vin, True, + nap_conf.radar_position, nap_conf.radar_epas_type, + )) + self.radar_vin_idx = (self.radar_vin_idx + 1) % 3 + # Turn-signal drive: keep the indicator flashing during the lane-change # arming window and maneuver. controlsd sets CC.leftBlinker/rightBlinker # whenever laneChangeState != off, and clears them when it returns to off diff --git a/opendbc/car/tesla/preap/carcontroller.py b/opendbc/car/tesla/preap/carcontroller.py index ce5126d1ac4..391cfd5c4ba 100644 --- a/opendbc/car/tesla/preap/carcontroller.py +++ b/opendbc/car/tesla/preap/carcontroller.py @@ -27,26 +27,27 @@ def init_preap_can(dbc_names, packers): ENGAGE_GRACE_PEDAL_RAMP_RATE_UP = 0.9 # DI/update at 50Hz # The pedal controller updates at 50 Hz. Prompt when a sustained deceleration -# request is not being delivered while the command sits deep in the regen -# range. Available regen varies with SOC and battery temperature, so proximity -# to the DI rail is not a reliable "at the limit" signal: on a full battery the -# rail delivers a fraction of nominal regen, and the integrator only reaches it -# near the end of an under-delivery event. Field capture (2026-07-22, minutes -# after supercharging) showed the command within 0.5 DI of the rail for just -# 0.3 s of a multi-second under-delivery, so a rail gate plus a consecutive -# dwell can never complete. Evidence instead accumulates in a saturating -# up/down counter, which also rides through single-update shortfall noise. -# Trigger and clear thresholds are separated so the visible prompt cannot -# chatter. -REGEN_DECEL_PROMPT_DWELL_UPDATES = 40 # 0.8s of net evidence at 50Hz +# request is not being delivered while the command is in the regen range. +# Available regen varies with SOC and battery temperature, so proximity to +# the DI rail is not a reliable "at the limit" signal: on a full battery the +# rail delivers a fraction of nominal regen, and the integrator only reaches +# it near the end of an under-delivery event. Field capture (2026-07-22, +# minutes after supercharging) showed the command within 0.5 DI of the rail +# for just 0.3 s of a multi-second under-delivery, so a rail gate plus a +# consecutive dwell can never complete. Evidence instead accumulates in a +# saturating up/down counter, which also rides through single-update +# shortfall noise. Trigger and clear thresholds are separated so the visible +# prompt cannot chatter. Thresholds are biased early so the driver has time +# to add friction brake. +REGEN_DECEL_PROMPT_DWELL_UPDATES = 25 # 0.5s of net evidence at 50Hz REGEN_DECEL_PROMPT_MIN_SPEED = 2.0 # m/s; do not prompt for a stopped/settling car REGEN_DECEL_PROMPT_CLEAR_SPEED = 1.0 # m/s -REGEN_DECEL_SHORTFALL_TRIGGER = 0.35 # m/s² -REGEN_DECEL_SHORTFALL_CLEAR = 0.15 # m/s² -REGEN_COMMAND_TRIGGER_DI = -2.0 # command deep in the regen range -REGEN_COMMAND_CLEAR_DI = -1.0 -REGEN_DECEL_REQUEST_TRIGGER = -0.5 # m/s²; a meaningful deceleration request -REGEN_DECEL_REQUEST_CLEAR = -0.2 # m/s² +REGEN_DECEL_SHORTFALL_TRIGGER = 0.20 # m/s² +REGEN_DECEL_SHORTFALL_CLEAR = 0.08 # m/s² +REGEN_COMMAND_TRIGGER_DI = -1.0 # command in the regen range +REGEN_COMMAND_CLEAR_DI = -0.5 +REGEN_DECEL_REQUEST_TRIGGER = -0.35 # m/s²; a meaningful deceleration request +REGEN_DECEL_REQUEST_CLEAR = -0.15 # m/s² class RegenDecelMonitor: diff --git a/opendbc/car/tesla/preap/interface.py b/opendbc/car/tesla/preap/interface.py index dc080dce613..13862ab9aff 100644 --- a/opendbc/car/tesla/preap/interface.py +++ b/opendbc/car/tesla/preap/interface.py @@ -7,7 +7,6 @@ # Safety param flags matching tesla_preap.h (renumbered — LONG_CONTROL removed as dead code) PREAP_FLAG_ENABLE_PEDAL = 1 PREAP_FLAG_RADAR_EMULATION = 2 -PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 from opendbc.car.tesla.preap.constants import ( ACCEL_PREAP_BP, ACCEL_PREAP_PROFILES, PEDAL_LONG_K_BP, PEDAL_LONG_KP_V, PEDAL_LONG_KI_V, @@ -37,16 +36,12 @@ def get_preap_params(ret, fingerprint): flags = 0 use_pedal = nap_conf.use_pedal radar_enabled = nap_conf.radar_enabled - radar_behind_nosecone = nap_conf.radar_behind_nosecone - carlog.info("Pre-AP fingerprint: use_pedal=%s radar_enabled=%s behind_nosecone=%s", - use_pedal, radar_enabled, radar_behind_nosecone) + carlog.info("Pre-AP fingerprint: use_pedal=%s radar_enabled=%s", use_pedal, radar_enabled) if use_pedal: flags |= PREAP_FLAG_ENABLE_PEDAL if radar_enabled: flags |= PREAP_FLAG_RADAR_EMULATION - if radar_behind_nosecone: - flags |= PREAP_FLAG_RADAR_BEHIND_NOSECONE ret.safetyConfigs = [ get_safety_config(structs.CarParams.SafetyModel.teslaPreap, int(flags)), diff --git a/opendbc/car/tesla/preap/nap_conf.py b/opendbc/car/tesla/preap/nap_conf.py index 1619fc125bb..ad64af23e97 100644 --- a/opendbc/car/tesla/preap/nap_conf.py +++ b/opendbc/car/tesla/preap/nap_conf.py @@ -33,10 +33,23 @@ 'pedal_calib_zero': 0.0, 'pedal_calib_factor': 1.0, 'radar_enabled': False, + 'radar_ignore_hw_fail': False, 'radar_behind_nosecone': False, 'radar_offset': 0.0, + 'radar_donor_vin': '', + 'radar_epas_type': 0, + 'radar_position': 0, } + +def normalize_radar_donor_vin(value): + """Return a 17-char donor VIN, or empty if the value is not one.""" + text = "" if value is None else str(value) + if isinstance(value, bytes): + text = value.decode("ascii", errors="ignore") + vin = "".join(ch for ch in text.upper() if ch.isalnum()) + return vin if len(vin) == 17 else "" + # Pedal DI (Driver Intent) constants — internal representation before calibration PEDAL_DI_MIN = -5 # Max regen (coasting hard) PEDAL_DI_ZERO = 0 # Neutral @@ -149,6 +162,36 @@ def _put_param_float(self, param_key, json_key, value): _params.put(param_key, float(value)) self._put(json_key, float(value)) + def _get_param_int(self, param_key, json_key, default): + if _PARAMS_AVAILABLE: + val = _params.get(param_key, return_default=True) + try: + return int(val) + except (TypeError, ValueError): + return default + return int(self._get(json_key, default)) + + def _put_param_int(self, param_key, json_key, value): + if _PARAMS_AVAILABLE: + _params.put(param_key, int(value)) + self._put(json_key, int(value)) + + def _get_param_str(self, param_key, json_key, default=""): + if _PARAMS_AVAILABLE: + val = _params.get(param_key, return_default=True) + if val is None: + return default + if isinstance(val, bytes): + return val.decode("ascii", errors="ignore") + return str(val) + return str(self._get(json_key, default)) + + def _put_param_str(self, param_key, json_key, value): + text = str(value) + if _PARAMS_AVAILABLE: + _params.put(param_key, text) + self._put(json_key, text) + # Bool properties @property @@ -167,6 +210,14 @@ def radar_enabled(self): def radar_enabled(self, value): self._put_param_bool(NAPParamKeys.RADAR_ENABLED, 'radar_enabled', value) + @property + def radar_ignore_hw_fail(self): + return self._get_param_bool(NAPParamKeys.RADAR_IGNORE_HW_FAIL, 'radar_ignore_hw_fail') + + @radar_ignore_hw_fail.setter + def radar_ignore_hw_fail(self, value): + self._put_param_bool(NAPParamKeys.RADAR_IGNORE_HW_FAIL, 'radar_ignore_hw_fail', value) + @property def radar_behind_nosecone(self): return self._get_param_bool(NAPParamKeys.RADAR_BEHIND_NOSECONE, 'radar_behind_nosecone') @@ -175,6 +226,30 @@ def radar_behind_nosecone(self): def radar_behind_nosecone(self, value): self._put_param_bool(NAPParamKeys.RADAR_BEHIND_NOSECONE, 'radar_behind_nosecone', value) + @property + def radar_donor_vin(self): + return normalize_radar_donor_vin(self._get_param_str(NAPParamKeys.RADAR_DONOR_VIN, 'radar_donor_vin', '')) + + @radar_donor_vin.setter + def radar_donor_vin(self, value): + self._put_param_str(NAPParamKeys.RADAR_DONOR_VIN, 'radar_donor_vin', normalize_radar_donor_vin(value)) + + @property + def radar_epas_type(self): + return max(0, min(4, self._get_param_int(NAPParamKeys.RADAR_EPAS_TYPE, 'radar_epas_type', 0))) + + @radar_epas_type.setter + def radar_epas_type(self, value): + self._put_param_int(NAPParamKeys.RADAR_EPAS_TYPE, 'radar_epas_type', max(0, min(4, int(value)))) + + @property + def radar_position(self): + return max(0, min(2, self._get_param_int(NAPParamKeys.RADAR_POSITION, 'radar_position', 0))) + + @radar_position.setter + def radar_position(self, value): + self._put_param_int(NAPParamKeys.RADAR_POSITION, 'radar_position', max(0, min(2, int(value)))) + @property def pedal_calibrated(self): if not self._get_param_bool(NAPParamKeys.PEDAL_CALIB_DONE, 'pedal_calibrated'): @@ -349,6 +424,7 @@ def print_config(self): print("") print(" [RADAR]") print(f" Radar Enabled: {'ON' if self.radar_enabled else 'OFF'}") + print(f" Ignore HWFail: {'ON' if self.radar_ignore_hw_fail else 'OFF'}") print(f" Behind Nosecone: {'YES' if self.radar_behind_nosecone else 'NO'}") print(f" Radar Offset: {self.radar_offset}m") print("") @@ -369,6 +445,7 @@ def get_all_params(self): 'pedal_can_bus': self.pedal_can_bus, 'pedal_can_zero': self.pedal_can_zero, 'radar_enabled': self.radar_enabled, + 'radar_ignore_hw_fail': self.radar_ignore_hw_fail, 'radar_behind_nosecone': self.radar_behind_nosecone, 'radar_offset': self.radar_offset, } diff --git a/opendbc/car/tesla/preap/nap_params.py b/opendbc/car/tesla/preap/nap_params.py index 0f2e138dd13..996622f2278 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -24,8 +24,15 @@ class NAPParamKeys: # Radar RADAR_ENABLED = "NAPRadarEnabled" + RADAR_HUD = "NAPRadarHud" + RADAR_IGNORE_HW_FAIL = "NAPRadarIgnoreHwFail" RADAR_BEHIND_NOSECONE = "NAPRadarBehindNosecone" RADAR_OFFSET = "NAPRadarOffset" + RADAR_DONOR_VIN = "NAPRadarDonorVin" + RADAR_EPAS_TYPE = "NAPRadarEpasType" + RADAR_POSITION = "NAPRadarPosition" + RADAR_READ_VIN = "NAPRadarReadVin" + RADAR_VIN_READ_STATUS = "NAPRadarVinReadStatus" # iBooster / Braking IBOOSTER_ENABLED = "NAPiBoosterEnabled" @@ -48,8 +55,13 @@ class NAPParamKeys: NAPParamKeys.PEDAL_CALIB_FACTOR: 1.0, NAPParamKeys.PEDAL_CALIB_ZERO: 0.0, NAPParamKeys.RADAR_ENABLED: False, + NAPParamKeys.RADAR_HUD: False, + NAPParamKeys.RADAR_IGNORE_HW_FAIL: False, NAPParamKeys.RADAR_BEHIND_NOSECONE: False, NAPParamKeys.RADAR_OFFSET: 0.0, + NAPParamKeys.RADAR_DONOR_VIN: "", + NAPParamKeys.RADAR_EPAS_TYPE: 0, + NAPParamKeys.RADAR_POSITION: 0, NAPParamKeys.IBOOSTER_ENABLED: False, NAPParamKeys.BRAKE_FACTOR: 1.0, NAPParamKeys.FORCE_PRE_AP: False, diff --git a/opendbc/car/tesla/preap/radar_donor_vin.py b/opendbc/car/tesla/preap/radar_donor_vin.py new file mode 100644 index 00000000000..e10a28ae881 --- /dev/null +++ b/opendbc/car/tesla/preap/radar_donor_vin.py @@ -0,0 +1,361 @@ +"""Read the VIN already stored in a Tesla Bosch radar. + +This is a read-only UDS conversation on the radar bus. It never writes +F190, never runs routine 0x0A03, and never flashes firmware. The host +uses the returned VIN to impersonate the donor on the live GTW feeds. +""" + +from dataclasses import dataclass +from enum import Enum, auto + +from opendbc.car.can_definitions import CanData +from opendbc.car.tesla.preap.nap_conf import normalize_radar_donor_vin + + +RADAR_UDS_TX = 0x641 +RADAR_UDS_RX = 0x651 +RADAR_UDS_BUS = 1 +F190 = 0xF190 + +TESTER_PRESENT = b"\x3e\x00" +DEFAULT_SESSION = b"\x10\x01" +EXTENDED_SESSION = b"\x10\x03" +READ_F190 = b"\x22\xf1\x90" +CLEANUP_MARKER = b"\x3e\x80" +FLOW_CONTROL = b"\x30\x00\x00\x00\x00\x00\x00\x00" + +# TeslaRadarAlertMatrix 0x501, Intel 1-bit signals. +VIN_VALIDITY_BYTE = 4 +VIN_VALIDITY_MASK = 0x10 + + +class RadarDonorVinState(Enum): + IDLE = auto() + TESTER_PRESENT = auto() + DEFAULT_SESSION = auto() + EXTENDED_SESSION = auto() + READINESS = auto() + READ_F190 = auto() + CLEANUP = auto() + COMPLETE = auto() + FAILED = auto() + + +class RadarDonorVinFailure(Enum): + ABORTED = auto() + TIMEOUT = auto() + MALFORMED_RESPONSE = auto() + UNEXPECTED_RESPONSE = auto() + NEGATIVE_RESPONSE = auto() + INVALID_VIN = auto() + + +class _IsoTpState(Enum): + INCOMPLETE = auto() + COMPLETE = auto() + MALFORMED = auto() + + +@dataclass(frozen=True) +class _IsoTpResult: + state: _IsoTpState + payload: bytes | None = None + can_sends: tuple[CanData, ...] = () + + +class _IsoTpAssembler: + def __init__(self): + self.reset() + + def consume(self, data: bytes, expected_prefix: bytes) -> _IsoTpResult: + if len(data) != 8: + return self._malformed() + frame_type = data[0] >> 4 + if frame_type == 0: + length = data[0] & 0x0F + if self._length is not None or not 1 <= length <= 7: + return self._malformed() + payload = data[1:1 + length] + if not payload.startswith(expected_prefix) and not payload.startswith(b"\x7f"): + return self._malformed() + return _IsoTpResult(_IsoTpState.COMPLETE, payload) + if frame_type == 1: + length = ((data[0] & 0x0F) << 8) | data[1] + if self._length is not None or length <= 7: + return self._malformed() + prefix = data[2:] + if not prefix.startswith(expected_prefix) and not prefix.startswith(b"\x7f"): + return self._malformed() + self._payload = bytearray(prefix) + self._length = length + self._sequence = 1 + return _IsoTpResult( + _IsoTpState.INCOMPLETE, + can_sends=(CanData(RADAR_UDS_TX, FLOW_CONTROL, RADAR_UDS_BUS),), + ) + if frame_type == 2: + sequence = data[0] & 0x0F + if self._length is None or sequence != self._sequence: + return self._malformed() + self._sequence = (self._sequence + 1) & 0x0F + remaining = self._length - len(self._payload) + self._payload.extend(data[1:1 + remaining]) + if len(self._payload) < self._length: + return _IsoTpResult(_IsoTpState.INCOMPLETE) + payload = bytes(self._payload) + self.reset() + return _IsoTpResult(_IsoTpState.COMPLETE, payload) + return self._malformed() + + def reset(self) -> None: + self._payload = bytearray() + self._length: int | None = None + self._sequence = 1 + + def _malformed(self) -> _IsoTpResult: + self.reset() + return _IsoTpResult(_IsoTpState.MALFORMED) + + +@dataclass(frozen=True) +class RadarDonorVinOutput: + state: RadarDonorVinState + can_sends: tuple[CanData, ...] + vin: str | None = None + failure: RadarDonorVinFailure | None = None + + +@dataclass +class _Request: + payload: bytes + positive_prefix: bytes + sent_at: float + timeout: float + + +def radar_alert_vin_validity(data: bytes) -> bool: + """True when TeslaRadarAlertMatrix asserts RADC_a037_vinValidity.""" + return len(data) == 8 and (data[VIN_VALIDITY_BYTE] & VIN_VALIDITY_MASK) != 0 + + +def _uds_frame(payload: bytes) -> CanData: + return CanData(RADAR_UDS_TX, (bytes([len(payload)]) + payload).ljust(8, b"\x00"), RADAR_UDS_BUS) + + +class RadarDonorVinCommissioner: + """Read F190 when vinValidity sticks, or when the UI asks once.""" + + STABLE_FRAMES = 8 + + def __init__(self, store_vin): + self.store_vin = store_vin + self.reader = RadarDonorVinReader() + self._vin_fault_frames = 0 + self._attempted = False + self._force_used = False + self._persisted_vin: str | None = None + + @property + def read_finished(self) -> bool: + return self.reader.state in (RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED) + + def _persist_vin(self, vin: str | None) -> None: + # F190 is the VIN. Cleanup is courtesy. A cleanup timeout used to + # drop a completed read, which left the spare radar on this-car + # passthrough for the rest of the drive. + if not vin or vin == self._persisted_vin: + return + self.store_vin(vin) + self._persisted_vin = vin + + def _start_read(self, now: float) -> None: + self._attempted = True + self._vin_fault_frames = 0 + self._persisted_vin = None + self.reader.start(now) + + def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, + stored_vin: str, controls_allowed: bool, enabled: bool, + force_read: bool = False) -> tuple[CanData, ...]: + if not radar_enabled: + return () + + running = self.reader.state not in ( + RadarDonorVinState.IDLE, RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED, + ) + if controls_allowed or enabled: + if running: + return self.reader.abort(now).can_sends + return () + + if not force_read: + self._force_used = False + elif force_read and not self._force_used and not running: + self._force_used = True + self._start_read(now) + running = True + + if self.reader.state in (RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED): + self._persist_vin(self.reader.vin) + return () + if running: + output = self.reader.update(can_packets, now) + self._persist_vin(output.vin) + return output.can_sends + + if stored_vin or self._attempted: + return () + for packet in can_packets: + if packet.address == 0x501 and packet.src == RADAR_UDS_BUS and radar_alert_vin_validity(packet.dat): + self._vin_fault_frames += 1 + if self._vin_fault_frames < self.STABLE_FRAMES: + return () + self._start_read(now) + output = self.reader.update(can_packets, now) + self._persist_vin(output.vin) + return output.can_sends + + +class RadarDonorVinReader: + """One-shot F190 read. Caller owns Params, sockets, and when to start.""" + + RESPONSE_TIMEOUT = 3.0 + RESPONSE_PENDING_TIMEOUT = 5.0 + CLEANUP_TIMEOUT = 3.0 + OVERALL_TIMEOUT = 20.0 + + def __init__(self): + self.state = RadarDonorVinState.IDLE + self.failure: RadarDonorVinFailure | None = None + self.vin: str | None = None + self._assembler = _IsoTpAssembler() + self._request: _Request | None = None + self._started_at = 0.0 + self._cleanup_deadline: float | None = None + self._cleanup_acks_remaining = 2 + + def start(self, now: float) -> None: + self.state = RadarDonorVinState.TESTER_PRESENT + self.failure = None + self.vin = None + self._assembler.reset() + self._request = None + self._started_at = now + self._cleanup_deadline = None + self._cleanup_acks_remaining = 2 + + def abort(self, now: float) -> RadarDonorVinOutput: + if self.state not in (RadarDonorVinState.IDLE, RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED): + self._fail(RadarDonorVinFailure.ABORTED, now) + return self._output(()) + + def update(self, can_packets: list[CanData], now: float) -> RadarDonorVinOutput: + if self.state in (RadarDonorVinState.IDLE, RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED): + return self._output(()) + if self.state != RadarDonorVinState.CLEANUP and now - self._started_at >= self.OVERALL_TIMEOUT: + self._fail(RadarDonorVinFailure.TIMEOUT, now) + return self._output(()) + if self.state == RadarDonorVinState.CLEANUP and self._cleanup_deadline is not None and now >= self._cleanup_deadline: + self._fail(RadarDonorVinFailure.TIMEOUT, now) + return self._output(()) + + can_sends: list[CanData] = [] + if self._request is not None: + if now - self._request.sent_at >= self._request.timeout: + self._fail(RadarDonorVinFailure.TIMEOUT, now) + return self._output(()) + for packet in can_packets: + if packet.address != RADAR_UDS_RX or packet.src != RADAR_UDS_BUS: + continue + result = self._assembler.consume(packet.dat, self._request.positive_prefix) + can_sends.extend(result.can_sends) + if result.state == _IsoTpState.MALFORMED: + self._fail(RadarDonorVinFailure.MALFORMED_RESPONSE, now) + return self._output(can_sends) + if result.state == _IsoTpState.COMPLETE: + self._handle_payload(result.payload or b"", now) + return self._output(can_sends) + return self._output(can_sends) + + request = self._request_for_state() + if request is not None: + payload, prefix = request + timeout = self.CLEANUP_TIMEOUT if self.state == RadarDonorVinState.CLEANUP else self.RESPONSE_TIMEOUT + self._assembler = _IsoTpAssembler() + self._request = _Request(payload, prefix, now, timeout) + if self.state == RadarDonorVinState.CLEANUP: + if self._cleanup_deadline is None: + # Marker first, then the two default-session ACKs share one deadline. + can_sends.append(_uds_frame(CLEANUP_MARKER)) + self._cleanup_deadline = now + self.CLEANUP_TIMEOUT + can_sends.append(_uds_frame(payload)) + return self._output(can_sends) + + def _request_for_state(self) -> tuple[bytes, bytes] | None: + if self.state in (RadarDonorVinState.TESTER_PRESENT, RadarDonorVinState.READINESS): + return TESTER_PRESENT, b"\x7e\x00" + if self.state in (RadarDonorVinState.DEFAULT_SESSION, RadarDonorVinState.CLEANUP): + return DEFAULT_SESSION, b"\x50\x01" + if self.state == RadarDonorVinState.EXTENDED_SESSION: + return EXTENDED_SESSION, b"\x50\x03" + if self.state == RadarDonorVinState.READ_F190: + return READ_F190, b"\x62\xf1\x90" + return None + + def _handle_payload(self, payload: bytes, now: float) -> None: + assert self._request is not None + request = self._request + if payload.startswith(b"\x7f"): + self._handle_negative(payload, request.payload[0], now) + return + if not payload.startswith(request.positive_prefix): + self._fail(RadarDonorVinFailure.UNEXPECTED_RESPONSE, now) + return + self._request = None + if self.state == RadarDonorVinState.TESTER_PRESENT: + self.state = RadarDonorVinState.DEFAULT_SESSION + elif self.state == RadarDonorVinState.DEFAULT_SESSION: + self.state = RadarDonorVinState.EXTENDED_SESSION + elif self.state == RadarDonorVinState.EXTENDED_SESSION: + self.state = RadarDonorVinState.READINESS + elif self.state == RadarDonorVinState.READINESS: + self.state = RadarDonorVinState.READ_F190 + elif self.state == RadarDonorVinState.READ_F190: + vin = normalize_radar_donor_vin(payload[3:]) + if not vin: + self._fail(RadarDonorVinFailure.INVALID_VIN, now) + return + self.vin = vin + self.state = RadarDonorVinState.CLEANUP + self._cleanup_acks_remaining = 2 + elif self.state == RadarDonorVinState.CLEANUP: + self._cleanup_acks_remaining -= 1 + if self._cleanup_acks_remaining <= 0: + self.state = RadarDonorVinState.FAILED if self.failure else RadarDonorVinState.COMPLETE + + def _handle_negative(self, payload: bytes, request_sid: int, now: float) -> None: + if len(payload) != 3 or payload[1] != request_sid: + self._fail(RadarDonorVinFailure.MALFORMED_RESPONSE, now) + return + if payload[2] == 0x78: + assert self._request is not None + self._request.sent_at = now + self._request.timeout = self.RESPONSE_PENDING_TIMEOUT + return + self._fail(RadarDonorVinFailure.NEGATIVE_RESPONSE, now) + + def _fail(self, failure: RadarDonorVinFailure, now: float) -> None: + if self.failure is None: + self.failure = failure + self._request = None + self._assembler.reset() + if self.state == RadarDonorVinState.CLEANUP: + self.state = RadarDonorVinState.FAILED + else: + self._cleanup_acks_remaining = 2 + self._cleanup_deadline = None + self.state = RadarDonorVinState.CLEANUP + _ = now + + def _output(self, can_sends: list[CanData] | tuple[CanData, ...]) -> RadarDonorVinOutput: + return RadarDonorVinOutput(self.state, tuple(can_sends), self.vin, self.failure) diff --git a/opendbc/car/tesla/preap/radar_table_freeze.py b/opendbc/car/tesla/preap/radar_table_freeze.py new file mode 100644 index 00000000000..88e4640069a --- /dev/null +++ b/opendbc/car/tesla/preap/radar_table_freeze.py @@ -0,0 +1,47 @@ +"""Detect a Bosch object table that has stopped changing. + +Tinkla's five-second rule: a salvage radar that rejects VIN / xWD / +position keeps publishing the same targets. Calibration SGUFail with +live tracks must not trip this. A static table of four or more points +for ~5 s at 8 Hz, together with SGUFail, is the lockout. +""" + +from __future__ import annotations + + +class BoschTableFreezeWatch: + STABLE_CYCLES = 40 # ~5 s at the Bosch trigger rate + MIN_POINTS = 4 + + def __init__(self, stable_cycles: int = STABLE_CYCLES, min_points: int = MIN_POINTS): + self.stable_cycles = stable_cycles + self.min_points = min_points + self._signature: tuple | None = None + self._stuck_cycles = 0 + + def reset(self) -> None: + self._signature = None + self._stuck_cycles = 0 + + def update(self, points) -> bool: + signature = _point_signature(points) + if signature is None or len(signature) < self.min_points: + self.reset() + return False + if signature == self._signature: + self._stuck_cycles += 1 + else: + self._signature = signature + self._stuck_cycles = 1 + return self._stuck_cycles >= self.stable_cycles + + +def _point_signature(points) -> tuple | None: + rows = [] + for point in points: + try: + rows.append((round(float(point.dRel), 2), round(float(point.yRel), 2), + round(float(point.vRel), 2))) + except (AttributeError, TypeError, ValueError): + return None + return tuple(sorted(rows)) if rows else None diff --git a/opendbc/car/tesla/preap/teslacan.py b/opendbc/car/tesla/preap/teslacan.py index 0b0e8136c96..0fd829548f2 100644 --- a/opendbc/car/tesla/preap/teslacan.py +++ b/opendbc/car/tesla/preap/teslacan.py @@ -80,6 +80,22 @@ def create_pedal_command(self, accel_command, enable=1, pedal_can_bus=None): return (GAS_COMMAND_ID, bytes(msg.raw), pedal_can_bus) + @staticmethod + def create_radar_vin_msg(fragment, vin, use_radar, position, epas_type): + """Host→panda 0x560 fragment. Panda consumes this; it does not go on the car. + + Layout is the Tinkla 0.6.6 create_radar_VIN_msg packing. + """ + padded = (vin + (" " * 17))[:17] + if fragment == 0: + flags = (1 if use_radar else 0) + ((position & 0x03) << 1) + ((epas_type & 0x07) << 3) + dat = bytes((0, 1, flags, 0, 0, ord(padded[0]), ord(padded[1]), ord(padded[2]))) + elif fragment == 1: + dat = bytes((1, *(ord(padded[i]) for i in range(3, 10)))) + else: + dat = bytes((2, *(ord(padded[i]) for i in range(10, 17)))) + return (0x560, dat, 0) + def create_epas_control(self, counter, mode): values = { "EPB_epasEACAllow": mode, diff --git a/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py b/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py index 39e2f50fede..3c5ea76eac4 100644 --- a/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py +++ b/opendbc/car/tesla/preap/tests/test_longitudinal_tuning.py @@ -10,7 +10,6 @@ def test_pedal_params_leave_generic_outer_feedback_disabled(monkeypatch): pedal_conf = SimpleNamespace( use_pedal=True, radar_enabled=False, - radar_behind_nosecone=False, ) monkeypatch.setattr(interface, "nap_conf", pedal_conf) params = structs.CarParams.new_message() diff --git a/opendbc/car/tesla/preap/tests/test_pedal_authority.py b/opendbc/car/tesla/preap/tests/test_pedal_authority.py index f31c860052c..0af3fde9073 100644 --- a/opendbc/car/tesla/preap/tests/test_pedal_authority.py +++ b/opendbc/car/tesla/preap/tests/test_pedal_authority.py @@ -835,9 +835,10 @@ def test_regen_prompt_uses_hysteresis_and_clears_when_decel_recovers(): # These values have crossed back over the trigger thresholds, but remain # inside the clear thresholds so sensor noise cannot chatter the prompt. - assert _update_regen_monitor(monitor, actual_accel=-1.25, v_ego=1.5, pedal_di=-4.25) + # shortfall 0.15 m/s²: below 0.20 trigger, above 0.08 clear. + assert _update_regen_monitor(monitor, actual_accel=-1.35, v_ego=1.5, pedal_di=-4.25) - assert not _update_regen_monitor(monitor, actual_accel=-1.35) + assert not _update_regen_monitor(monitor, actual_accel=-1.45) assert not monitor.active @@ -859,19 +860,26 @@ def test_regen_prompt_survives_brief_shortfall_dropouts(): for _ in range(3 * REGEN_DECEL_PROMPT_DWELL_UPDATES): for _ in range(9): fired = _update_regen_monitor(monitor) or fired - # shortfall 0.3 m/s²: below the trigger, above the clear threshold - fired = _update_regen_monitor(monitor, actual_accel=-1.2) or fired + # shortfall 0.15 m/s²: below the 0.20 trigger, above the 0.08 clear + fired = _update_regen_monitor(monitor, actual_accel=-1.35) or fired if fired: break assert fired -def test_regen_prompt_requires_deep_regen_command(): - # A command still in the shallow-regen range has authority left; the +def test_regen_prompt_requires_regen_command(): + # A command still on the gas/coast side has regen authority left; the # driver does not need the brake yet. monitor = RegenDecelMonitor() for _ in range(3 * REGEN_DECEL_PROMPT_DWELL_UPDATES): + assert not _update_regen_monitor(monitor, pedal_di=0.0) + + +def test_regen_prompt_fires_in_shallow_regen_command(): + monitor = RegenDecelMonitor() + for _ in range(REGEN_DECEL_PROMPT_DWELL_UPDATES - 1): assert not _update_regen_monitor(monitor, pedal_di=-1.5) + assert _update_regen_monitor(monitor, pedal_di=-1.5) def _replay_fixture(name): diff --git a/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py new file mode 100644 index 00000000000..23accb04431 --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py @@ -0,0 +1,35 @@ +from opendbc.car.tesla.preap.nap_conf import normalize_radar_donor_vin +from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP + + +def test_normalize_radar_donor_vin(): + assert normalize_radar_donor_vin("5yjsa1e42ff156789") == "5YJSA1E42FF156789" + assert normalize_radar_donor_vin("5YJ-SA1E42-FF156789") == "5YJSA1E42FF156789" + assert normalize_radar_donor_vin("short") == "" + assert normalize_radar_donor_vin(" ") == "" + + +def test_create_radar_vin_msg_matches_tinkla_layout(): + vin = "5YJSA1E42FF156789" + addr, dat, bus = TeslaCANPreAP.create_radar_vin_msg(0, vin, True, 1, 3) + assert addr == 0x560 + assert bus == 0 + assert dat[0] == 0 + assert dat[2] == 1 + (1 << 1) + (3 << 3) + assert dat[5:8] == b"5YJ" + + _addr, dat1, _bus = TeslaCANPreAP.create_radar_vin_msg(1, vin, True, 1, 3) + assert dat1[0] == 1 + assert dat1[1:] == b"SA1E42F" + + _addr, dat2, _bus = TeslaCANPreAP.create_radar_vin_msg(2, vin, True, 1, 3) + assert dat2[0] == 2 + assert dat2[1:] == b"F156789" + + +def test_create_radar_vin_msg_empty_vin_still_packs_position(): + addr, dat, bus = TeslaCANPreAP.create_radar_vin_msg(0, "", True, 1, 0) + assert addr == 0x560 + assert bus == 0 + assert dat[2] == 1 + (1 << 1) + assert dat[5:8] == b" " diff --git a/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py new file mode 100644 index 00000000000..2e6da80747d --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py @@ -0,0 +1,239 @@ +from opendbc.car.can_definitions import CanData +from opendbc.car.tesla.preap.radar_donor_vin import ( + FLOW_CONTROL, + RADAR_UDS_BUS, + RADAR_UDS_RX, + RADAR_UDS_TX, + RadarDonorVinCommissioner, + RadarDonorVinFailure, + RadarDonorVinReader, + RadarDonorVinState, + radar_alert_vin_validity, +) + + +VIN = "5YJSA1E42FF156789" + + +def _rx(payload: bytes) -> CanData: + return CanData(RADAR_UDS_RX, (bytes([len(payload)]) + payload).ljust(8, b"\x00"), RADAR_UDS_BUS) + + +def _first_frame(payload: bytes) -> CanData: + length = len(payload) + dat = bytes([0x10 | (length >> 8), length & 0xFF]) + payload[:6] + return CanData(RADAR_UDS_RX, dat.ljust(8, b"\x00"), RADAR_UDS_BUS) + + +def _consecutive(sequence: int, data: bytes) -> CanData: + return CanData(RADAR_UDS_RX, bytes([0x20 | sequence]) + data.ljust(7, b"\x00"), RADAR_UDS_BUS) + + +def _drive_to_read(reader: RadarDonorVinReader, now: float) -> float: + replies = { + RadarDonorVinState.TESTER_PRESENT: b"\x7e\x00", + RadarDonorVinState.DEFAULT_SESSION: b"\x50\x01", + RadarDonorVinState.EXTENDED_SESSION: b"\x50\x03", + RadarDonorVinState.READINESS: b"\x7e\x00", + } + while reader.state in replies: + output = reader.update([], now) + assert output.can_sends + now += 0.01 + output = reader.update([_rx(replies[reader.state])], now) + now += 0.01 + assert output.failure is None + assert reader.state == RadarDonorVinState.READ_F190 + return now + + +def test_radar_alert_vin_validity_bit(): + clear = bytes(8) + set_bit = bytes([0, 0, 0, 0, 0x10, 0, 0, 0]) + assert radar_alert_vin_validity(clear) is False + assert radar_alert_vin_validity(set_bit) is True + assert radar_alert_vin_validity(b"\x00") is False + + +def test_reader_completes_on_multiframe_f190(): + reader = RadarDonorVinReader() + now = 0.0 + reader.start(now) + now = _drive_to_read(reader, now) + + output = reader.update([], now) + assert output.can_sends[0].address == RADAR_UDS_TX + assert output.can_sends[0].dat[:4] == b"\x03\x22\xf1\x90" + + vin_payload = b"\x62\xf1\x90" + VIN.encode("ascii") + now += 0.01 + first = reader.update([_first_frame(vin_payload)], now) + assert first.can_sends == (CanData(RADAR_UDS_TX, FLOW_CONTROL, RADAR_UDS_BUS),) + now += 0.01 + rest = vin_payload[6:] + output = reader.update([_consecutive(1, rest[:7]), _consecutive(2, rest[7:])], now) + assert output.vin == VIN + assert output.state == RadarDonorVinState.CLEANUP + + now += 0.01 + output = reader.update([], now) + assert output.can_sends[0].dat[:3] == b"\x02\x3e\x80" + assert output.can_sends[1].dat[:3] == b"\x02\x10\x01" + now += 0.01 + output = reader.update([_rx(b"\x50\x01")], now) + now += 0.01 + output = reader.update([], now) + now += 0.01 + output = reader.update([_rx(b"\x50\x01")], now) + assert output.state == RadarDonorVinState.COMPLETE + assert output.vin == VIN + assert output.failure is None + + +def test_reader_rejects_short_vin_and_cleans_up(): + reader = RadarDonorVinReader() + now = 0.0 + reader.start(now) + now = _drive_to_read(reader, now) + reader.update([], now) + now += 0.01 + output = reader.update([_rx(b"\x62\xf1\x90ABC")], now) + assert output.failure == RadarDonorVinFailure.INVALID_VIN + assert output.state == RadarDonorVinState.CLEANUP + assert output.vin is None + + +def test_reader_times_out_waiting_for_f190(): + reader = RadarDonorVinReader() + now = 0.0 + reader.start(now) + now = _drive_to_read(reader, now) + reader.update([], now) + output = reader.update([], now + RadarDonorVinReader.RESPONSE_TIMEOUT) + assert output.failure == RadarDonorVinFailure.TIMEOUT + assert output.state == RadarDonorVinState.CLEANUP + + +def test_reader_does_not_start_from_idle(): + reader = RadarDonorVinReader() + output = reader.update([], 1.0) + assert output.state == RadarDonorVinState.IDLE + assert output.can_sends == () + + +def _fault_frame() -> CanData: + return CanData(0x501, bytes([0, 0, 0, 0, 0x10, 0, 0, 0]), RADAR_UDS_BUS) + + +def _commissioner_kwargs(**overrides): + values = { + "radar_enabled": True, + "stored_vin": "", + "controls_allowed": False, + "enabled": False, + } + values.update(overrides) + return values + + +def test_commissioner_ignores_happy_radar(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + clear = CanData(0x501, bytes(8), RADAR_UDS_BUS) + for now in range(10): + sends = commissioner.update([clear], float(now), **_commissioner_kwargs()) + assert sends == () + assert stored == [] + assert commissioner.reader.state == RadarDonorVinState.IDLE + + +def test_commissioner_starts_after_stable_vin_fault(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + sends = () + for now in range(RadarDonorVinCommissioner.STABLE_FRAMES): + sends = commissioner.update([_fault_frame()], float(now), **_commissioner_kwargs()) + assert sends + assert sends[0].address == RADAR_UDS_TX + assert stored == [] + + +def test_commissioner_does_not_touch_existing_donor_vin(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + for now in range(10): + sends = commissioner.update([_fault_frame()], float(now), **_commissioner_kwargs(stored_vin=VIN)) + assert sends == () + assert stored == [] + + +def test_commissioner_force_read_starts_without_vin_fault(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + sends = commissioner.update([], 0.0, **_commissioner_kwargs(stored_vin=VIN), force_read=True) + assert sends + assert sends[0].address == RADAR_UDS_TX + assert stored == [] + assert commissioner.reader.state == RadarDonorVinState.TESTER_PRESENT + + +def test_commissioner_force_read_does_not_restart_every_tick(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + first = commissioner.update([], 0.0, **_commissioner_kwargs(), force_read=True) + second = commissioner.update([], 0.01, **_commissioner_kwargs(), force_read=True) + assert first + assert second == () + assert commissioner.reader.state == RadarDonorVinState.TESTER_PRESENT + + +def test_commissioner_force_read_can_run_again_after_clear(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + commissioner.update([], 0.0, **_commissioner_kwargs(), force_read=True) + commissioner.reader.state = RadarDonorVinState.COMPLETE + commissioner.update([], 0.01, **_commissioner_kwargs(), force_read=False) + sends = commissioner.update([], 0.02, **_commissioner_kwargs(), force_read=True) + assert sends + assert commissioner.reader.state == RadarDonorVinState.TESTER_PRESENT + + +def test_commissioner_stores_vin_when_cleanup_fails(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + commissioner.reader.vin = VIN + commissioner.reader.state = RadarDonorVinState.FAILED + commissioner.reader.failure = RadarDonorVinFailure.TIMEOUT + commissioner.update([], 1.0, **_commissioner_kwargs()) + commissioner.update([], 1.1, **_commissioner_kwargs()) + assert stored == [VIN] + + +def test_commissioner_stores_vin_as_soon_as_f190_decodes(): + stored = [] + commissioner = RadarDonorVinCommissioner(stored.append) + now = 0.0 + commissioner.update([], now, **_commissioner_kwargs(), force_read=True) + replies = { + RadarDonorVinState.TESTER_PRESENT: b"\x7e\x00", + RadarDonorVinState.DEFAULT_SESSION: b"\x50\x01", + RadarDonorVinState.EXTENDED_SESSION: b"\x50\x03", + RadarDonorVinState.READINESS: b"\x7e\x00", + } + while commissioner.reader.state in replies: + now += 0.01 + commissioner.update([_rx(replies[commissioner.reader.state])], now, **_commissioner_kwargs()) + assert commissioner.reader.state == RadarDonorVinState.READ_F190 + now += 0.01 + commissioner.update([], now, **_commissioner_kwargs()) + vin_payload = b"\x62\xf1\x90" + VIN.encode("ascii") + now += 0.01 + commissioner.update([_first_frame(vin_payload)], now, **_commissioner_kwargs()) + now += 0.01 + rest = vin_payload[6:] + commissioner.update( + [_consecutive(1, rest[:7]), _consecutive(2, rest[7:])], + now, **_commissioner_kwargs(), + ) + assert stored == [VIN] + assert commissioner.reader.state == RadarDonorVinState.CLEANUP diff --git a/opendbc/car/tesla/preap/tests/test_radar_table_freeze.py b/opendbc/car/tesla/preap/tests/test_radar_table_freeze.py new file mode 100644 index 00000000000..1230fee2b18 --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_radar_table_freeze.py @@ -0,0 +1,32 @@ +from types import SimpleNamespace + +from opendbc.car.tesla.preap.radar_table_freeze import BoschTableFreezeWatch + + +def _pts(*rows): + return [SimpleNamespace(dRel=d, yRel=y, vRel=v) for d, y, v in rows] + + +def test_live_table_does_not_freeze(): + watch = BoschTableFreezeWatch(stable_cycles=3, min_points=4) + points = _pts((10, 0, -1), (20, 1, 0), (30, -1, 0), (40, 0.5, 0)) + assert watch.update(points) is False + points[0].dRel = 9.5 + assert watch.update(points) is False + points[0].dRel = 9.0 + assert watch.update(points) is False + + +def test_stuck_table_freezes_after_stable_cycles(): + watch = BoschTableFreezeWatch(stable_cycles=3, min_points=4) + points = _pts((10, 0, 0), (20, 1, 0), (30, -1, 0), (40, 0.5, 0)) + assert watch.update(points) is False + assert watch.update(points) is False + assert watch.update(points) is True + + +def test_too_few_points_never_freezes(): + watch = BoschTableFreezeWatch(stable_cycles=2, min_points=4) + points = _pts((10, 0, 0), (20, 1, 0)) + assert watch.update(points) is False + assert watch.update(points) is False diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index d24dfef31df..dbc10c28a4a 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -11,6 +11,11 @@ except ImportError: nap_conf = None +try: + from opendbc.car.tesla.preap.radar_table_freeze import BoschTableFreezeWatch +except ImportError: + BoschTableFreezeWatch = None + BOSCH_POINT_BASE_ADDRESS = 0x310 BOSCH_POINT_ADDRESS_STRIDE = 3 @@ -18,6 +23,39 @@ BOSCH_TRACK_MAX_DISTANCE_DELTA_M = 10.0 BOSCH_TRACK_MAX_VELOCITY_DELTA_MPS = 10.0 +IGNORE_HW_FAIL_PARAM = "NAPRadarIgnoreHwFail" +IGNORE_HW_FAIL_PATH = "/data/params/d/NAPRadarIgnoreHwFail" + + +def _ignore_hw_fail_from_params(): + from openpilot.common.params import Params + return bool(Params().get_bool(IGNORE_HW_FAIL_PARAM)) + + +def _ignore_hw_fail_from_file(path=None): + with open(path or IGNORE_HW_FAIL_PATH, "rb") as f: + return f.read().strip() == b"1" + + +def _resolve_ignore_hw_fail(): + """Live NAPRadarIgnoreHwFail. Exceptions never force False if the file is 1.""" + try: + if nap_conf is not None and bool(nap_conf.radar_ignore_hw_fail): + return True + except Exception: + pass + try: + if _ignore_hw_fail_from_params(): + return True + except Exception: + pass + try: + if _ignore_hw_fail_from_file(): + return True + except Exception: + pass + return False + @dataclass(frozen=True) class BoschTrackObservation: @@ -118,12 +156,14 @@ def __init__(self, CP): self.updated_messages = set() self.track_id = 0 self.bosch_tracks = BoschTrackLifecycle() + self.table_freeze = BoschTableFreezeWatch() if BoschTableFreezeWatch is not None else None # Keep parity with Tinkla radar lateral alignment behavior. # For behind-nosecone installs, users can configure horizontal offset in meters. if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP and nap_conf is not None: self.radar_offset = float(nap_conf.radar_offset) else: self.radar_offset = 0.0 + self.ignore_hw_fail = _resolve_ignore_hw_fail() def update(self, can_msgs): @@ -141,6 +181,11 @@ def update(self, can_msgs): if self.rcp is None: return ret + # Re-read every cycle: compiled params_pyx may not know the new key, + # and the on-disk param is the source of truth when that throws. + ignore_hw_fail = _resolve_ignore_hw_fail() + self.ignore_hw_fail = ignore_hw_fail + # Errors if not self.rcp.can_valid: ret.errors.canError = True @@ -155,8 +200,18 @@ def update(self, can_msgs): ret.errors.radarFault = True elif self.bosch_radar: radar_status = self.rcp.vl['TeslaRadarSguInfo'] - if radar_status['RADC_HWFail'] or radar_status['RADC_SGUFail']: + # SGUFail is a summary lamp: calibration, VIN reject, dirty-behind- + # nosecone, and worse. Tinkla ignored it on Pre-AP. IRITable's unit + # raised it for the adjustment triad while tracks were still live. + # Driving is how that calibration clears, so do not block engage. + # HWFail is the dead-sensor bit and still blocks, unless + # NAPRadarIgnoreHwFail is set (tracks can stay live on a false HWFail). + # Frozen+SGUFail is stock-safe radarFault, but Ignore drops those + # ghost tracks instead of bricking engage. + if radar_status['RADC_HWFail'] and not ignore_hw_fail: ret.errors.radarFault = True + if self.table_freeze is not None: + self.table_freeze.reset() if radar_status['RADC_SensorDirty']: ret.errors.radarUnavailableTemporary = True @@ -199,6 +254,14 @@ def update(self, can_msgs): self.pts[i].measured = bool(msg_a['Meas']) ret.points = self.bosch_tracks.points if self.bosch_radar else list(self.pts.values()) + if self.bosch_radar and self.table_freeze is not None: + frozen = self.table_freeze.update(ret.points) + if frozen and bool(self.rcp.vl['TeslaRadarSguInfo']['RADC_SGUFail']): + if ignore_hw_fail: + # Stay engageable; never publish the frozen ghost table as leads. + ret.points = [] + else: + ret.errors.radarFault = True self.updated_messages.clear() return ret diff --git a/opendbc/car/tesla/tests/test_radar_interface.py b/opendbc/car/tesla/tests/test_radar_interface.py index 1ba3cc60d87..17589943426 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -3,6 +3,7 @@ import pytest from opendbc.car.tesla import radar_interface as radar_interface_module +from opendbc.car.tesla.preap.radar_table_freeze import BoschTableFreezeWatch BOSCH_POINT_A_ADDRESS = 0x310 @@ -64,6 +65,7 @@ def _make_bosch_interface(): radar.pts = {} if hasattr(radar_interface_module, "BoschTrackLifecycle"): radar.bosch_tracks = radar_interface_module.BoschTrackLifecycle() + radar.table_freeze = BoschTableFreezeWatch(stable_cycles=3, min_points=4) radar.rcp = FakeRadarParser() return radar @@ -77,6 +79,14 @@ def _run_cycle(radar, *, tracked=True, d_rel=40.0, v_rel=-2.0, slot_updated=True return radar.update([]) +def _run_frozen_table(radar): + radar.rcp.updated_addresses = {BOSCH_TRIGGER_ADDRESS} + for slot in range(4): + addr = 0x310 + slot * 3 + radar.rcp.updated_addresses.update((addr, addr + 1)) + return radar.update([]) + + class TestBoschSlotLifecycle: def test_one_missing_frame_coasts_as_unmeasured(self): radar = _make_bosch_interface() @@ -135,7 +145,7 @@ class TestBoschHealth: ("signal", "radar_fault", "temporary_unavailable"), ( ("RADC_HWFail", True, False), - ("RADC_SGUFail", True, False), + ("RADC_SGUFail", False, False), ("RADC_SensorDirty", False, True), ), ) @@ -165,3 +175,130 @@ def test_unknown_alert_bit_is_diagnostic_only(self): assert result.errors.radarFault is False assert result.errors.radarUnavailableTemporary is False + + def test_sgufail_with_frozen_table_is_radar_fault(self): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_SGUFail"] = 1 + for slot in range(4): + radar.rcp.vl[f"RadarPoint{slot}_A"] = _point_a(d_rel=10.0 + slot * 5, v_rel=0.0) + radar.rcp.vl[f"RadarPoint{slot}_B"] = _point_b() + + assert _run_frozen_table(radar).errors.radarFault is False + assert _run_frozen_table(radar).errors.radarFault is False + assert _run_frozen_table(radar).errors.radarFault is True + + def test_frozen_table_without_sgufail_is_not_fault(self): + radar = _make_bosch_interface() + for slot in range(4): + radar.rcp.vl[f"RadarPoint{slot}_A"] = _point_a(d_rel=10.0 + slot * 5, v_rel=0.0) + radar.rcp.vl[f"RadarPoint{slot}_B"] = _point_b() + + assert _run_frozen_table(radar).errors.radarFault is False + assert _run_frozen_table(radar).errors.radarFault is False + assert _run_frozen_table(radar).errors.radarFault is False + + def test_ignore_hw_fail_param_does_not_set_radar_fault(self, monkeypatch): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 + monkeypatch.setattr(radar_interface_module, "nap_conf", SimpleNamespace(radar_ignore_hw_fail=True)) + + result = _run_cycle(radar) + + assert result.errors.radarFault is False + assert result.errors.radarUnavailableTemporary is False + + def test_ignore_hw_fail_leaves_sensor_dirty_temp_unavailable(self, monkeypatch): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_SensorDirty"] = 1 + monkeypatch.setattr(radar_interface_module, "nap_conf", SimpleNamespace(radar_ignore_hw_fail=True)) + + result = _run_cycle(radar) + + assert result.errors.radarFault is False + assert result.errors.radarUnavailableTemporary is True + + def test_ignore_hw_fail_drops_frozen_sgufail_tracks_without_fault(self, monkeypatch): + radar = _make_bosch_interface() + radar.table_freeze = BoschTableFreezeWatch() + monkeypatch.setattr(radar_interface_module, "nap_conf", SimpleNamespace(radar_ignore_hw_fail=True)) + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_SGUFail"] = 1 + for slot in range(4): + radar.rcp.vl[f"RadarPoint{slot}_A"] = _point_a(d_rel=10.0 + slot * 5, v_rel=0.0) + radar.rcp.vl[f"RadarPoint{slot}_B"] = _point_b() + + for _ in range(39): + early = _run_frozen_table(radar) + assert early.errors.radarFault is False # HWFail is ignored; freeze not latched yet. + result = _run_frozen_table(radar) + assert result.errors.radarFault is False + assert len(result.points) == 0 + + def test_frozen_sgufail_still_faults_when_ignore_hw_fail_off(self, monkeypatch): + radar = _make_bosch_interface() + radar.table_freeze = BoschTableFreezeWatch() + monkeypatch.setattr(radar_interface_module, "nap_conf", SimpleNamespace(radar_ignore_hw_fail=False)) + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_SGUFail"] = 1 + for slot in range(4): + radar.rcp.vl[f"RadarPoint{slot}_A"] = _point_a(d_rel=10.0 + slot * 5, v_rel=0.0) + radar.rcp.vl[f"RadarPoint{slot}_B"] = _point_b() + + for _ in range(39): + assert _run_frozen_table(radar).errors.radarFault is False + assert _run_frozen_table(radar).errors.radarFault is True + + def test_ignore_hw_fail_is_resolved_live_each_update(self, monkeypatch): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 + ns = SimpleNamespace(radar_ignore_hw_fail=False) + monkeypatch.setattr(radar_interface_module, "nap_conf", ns) + + assert _run_cycle(radar).errors.radarFault is True + ns.radar_ignore_hw_fail = True + assert _run_cycle(radar).errors.radarFault is False + + def test_ignore_hw_fail_file_fallback_when_sources_raise(self, monkeypatch, tmp_path): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 + param_file = tmp_path / "NAPRadarIgnoreHwFail" + param_file.write_bytes(b"1") + + class _MissingKey: + @property + def radar_ignore_hw_fail(self): + raise AttributeError("unknown key") + + monkeypatch.setattr(radar_interface_module, "nap_conf", _MissingKey()) + monkeypatch.setattr( + radar_interface_module, + "_ignore_hw_fail_from_params", + lambda: (_ for _ in ()).throw(RuntimeError("params_pyx")), + ) + monkeypatch.setattr(radar_interface_module, "IGNORE_HW_FAIL_PATH", str(param_file)) + + result = _run_cycle(radar) + + assert result.errors.radarFault is False + + def test_ignore_hw_fail_file_zero_does_not_ignore(self, monkeypatch, tmp_path): + radar = _make_bosch_interface() + radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 + param_file = tmp_path / "NAPRadarIgnoreHwFail" + param_file.write_bytes(b"0") + + class _MissingKey: + @property + def radar_ignore_hw_fail(self): + raise AttributeError("unknown key") + + monkeypatch.setattr(radar_interface_module, "nap_conf", _MissingKey()) + monkeypatch.setattr( + radar_interface_module, + "_ignore_hw_fail_from_params", + lambda: (_ for _ in ()).throw(RuntimeError("params_pyx")), + ) + monkeypatch.setattr(radar_interface_module, "IGNORE_HW_FAIL_PATH", str(param_file)) + + result = _run_cycle(radar) + + assert result.errors.radarFault is True diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index 3e39f40fcac..4f5c19f26f4 100644 --- a/opendbc/safety/modes/tesla_preap.h +++ b/opendbc/safety/modes/tesla_preap.h @@ -77,6 +77,7 @@ void can_set_checksum(CANPacket_t *packet); #define PREAP_FLAG_ENABLE_PEDAL 1U #define PREAP_FLAG_RADAR_EMULATION 2U +// Leftover bit. Position comes from host 0x560, not this flag. #define PREAP_FLAG_RADAR_BEHIND_NOSECONE 4U #define PREAP_HANDS_ON_DISENGAGE_LEVEL 2 @@ -86,7 +87,6 @@ void can_set_checksum(CANPacket_t *packet); static bool preap_enable_pedal = false; static bool preap_radar_emulation = false; -static bool preap_radar_behind_nosecone = false; static int preap_pedal_can = -1; @@ -106,6 +106,107 @@ static int preap_radar_status = 0; static uint32_t preap_last_radar_signal = 0; static int preap_radar_epas_type = 0; static int preap_radar_position = 0; +static uint8_t preap_radar_vin[17]; +static uint8_t preap_radar_vin_complete = 0; +static bool preap_radar_should_send = false; + +// Host→panda donor config. 0x560 never goes on the car; tesla_preap_tx_hook +// consumes it. Layout matches Tinkla 0.6.6 create_radar_VIN_msg. +#define PREAP_RADAR_VIN_ADDR 0x560U +#define PREAP_RADAR_UDS_ADDR 0x641U + +static bool preap_f190_payload_allowed(const CANPacket_t *msg) { + static const uint8_t tester[8] = {0x02U, 0x3EU, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + static const uint8_t default_session[8] = {0x02U, 0x10U, 0x01U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + static const uint8_t extended_session[8] = {0x02U, 0x10U, 0x03U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + static const uint8_t read_f190[8] = {0x03U, 0x22U, 0xF1U, 0x90U, 0x00U, 0x00U, 0x00U, 0x00U}; + static const uint8_t flow_control[8] = {0x30U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + static const uint8_t cleanup_marker[8] = {0x02U, 0x3EU, 0x80U, 0x00U, 0x00U, 0x00U, 0x00U, 0x00U}; + const uint8_t *allowed[] = {tester, default_session, extended_session, read_f190, flow_control, cleanup_marker}; + if (GET_LEN(msg) != 8U) { + return false; + } + for (unsigned int i = 0U; i < (sizeof(allowed) / sizeof(allowed[0])); i++) { + bool match = true; + for (int b = 0; b < 8; b++) { + if (msg->data[b] != allowed[i][b]) { + match = false; + break; + } + } + if (match) { + return true; + } + } + return false; +} + +static bool preap_f190_tx_ok(const CANPacket_t *msg) { + // Read-only F190 on the radar bus. Never writes, routines, or security. + if (!preap_radar_emulation || controls_allowed) { + return false; + } + if (GET_BUS(msg) != 1U) { + return false; + } + return preap_f190_payload_allowed(msg); +} + +static uint32_t preap_radar_vin_char(int pos, int shift) { + return ((uint32_t)preap_radar_vin[pos]) << (shift * 8); +} + +static bool preap_radar_ready(void) { + // Tinkla 0.6.6 sent no radar-bus GTW frames until the host 0x560 + // stream was complete and useRadar was set. Talking earlier lets the + // radar hear this-car VIN/2WD/EPAS0, then a different contract a + // second later. + return (preap_radar_vin_complete == 7U) && preap_radar_should_send; +} + +static bool preap_radar_donor_active(void) { + if (preap_radar_vin_complete != 7U) { + return false; + } + // 0.6.6 default was 17 spaces. Treat that as "use this car." + for (int i = 0; i < 17; i++) { + if ((preap_radar_vin[i] != 0U) && (preap_radar_vin[i] != (uint8_t)' ')) { + return true; + } + } + return false; +} + +static void preap_apply_radar_vin_msg(const CANPacket_t *msg) { + const int rec = msg->data[0]; + if (rec == 0) { + preap_radar_should_send = (msg->data[2] & 0x01U) != 0U; + preap_radar_position = (msg->data[2] >> 1) & 0x03; + preap_radar_epas_type = (msg->data[2] >> 3) & 0x07; + preap_radar_vin[0] = msg->data[5]; + preap_radar_vin[1] = msg->data[6]; + preap_radar_vin[2] = msg->data[7]; + preap_radar_vin_complete |= 1U; + } else if (rec == 1) { + preap_radar_vin[3] = msg->data[1]; + preap_radar_vin[4] = msg->data[2]; + preap_radar_vin[5] = msg->data[3]; + preap_radar_vin[6] = msg->data[4]; + preap_radar_vin[7] = msg->data[5]; + preap_radar_vin[8] = msg->data[6]; + preap_radar_vin[9] = msg->data[7]; + preap_radar_vin_complete |= 2U; + } else if (rec == 2) { + preap_radar_vin[10] = msg->data[1]; + preap_radar_vin[11] = msg->data[2]; + preap_radar_vin[12] = msg->data[3]; + preap_radar_vin[13] = msg->data[4]; + preap_radar_vin[14] = msg->data[5]; + preap_radar_vin[15] = msg->data[6]; + preap_radar_vin[16] = msg->data[7]; + preap_radar_vin_complete |= 4U; + } +} // ============================================ // Checksum and counter (for EPAS validation) @@ -210,6 +311,40 @@ static void preap_transform_radar_car_config(const CANPacket_t *src, CANPacket_t uint32_t hi = PREAP_GET_BYTES_48(src); lo = (lo & 0xFFFFF33F) | 0x100 | 0x440; // country=US, radar_type=Bosch hi = (hi & 0xCFFF0F0F) | 0x10000000 | (preap_radar_position << 4) | (preap_radar_epas_type << 12); + // Bosch xWD checks 0x2A9 against the VIN on 0x2B9, not chassis 0x398. + // Tesla char 8 '2'/'4' is dual motor. This car VIN 5YJSA1E25FF106153 is + // '2'; GTW still declares 2WD. Honest 2WD then latches xwdValidity and + // freezes the object table (~12 s). Tinkla 0.6.6 ORed 4WD from that char. + // Routes 8/9 with the OR kept a live table. Empty donor leaves 0x398 xWD + // (old A, char 8 '1', 2WD, rock-solid). + if (preap_radar_donor_active()) { + const uint8_t drive = preap_radar_vin[7]; + if ((drive == (uint8_t)'2') || (drive == (uint8_t)'4')) { + lo |= 0x08U; + } + } + PREAP_WORD_TO_BYTES(&dst->data[0], lo); + PREAP_WORD_TO_BYTES(&dst->data[4], hi); +} + +static void preap_transform_radar_vin_feed(const CANPacket_t *src, CANPacket_t *dst) { + *dst = (CANPacket_t){.returned = 0U, .rejected = 0U, .extended = src->extended, + .bus = 1, .addr = 0x2B9, .data_len_code = src->data_len_code}; + uint32_t lo = PREAP_GET_BYTES_04(src); + uint32_t hi = PREAP_GET_BYTES_48(src); + if (preap_radar_donor_active() && ((lo & 0x10U) == 0x10U)) { + const int rec = (int)(lo & 0xFFU); + if (rec == 0x10) { + lo = (uint32_t)rec; + hi = preap_radar_vin_char(0, 1) | preap_radar_vin_char(1, 2) | preap_radar_vin_char(2, 3); + } else if (rec == 0x11) { + lo = (uint32_t)rec | preap_radar_vin_char(3, 1) | preap_radar_vin_char(4, 2) | preap_radar_vin_char(5, 3); + hi = preap_radar_vin_char(6, 0) | preap_radar_vin_char(7, 1) | preap_radar_vin_char(8, 2) | preap_radar_vin_char(9, 3); + } else if (rec == 0x12) { + lo = (uint32_t)rec | preap_radar_vin_char(10, 1) | preap_radar_vin_char(11, 2) | preap_radar_vin_char(12, 3); + hi = preap_radar_vin_char(13, 0) | preap_radar_vin_char(14, 1) | preap_radar_vin_char(15, 2) | preap_radar_vin_char(16, 3); + } + } PREAP_WORD_TO_BYTES(&dst->data[0], lo); PREAP_WORD_TO_BYTES(&dst->data[4], hi); } @@ -217,6 +352,8 @@ static void preap_transform_radar_car_config(const CANPacket_t *src, CANPacket_t #if defined(ALLOW_DEBUG) && !defined(STM32H7) && !defined(STM32F4) static bool preap_radar_car_config_captured = false; static CANPacket_t preap_radar_car_config_capture; +static bool preap_radar_vin_feed_captured = false; +static CANPacket_t preap_radar_vin_feed_capture; #endif // ============================================ @@ -227,7 +364,7 @@ static void tesla_preap_gtw_emulation(const CANPacket_t *to_fwd) { int bus_num = GET_BUS(to_fwd); int addr = GET_ADDR(to_fwd); - if (bus_num == 0 && preap_radar_emulation) { + if (bus_num == 0 && preap_radar_emulation && preap_radar_ready()) { // Group A: Simple re-addresses switch (addr) { case 0x45: preap_radar_readdr(to_fwd, 0x219); break; // STW_ACTN_RQ @@ -236,10 +373,22 @@ static void tesla_preap_gtw_emulation(const CANPacket_t *to_fwd) { case 0x20A: preap_radar_readdr(to_fwd, 0x159); break; // BrakeMessage -> ESP_C case 0x308: preap_radar_readdr(to_fwd, 0x209); break; // GTW_odo case 0x30A: preap_radar_readdr(to_fwd, 0x2D9); break; // BC_status - case 0x405: preap_radar_readdr(to_fwd, 0x2B9); break; // VIP_405HS default: break; } + if (addr == 0x405) { + CANPacket_t vin_pkt; + preap_transform_radar_vin_feed(to_fwd, &vin_pkt); +#if defined(ALLOW_DEBUG) && !defined(STM32H7) && !defined(STM32F4) + preap_radar_vin_feed_capture = vin_pkt; + preap_radar_vin_feed_captured = true; +#endif +#if defined(STM32H7) || defined(STM32F4) + can_set_checksum(&vin_pkt); + can_send(&vin_pkt, 1, true); +#endif + } + // Group B: GTW_carConfig (0x398) → 0x2A9 with bitfield patching if (addr == 0x398) { CANPacket_t pkt; @@ -358,6 +507,25 @@ uint8_t tesla_preap_radar_car_config_data(int index) { } return preap_radar_car_config_capture.data[index]; } + +bool tesla_preap_radar_vin_feed_captured(void) { + return preap_radar_vin_feed_captured; +} + +uint8_t tesla_preap_radar_vin_feed_data(int index) { + if ((index < 0) || (index >= 8)) { + return 0U; + } + return preap_radar_vin_feed_capture.data[index]; +} + +bool tesla_preap_radar_donor_active_debug(void) { + return preap_radar_donor_active(); +} + +bool tesla_preap_radar_ready_debug(void) { + return preap_radar_ready(); +} #endif // ============================================ @@ -511,6 +679,17 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) { bool tx = true; bool violation = false; + // Host→panda donor VIN/config. Intercept; do not put 0x560 on the car. + if (msg->addr == PREAP_RADAR_VIN_ADDR) { + preap_apply_radar_vin_msg(msg); + return false; + } + + // Radar UDS on bus 1. Allow only the F190 read sequence while disengaged. + if (msg->addr == PREAP_RADAR_UDS_ADDR) { + return preap_f190_tx_ok(msg); + } + // DAS_steeringControl (0x488) if (msg->addr == 0x488U) { int raw_angle_can = ((msg->data[0] & 0x7FU) << 8) | msg->data[1]; @@ -619,7 +798,6 @@ static bool tesla_preap_fwd_hook(int bus_num, int addr) { static safety_config tesla_preap_init(uint16_t param) { preap_enable_pedal = GET_FLAG(param, PREAP_FLAG_ENABLE_PEDAL); preap_radar_emulation = GET_FLAG(param, PREAP_FLAG_RADAR_EMULATION); - preap_radar_behind_nosecone = GET_FLAG(param, PREAP_FLAG_RADAR_BEHIND_NOSECONE); preap_gear = 4; preap_gear_prev = 4; @@ -630,9 +808,16 @@ static safety_config tesla_preap_init(uint16_t param) { preap_radar_status = 0; preap_last_radar_signal = 0; preap_last_stalk_engage_us = 0; - preap_radar_position = preap_radar_behind_nosecone ? 1 : 0; + preap_radar_position = 0; + preap_radar_epas_type = 0; + preap_radar_vin_complete = 0; + preap_radar_should_send = false; + for (int i = 0; i < 17; i++) { + preap_radar_vin[i] = (uint8_t)' '; + } #if defined(ALLOW_DEBUG) && !defined(STM32H7) && !defined(STM32F4) preap_radar_car_config_captured = false; + preap_radar_vin_feed_captured = false; #endif // TX whitelist — no harness relay on Pre-AP @@ -644,6 +829,8 @@ static safety_config tesla_preap_init(uint16_t param) { {0x551, 2, 6, .check_relay = false, .disable_static_blocking = true}, // Pedal on bus 2 {0x45, 0, 8, .check_relay = false, .disable_static_blocking = true}, // STW_ACTN_RQ (stalk spoof) {0x3E9, 0, 8, .check_relay = false, .disable_static_blocking = true}, // DAS_bodyControls (turn signal) + {0x560, 0, 8, .check_relay = false, .disable_static_blocking = true}, // donor VIN/config to panda + {0x641, 1, 8, .check_relay = false, .disable_static_blocking = true}, // radar F190 read }; // RX checks — disable EPAS counter/checksum until we verify the Pre-AP diff --git a/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc/safety/tests/libsafety/libsafety_py.py index 989502635aa..2e9ef72ad8e 100644 --- a/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc/safety/tests/libsafety/libsafety_py.py @@ -77,6 +77,10 @@ class CANPacket: uint8_t tesla_preap_radar_car_config_bus(void); uint8_t tesla_preap_radar_car_config_dlc(void); uint8_t tesla_preap_radar_car_config_data(int index); +bool tesla_preap_radar_vin_feed_captured(void); +uint8_t tesla_preap_radar_vin_feed_data(int index); +bool tesla_preap_radar_donor_active_debug(void); +bool tesla_preap_radar_ready_debug(void); void set_honda_fwd_brake(bool c); bool get_honda_fwd_brake(void); diff --git a/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py b/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py index 015a8f87c35..048616f3356 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py @@ -1,11 +1,11 @@ #!/usr/bin/env python3 from opendbc.car.structs import CarParams +from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP from opendbc.safety import DLC_TO_LEN from opendbc.safety.tests.libsafety import libsafety_py PREAP_FLAG_RADAR_EMULATION = 2 -PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 class TestTeslaPreAPRadarCarConfig: @@ -15,16 +15,14 @@ def setup_method(self): self.safety = libsafety_py.libsafety def _set_safety_hooks(self, radar_emulation): - radar_flags = PREAP_FLAG_RADAR_BEHIND_NOSECONE - if radar_emulation: - radar_flags |= PREAP_FLAG_RADAR_EMULATION + radar_flags = PREAP_FLAG_RADAR_EMULATION if radar_emulation else 0 self.safety.set_safety_hooks(CarParams.SafetyModel.teslaPreap, radar_flags) self.safety.init_tests() def test_car_config_emulation_preserves_application_data(self): test_cases = ( - (bytes.fromhex("0290555300001700"), bytes.fromhex("4295555310001710")), - (bytes.fromhex("0281555300000000"), bytes.fromhex("4285555310000010")), + (bytes.fromhex("0290555300001700"), bytes.fromhex("4295555300001710")), + (bytes.fromhex("0281555300000000"), bytes.fromhex("4285555300000010")), ) actual_payloads = [] expected_payloads = [] @@ -34,6 +32,9 @@ def test_car_config_emulation_preserves_application_data(self): assert not self.safety.tesla_preap_radar_car_config_captured() self._set_safety_hooks(True) + for fragment in range(3): + _addr, dat, _bus = TeslaCANPreAP.create_radar_vin_msg(fragment, "", True, 0, 0) + self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x560, 0, dat)) for source_data, expected_data in test_cases: source = libsafety_py.make_CANPacket(0x398, 0, source_data) self.safety.safety_rx_hook(source) diff --git a/opendbc/safety/tests/test_tesla_preap_radar_donor.py b/opendbc/safety/tests/test_tesla_preap_radar_donor.py new file mode 100644 index 00000000000..65dc359437b --- /dev/null +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -0,0 +1,106 @@ +from opendbc.car.structs import CarParams +from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP +from opendbc.safety.tests.libsafety import libsafety_py + + +PREAP_FLAG_RADAR_EMULATION = 2 + +AWD_VIN = "5YJSA1E42FF156789" # character 8 is '4' +JACK_RWD_VIN = "5YJSA1E25FF106153" # character 8 is '2' (Tesla dual-motor) +OLD_A_RWD_VIN = "5YJSA1H13EFP20460" # character 8 is '1' (old A daily lock) + + +def _payload(safety, getter): + return bytes(getter(i) for i in range(8)) + + +def _send_donor(safety, vin, position=0, epas_type=1): + for fragment in range(3): + _addr, dat, _bus = TeslaCANPreAP.create_radar_vin_msg(fragment, vin, True, position, epas_type) + allowed = safety.safety_tx_hook(libsafety_py.make_CANPacket(0x560, 0, dat)) + assert allowed is False + + +class TestTeslaPreAPRadarDonor: + TX_MSGS = [] + + def setup_method(self): + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.teslaPreap, PREAP_FLAG_RADAR_EMULATION) + self.safety.init_tests() + + def test_gtw_silent_until_vin_stream_complete(self): + source = bytes.fromhex("0281555300000000") + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, source)) + assert self.safety.tesla_preap_radar_car_config_captured() is False + assert self.safety.tesla_preap_radar_ready_debug() is False + + _addr, dat, _bus = TeslaCANPreAP.create_radar_vin_msg(0, "", True, 0, 0) + self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x560, 0, dat)) + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, source)) + assert self.safety.tesla_preap_radar_car_config_captured() is False + assert self.safety.tesla_preap_radar_ready_debug() is False + + _send_donor(self.safety, "", position=0, epas_type=0) + assert self.safety.tesla_preap_radar_ready_debug() is True + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, source)) + assert self.safety.tesla_preap_radar_car_config_captured() is True + + def test_use_radar_clear_keeps_gtw_silent(self): + for fragment in range(3): + _addr, dat, _bus = TeslaCANPreAP.create_radar_vin_msg(fragment, AWD_VIN, False, 0, 0) + self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x560, 0, dat)) + assert self.safety.tesla_preap_radar_ready_debug() is False + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0281555300000000"))) + assert self.safety.tesla_preap_radar_car_config_captured() is False + + def test_empty_vin_keeps_passthrough(self): + _send_donor(self.safety, "", position=0, epas_type=0) + source = bytes.fromhex("0281555300000000") + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, source)) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4285555300000010") + assert self.safety.tesla_preap_radar_donor_active_debug() is False + assert self.safety.tesla_preap_radar_ready_debug() is True + + def test_empty_vin_still_applies_position(self): + _send_donor(self.safety, "", position=1, epas_type=0) + assert self.safety.tesla_preap_radar_donor_active_debug() is False + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0281555300000000"))) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4285555310000010") + + def test_awd_donor_vin_sets_4wd_to_match_vin_char(self): + _send_donor(self.safety, AWD_VIN, position=1, epas_type=3) + assert self.safety.tesla_preap_radar_donor_active_debug() is True + + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0281555300000000"))) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4a85555310300010") + + def test_awd_source_config_remains_four_wheel_drive(self): + _send_donor(self.safety, AWD_VIN, position=0, epas_type=0) + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0a90555300001700"))) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4a95555300001710") + + def test_this_car_vin_char2_sets_4wd_matching_tinkla(self): + # 5YJSA1E25FF106153 char 8 is '2' (Tesla dual-motor encoding). Honest + # chassis 2WD against that VIN is the 1d/11/14/15 freeze (xwdValidity). + _send_donor(self.safety, JACK_RWD_VIN, position=0, epas_type=0) + assert self.safety.tesla_preap_radar_donor_active_debug() is True + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0290555300001700"))) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4a95555300001710") + + def test_rwd_single_motor_vin_preserves_chassis_2wd(self): + # Old A daily lock: char 8 '1', empty-style 2WD declaration. + _send_donor(self.safety, OLD_A_RWD_VIN, position=0, epas_type=0) + assert self.safety.tesla_preap_radar_donor_active_debug() is True + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x398, 0, bytes.fromhex("0290555300001700"))) + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4295555300001710") + + def test_donor_vin_replaces_mux_records(self): + _send_donor(self.safety, AWD_VIN) + mux = bytes([0x11, 0, 0, 0, 0, 0, 0, 0]) + self.safety.safety_rx_hook(libsafety_py.make_CANPacket(0x405, 0, mux)) + assert self.safety.tesla_preap_radar_vin_feed_captured() is True + dat = _payload(self.safety, self.safety.tesla_preap_radar_vin_feed_data) + assert dat[0] == 0x11 + assert dat[1:4] == b"SA1" + assert dat[4:8] == b"E42F" diff --git a/opendbc/safety/tests/test_tesla_preap_radar_f190.py b/opendbc/safety/tests/test_tesla_preap_radar_f190.py new file mode 100644 index 00000000000..fc61e52c869 --- /dev/null +++ b/opendbc/safety/tests/test_tesla_preap_radar_f190.py @@ -0,0 +1,50 @@ +from opendbc.car.structs import CarParams +from opendbc.safety.tests.libsafety import libsafety_py + + +PREAP_FLAG_RADAR_EMULATION = 2 + +F190 = bytes([0x03, 0x22, 0xF1, 0x90, 0x00, 0x00, 0x00, 0x00]) +TESTER = bytes([0x02, 0x3E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00]) +OTHER_DID = bytes([0x03, 0x22, 0xF0, 0x14, 0x00, 0x00, 0x00, 0x00]) +SECURITY = bytes([0x02, 0x27, 0x11, 0x00, 0x00, 0x00, 0x00, 0x00]) +ROUTINE = bytes([0x04, 0x31, 0x01, 0x0A, 0x03, 0x00, 0x00, 0x00]) + + +class TestTeslaPreAPRadarF190: + TX_MSGS = [] + + def setup_method(self): + self.safety = libsafety_py.libsafety + self.safety.set_safety_hooks(CarParams.SafetyModel.teslaPreap, PREAP_FLAG_RADAR_EMULATION) + self.safety.init_tests() + + def test_allows_f190_read_while_disengaged(self): + self.safety.set_controls_allowed(0) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, TESTER)) is True + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, F190)) is True + + def test_rejects_f190_when_controls_allowed(self): + self.safety.set_controls_allowed(1) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, F190)) is False + + def test_rejects_f190_without_radar(self): + self.safety.set_safety_hooks(CarParams.SafetyModel.teslaPreap, 0) + self.safety.init_tests() + self.safety.set_controls_allowed(0) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, F190)) is False + + def test_rejects_writes_routines_and_other_dids(self): + self.safety.set_controls_allowed(0) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, OTHER_DID)) is False + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, SECURITY)) is False + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 1, ROUTINE)) is False + + def test_rejects_uds_on_chassis_bus(self): + self.safety.set_controls_allowed(0) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x641, 0, F190)) is False + + def test_560_still_never_leaves_host(self): + self.safety.set_controls_allowed(0) + dat = bytes([0, 1, 1, 0, 0, ord("5"), ord("Y"), ord("J")]) + assert self.safety.safety_tx_hook(libsafety_py.make_CANPacket(0x560, 0, dat)) is False