From 347ac59b236e710d33de8909b15f4135da9fcaea Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Fri, 21 Aug 2026 21:00:59 +0000 Subject: [PATCH 01/12] tesla_preap: impersonate a salvage radar's VIN New radars still hold the donor VIN and reject this car. Read F190 when vinValidity sticks, then put that VIN on the GTW feed. Empty VIN keeps current passthrough. Co-authored-by: Cursor --- opendbc/car/tesla/carcontroller.py | 12 + opendbc/car/tesla/preap/nap_conf.py | 66 ++++ opendbc/car/tesla/preap/nap_params.py | 6 + opendbc/car/tesla/preap/radar_donor_vin.py | 335 ++++++++++++++++++ opendbc/car/tesla/preap/teslacan.py | 16 + .../preap/tests/test_radar_donor_contract.py | 27 ++ .../tesla/preap/tests/test_radar_donor_vin.py | 167 +++++++++ opendbc/safety/modes/tesla_preap.h | 169 ++++++++- .../safety/tests/libsafety/libsafety_py.py | 3 + .../tests/test_tesla_preap_radar_donor.py | 59 +++ .../tests/test_tesla_preap_radar_f190.py | 50 +++ 11 files changed, 909 insertions(+), 1 deletion(-) create mode 100644 opendbc/car/tesla/preap/radar_donor_vin.py create mode 100644 opendbc/car/tesla/preap/tests/test_radar_donor_contract.py create mode 100644 opendbc/car/tesla/preap/tests/test_radar_donor_vin.py create mode 100644 opendbc/safety/tests/test_tesla_preap_radar_donor.py create mode 100644 opendbc/safety/tests/test_tesla_preap_radar_f190.py diff --git a/opendbc/car/tesla/carcontroller.py b/opendbc/car/tesla/carcontroller.py index 7724a22c1f3..858b07432d5 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,16 @@ 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 to panda on 0x560. + # Empty VIN keeps current passthrough (this car). A 17-char VIN makes panda + # impersonate the radar's donor. + donor_vin = nap_conf.radar_donor_vin + if nap_conf.radar_enabled and donor_vin and self.frame % 100 == 0: + can_sends.append(self.tesla_can.create_radar_vin_msg( + self.radar_vin_idx, 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/nap_conf.py b/opendbc/car/tesla/preap/nap_conf.py index 1619fc125bb..53ac8ee79ad 100644 --- a/opendbc/car/tesla/preap/nap_conf.py +++ b/opendbc/car/tesla/preap/nap_conf.py @@ -35,8 +35,20 @@ 'radar_enabled': 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 +161,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 @@ -175,6 +217,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'): diff --git a/opendbc/car/tesla/preap/nap_params.py b/opendbc/car/tesla/preap/nap_params.py index 0f2e138dd13..663d4e63a84 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -26,6 +26,9 @@ class NAPParamKeys: RADAR_ENABLED = "NAPRadarEnabled" RADAR_BEHIND_NOSECONE = "NAPRadarBehindNosecone" RADAR_OFFSET = "NAPRadarOffset" + RADAR_DONOR_VIN = "NAPRadarDonorVin" + RADAR_EPAS_TYPE = "NAPRadarEpasType" + RADAR_POSITION = "NAPRadarPosition" # iBooster / Braking IBOOSTER_ENABLED = "NAPiBoosterEnabled" @@ -50,6 +53,9 @@ class NAPParamKeys: NAPParamKeys.RADAR_ENABLED: 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..92f33717e17 --- /dev/null +++ b/opendbc/car/tesla/preap/radar_donor_vin.py @@ -0,0 +1,335 @@ +"""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: + """Start one F190 read when a parked radar keeps asserting vinValidity.""" + + STABLE_FRAMES = 8 + + def __init__(self, store_vin): + self.store_vin = store_vin + self.reader = RadarDonorVinReader() + self._vin_fault_frames = 0 + self._attempted = False + + def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, + stored_vin: str, controls_allowed: bool, enabled: bool) -> tuple[CanData, ...]: + if not radar_enabled or stored_vin: + 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 self.reader.state == RadarDonorVinState.COMPLETE: + if self.reader.vin: + self.store_vin(self.reader.vin) + return () + if self.reader.state == RadarDonorVinState.FAILED: + return () + if running: + output = self.reader.update(can_packets, now) + if output.state == RadarDonorVinState.COMPLETE and output.vin: + self.store_vin(output.vin) + return output.can_sends + + if 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._attempted = True + self.reader.start(now) + return self.reader.update(can_packets, now).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/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_radar_donor_contract.py b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py new file mode 100644 index 00000000000..df465102062 --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py @@ -0,0 +1,27 @@ +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" 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..a412ef97370 --- /dev/null +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py @@ -0,0 +1,167 @@ +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 == [] diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index 3e39f40fcac..a4caedd7b31 100644 --- a/opendbc/safety/modes/tesla_preap.h +++ b/opendbc/safety/modes/tesla_preap.h @@ -106,6 +106,97 @@ 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; + +// 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_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_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 +301,34 @@ 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); + // Tinkla 0.6.6: VIN character 8 of '2' or '4' is dual-motor. Force 4WD + // on the live 0x2A9 so xWD matches the donor VIN, not this Pre-AP chassis. + if (preap_radar_donor_active() && ((preap_radar_vin[7] == (uint8_t)'2') || + (preap_radar_vin[7] == (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 +336,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 // ============================================ @@ -236,10 +357,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 +491,21 @@ 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(); +} #endif // ============================================ @@ -511,6 +659,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]; @@ -631,8 +790,14 @@ static safety_config tesla_preap_init(uint16_t param) { preap_last_radar_signal = 0; preap_last_stalk_engage_us = 0; preap_radar_position = preap_radar_behind_nosecone ? 1 : 0; + preap_radar_epas_type = 0; + preap_radar_vin_complete = 0; + 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 +809,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..420ccab4170 100644 --- a/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc/safety/tests/libsafety/libsafety_py.py @@ -77,6 +77,9 @@ 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); void set_honda_fwd_brake(bool c); bool get_honda_fwd_brake(void); 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..1c2594f220f --- /dev/null +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -0,0 +1,59 @@ +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 +PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 + +AWD_VIN = "5YJSA1E42FF156789" # character 8 is '4' +RWD_VIN = "5YJSA1E12FF156789" # character 8 is '1' + + +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_empty_vin_keeps_passthrough(self): + 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 + + def test_awd_vin_sets_four_wheel_drive(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"))) + # 0x02 | 0x08 = 0x0A, position nibble 1, EPAS 3 + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4a85555310300010") + + def test_rwd_vin_does_not_force_awd(self): + _send_donor(self.safety, RWD_VIN, position=0, epas_type=0) + 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 From f29a061c7a67ddc9189500a2db04717ee56593f1 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sat, 22 Aug 2026 04:22:37 +0000 Subject: [PATCH 02/12] tesla: force F190 read and ignore SGUFail IRITable never set vinValidity, so auto-read never started. The UI can now request one F190 read. SGUFail was blocking engage on a live uncalibrated radar; HWFail still blocks. Co-authored-by: Cursor --- opendbc/car/tesla/preap/nap_params.py | 1 + opendbc/car/tesla/preap/radar_donor_vin.py | 23 +++++++++++--- .../tesla/preap/tests/test_radar_donor_vin.py | 31 +++++++++++++++++++ opendbc/car/tesla/radar_interface.py | 7 ++++- .../car/tesla/tests/test_radar_interface.py | 2 +- 5 files changed, 58 insertions(+), 6 deletions(-) diff --git a/opendbc/car/tesla/preap/nap_params.py b/opendbc/car/tesla/preap/nap_params.py index 663d4e63a84..c4b1b51d9af 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -29,6 +29,7 @@ class NAPParamKeys: RADAR_DONOR_VIN = "NAPRadarDonorVin" RADAR_EPAS_TYPE = "NAPRadarEpasType" RADAR_POSITION = "NAPRadarPosition" + RADAR_READ_VIN = "NAPRadarReadVin" # iBooster / Braking IBOOSTER_ENABLED = "NAPiBoosterEnabled" diff --git a/opendbc/car/tesla/preap/radar_donor_vin.py b/opendbc/car/tesla/preap/radar_donor_vin.py index 92f33717e17..3753a8e7ef6 100644 --- a/opendbc/car/tesla/preap/radar_donor_vin.py +++ b/opendbc/car/tesla/preap/radar_donor_vin.py @@ -143,7 +143,7 @@ def _uds_frame(payload: bytes) -> CanData: class RadarDonorVinCommissioner: - """Start one F190 read when a parked radar keeps asserting vinValidity.""" + """Read F190 when vinValidity sticks, or when the UI asks once.""" STABLE_FRAMES = 8 @@ -152,10 +152,16 @@ def __init__(self, store_vin): self.reader = RadarDonorVinReader() self._vin_fault_frames = 0 self._attempted = False + self._force_used = False + + @property + def read_finished(self) -> bool: + return self.reader.state in (RadarDonorVinState.COMPLETE, RadarDonorVinState.FAILED) def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, - stored_vin: str, controls_allowed: bool, enabled: bool) -> tuple[CanData, ...]: - if not radar_enabled or stored_vin: + 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 ( @@ -166,6 +172,15 @@ def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, 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._attempted = True + self._vin_fault_frames = 0 + self.reader.start(now) + running = True + if self.reader.state == RadarDonorVinState.COMPLETE: if self.reader.vin: self.store_vin(self.reader.vin) @@ -178,7 +193,7 @@ def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, self.store_vin(output.vin) return output.can_sends - if self._attempted: + 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): diff --git a/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py index a412ef97370..6b71d27c8fe 100644 --- a/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py @@ -165,3 +165,34 @@ def test_commissioner_does_not_touch_existing_donor_vin(): 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 diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index d24dfef31df..d19abfee3d7 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -155,7 +155,12 @@ 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. + if radar_status['RADC_HWFail']: ret.errors.radarFault = True if radar_status['RADC_SensorDirty']: ret.errors.radarUnavailableTemporary = True diff --git a/opendbc/car/tesla/tests/test_radar_interface.py b/opendbc/car/tesla/tests/test_radar_interface.py index 1ba3cc60d87..80c1099630e 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -135,7 +135,7 @@ class TestBoschHealth: ("signal", "radar_fault", "temporary_unavailable"), ( ("RADC_HWFail", True, False), - ("RADC_SGUFail", True, False), + ("RADC_SGUFail", False, False), ("RADC_SensorDirty", False, True), ), ) From f00c8cb99aa4c1bdc0f3afbbab097e1a0e74753d Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sat, 22 Aug 2026 23:37:10 +0000 Subject: [PATCH 03/12] tesla_preap: stream donor position without nosecone Empty VIN used to skip 0x560, so panda wrote position from the leftover behind-nosecone flag. Always stream 0x560 so Donor Position is the nibble, including passthrough. Keep a decoded F190 if cleanup times out, and treat a frozen Bosch table plus SGUFail as radarFault. Co-authored-by: Cursor --- opendbc/car/tesla/carcontroller.py | 12 ++--- opendbc/car/tesla/preap/interface.py | 7 +-- opendbc/car/tesla/preap/nap_params.py | 1 + opendbc/car/tesla/preap/radar_donor_vin.py | 37 ++++++++++----- opendbc/car/tesla/preap/radar_table_freeze.py | 47 +++++++++++++++++++ .../preap/tests/test_longitudinal_tuning.py | 1 - .../preap/tests/test_radar_donor_contract.py | 8 ++++ .../tesla/preap/tests/test_radar_donor_vin.py | 41 ++++++++++++++++ .../preap/tests/test_radar_table_freeze.py | 32 +++++++++++++ opendbc/car/tesla/radar_interface.py | 12 +++++ .../car/tesla/tests/test_radar_interface.py | 37 +++++++++++++++ opendbc/safety/modes/tesla_preap.h | 5 +- .../tests/test_tesla_preap_radar_carconfig.py | 9 ++-- .../tests/test_tesla_preap_radar_donor.py | 7 ++- 14 files changed, 220 insertions(+), 36 deletions(-) create mode 100644 opendbc/car/tesla/preap/radar_table_freeze.py create mode 100644 opendbc/car/tesla/preap/tests/test_radar_table_freeze.py diff --git a/opendbc/car/tesla/carcontroller.py b/opendbc/car/tesla/carcontroller.py index 858b07432d5..4dccc8bb75a 100644 --- a/opendbc/car/tesla/carcontroller.py +++ b/opendbc/car/tesla/carcontroller.py @@ -123,13 +123,13 @@ 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 to panda on 0x560. - # Empty VIN keeps current passthrough (this car). A 17-char VIN makes panda - # impersonate the radar's donor. - donor_vin = nap_conf.radar_donor_vin - if nap_conf.radar_enabled and donor_vin and self.frame % 100 == 0: + # Tinkla 0.6.6 donor contract: always stream VIN/position/EPAS on 0x560 + # when radar is on. Empty VIN is 17 spaces (this-car passthrough); + # position and EPAS still apply. + if nap_conf.radar_enabled and self.frame % 100 == 0: can_sends.append(self.tesla_can.create_radar_vin_msg( - self.radar_vin_idx, donor_vin, True, nap_conf.radar_position, nap_conf.radar_epas_type, + 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 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_params.py b/opendbc/car/tesla/preap/nap_params.py index c4b1b51d9af..d4c5f983efe 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -30,6 +30,7 @@ class NAPParamKeys: RADAR_EPAS_TYPE = "NAPRadarEpasType" RADAR_POSITION = "NAPRadarPosition" RADAR_READ_VIN = "NAPRadarReadVin" + RADAR_VIN_READ_STATUS = "NAPRadarVinReadStatus" # iBooster / Braking IBOOSTER_ENABLED = "NAPiBoosterEnabled" diff --git a/opendbc/car/tesla/preap/radar_donor_vin.py b/opendbc/car/tesla/preap/radar_donor_vin.py index 3753a8e7ef6..e10a28ae881 100644 --- a/opendbc/car/tesla/preap/radar_donor_vin.py +++ b/opendbc/car/tesla/preap/radar_donor_vin.py @@ -153,11 +153,27 @@ def __init__(self, store_vin): 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, ...]: @@ -176,21 +192,15 @@ def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, self._force_used = False elif force_read and not self._force_used and not running: self._force_used = True - self._attempted = True - self._vin_fault_frames = 0 - self.reader.start(now) + self._start_read(now) running = True - if self.reader.state == RadarDonorVinState.COMPLETE: - if self.reader.vin: - self.store_vin(self.reader.vin) - return () - if self.reader.state == RadarDonorVinState.FAILED: + 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) - if output.state == RadarDonorVinState.COMPLETE and output.vin: - self.store_vin(output.vin) + self._persist_vin(output.vin) return output.can_sends if stored_vin or self._attempted: @@ -200,9 +210,10 @@ def update(self, can_packets: list[CanData], now: float, *, radar_enabled: bool, self._vin_fault_frames += 1 if self._vin_fault_frames < self.STABLE_FRAMES: return () - self._attempted = True - self.reader.start(now) - return self.reader.update(can_packets, now).can_sends + self._start_read(now) + output = self.reader.update(can_packets, now) + self._persist_vin(output.vin) + return output.can_sends class RadarDonorVinReader: 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/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_radar_donor_contract.py b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py index df465102062..23accb04431 100644 --- a/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_contract.py @@ -25,3 +25,11 @@ def test_create_radar_vin_msg_matches_tinkla_layout(): _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 index 6b71d27c8fe..2e6da80747d 100644 --- a/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py +++ b/opendbc/car/tesla/preap/tests/test_radar_donor_vin.py @@ -196,3 +196,44 @@ def test_commissioner_force_read_can_run_again_after_clear(): 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 d19abfee3d7..2658fefa22e 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 @@ -118,6 +123,7 @@ 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: @@ -162,6 +168,8 @@ def update(self, can_msgs): # HWFail is the dead-sensor bit and still blocks. if radar_status['RADC_HWFail']: ret.errors.radarFault = True + if self.table_freeze is not None: + self.table_freeze.reset() if radar_status['RADC_SensorDirty']: ret.errors.radarUnavailableTemporary = True @@ -204,6 +212,10 @@ 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']): + 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 80c1099630e..5445e4c5fa8 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -64,6 +64,8 @@ def _make_bosch_interface(): radar.pts = {} if hasattr(radar_interface_module, "BoschTrackLifecycle"): radar.bosch_tracks = radar_interface_module.BoschTrackLifecycle() + from opendbc.car.tesla.preap.radar_table_freeze import BoschTableFreezeWatch + radar.table_freeze = BoschTableFreezeWatch(stable_cycles=3, min_points=4) radar.rcp = FakeRadarParser() return radar @@ -165,3 +167,38 @@ 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() + + def _frozen_cycle(): + 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([]) + + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().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() + + def _frozen_cycle(): + 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([]) + + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().errors.radarFault is False diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index a4caedd7b31..c393fddcd3c 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; @@ -778,7 +778,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; @@ -789,7 +788,7 @@ 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; for (int i = 0; i < 17; i++) { diff --git a/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py b/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py index 015a8f87c35..5331ff98fc3 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py @@ -5,7 +5,6 @@ PREAP_FLAG_RADAR_EMULATION = 2 -PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 class TestTeslaPreAPRadarCarConfig: @@ -15,16 +14,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 = [] diff --git a/opendbc/safety/tests/test_tesla_preap_radar_donor.py b/opendbc/safety/tests/test_tesla_preap_radar_donor.py index 1c2594f220f..185fbdc271d 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_donor.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -4,7 +4,6 @@ PREAP_FLAG_RADAR_EMULATION = 2 -PREAP_FLAG_RADAR_BEHIND_NOSECONE = 4 AWD_VIN = "5YJSA1E42FF156789" # character 8 is '4' RWD_VIN = "5YJSA1E12FF156789" # character 8 is '1' @@ -35,6 +34,12 @@ def test_empty_vin_keeps_passthrough(self): 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 + 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_vin_sets_four_wheel_drive(self): _send_donor(self.safety, AWD_VIN, position=1, epas_type=3) assert self.safety.tesla_preap_radar_donor_active_debug() is True From 4578cec639929d977941a43f8991b7bbace883a8 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sun, 23 Aug 2026 01:57:30 +0000 Subject: [PATCH 04/12] tesla_preap: do not force 4WD from donor VIN nibble VIN character 8 of 2/4 is not dual-motor on 2015 RWD 5YJSA1E2. Forcing Msg2A9_FourWheelDrive on this chassis sprays Bosch tracks in motion. Keep chassis xWD from live 0x398; still impersonate the donor VIN. --- opendbc/safety/modes/tesla_preap.h | 9 +++------ .../tests/test_tesla_preap_radar_donor.py | 18 ++++++++++++------ 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index c393fddcd3c..f706fbf151d 100644 --- a/opendbc/safety/modes/tesla_preap.h +++ b/opendbc/safety/modes/tesla_preap.h @@ -301,12 +301,9 @@ 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); - // Tinkla 0.6.6: VIN character 8 of '2' or '4' is dual-motor. Force 4WD - // on the live 0x2A9 so xWD matches the donor VIN, not this Pre-AP chassis. - if (preap_radar_donor_active() && ((preap_radar_vin[7] == (uint8_t)'2') || - (preap_radar_vin[7] == (uint8_t)'4'))) { - lo |= 0x08U; - } + // Do not OR Msg2A9_FourWheelDrive from VIN[7]=='2'/'4'. 2015 RWD 5YJSA1E2 + // has char 8=='2'; forcing 4WD on this chassis sprays Bosch yRel in motion. + // Empty-VIN passthrough kept chassis 2WD and was clean. Preserve 0x398 xWD. PREAP_WORD_TO_BYTES(&dst->data[0], lo); PREAP_WORD_TO_BYTES(&dst->data[4], hi); } diff --git a/opendbc/safety/tests/test_tesla_preap_radar_donor.py b/opendbc/safety/tests/test_tesla_preap_radar_donor.py index 185fbdc271d..440484eede0 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_donor.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -6,7 +6,7 @@ PREAP_FLAG_RADAR_EMULATION = 2 AWD_VIN = "5YJSA1E42FF156789" # character 8 is '4' -RWD_VIN = "5YJSA1E12FF156789" # character 8 is '1' +JACK_RWD_VIN = "5YJSA1E25FF106153" # character 8 is '2' (2015 70 RWD) def _payload(safety, getter): @@ -40,16 +40,22 @@ def test_empty_vin_still_applies_position(self): 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_vin_sets_four_wheel_drive(self): + def test_awd_donor_vin_preserves_rwd_source_config(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"))) - # 0x02 | 0x08 = 0x0A, position nibble 1, EPAS 3 - assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4a85555310300010") + assert _payload(self.safety, self.safety.tesla_preap_radar_car_config_data) == bytes.fromhex("4285555310300010") - def test_rwd_vin_does_not_force_awd(self): - _send_donor(self.safety, RWD_VIN, position=0, epas_type=0) + 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_jack_rwd_vin_char2_does_not_force_awd(self): + # Live car VIN. Old heuristic treated char 8=='2' as dual-motor. + _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("4295555300001710") From a4c8a40fc16bea35fc3cd0822f49686e484cffcd Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sun, 23 Aug 2026 12:48:39 +0000 Subject: [PATCH 05/12] tesla_preap: add ignore-radar-HWFail toggle --- opendbc/car/tesla/preap/nap_conf.py | 11 +++++ opendbc/car/tesla/preap/nap_params.py | 2 + opendbc/car/tesla/radar_interface.py | 21 ++++++++-- .../car/tesla/tests/test_radar_interface.py | 40 +++++++++++++++++++ 4 files changed, 70 insertions(+), 4 deletions(-) diff --git a/opendbc/car/tesla/preap/nap_conf.py b/opendbc/car/tesla/preap/nap_conf.py index 53ac8ee79ad..ad64af23e97 100644 --- a/opendbc/car/tesla/preap/nap_conf.py +++ b/opendbc/car/tesla/preap/nap_conf.py @@ -33,6 +33,7 @@ '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': '', @@ -209,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') @@ -415,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("") @@ -435,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 d4c5f983efe..de4b3a8520f 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -24,6 +24,7 @@ class NAPParamKeys: # Radar RADAR_ENABLED = "NAPRadarEnabled" + RADAR_IGNORE_HW_FAIL = "NAPRadarIgnoreHwFail" RADAR_BEHIND_NOSECONE = "NAPRadarBehindNosecone" RADAR_OFFSET = "NAPRadarOffset" RADAR_DONOR_VIN = "NAPRadarDonorVin" @@ -53,6 +54,7 @@ class NAPParamKeys: NAPParamKeys.PEDAL_CALIB_FACTOR: 1.0, NAPParamKeys.PEDAL_CALIB_ZERO: 0.0, NAPParamKeys.RADAR_ENABLED: False, + NAPParamKeys.RADAR_IGNORE_HW_FAIL: False, NAPParamKeys.RADAR_BEHIND_NOSECONE: False, NAPParamKeys.RADAR_OFFSET: 0.0, NAPParamKeys.RADAR_DONOR_VIN: "", diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index 2658fefa22e..e153ab57a1b 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -128,8 +128,13 @@ def __init__(self, CP): # 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) + try: + self.ignore_hw_fail = bool(nap_conf.radar_ignore_hw_fail) + except Exception: + self.ignore_hw_fail = False else: self.radar_offset = 0.0 + self.ignore_hw_fail = False def update(self, can_msgs): @@ -165,11 +170,19 @@ def update(self, can_msgs): # 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. + # HWFail is the dead-sensor bit and still blocks, unless + # NAPRadarIgnoreHwFail is set (tracks can stay live on a false HWFail). if radar_status['RADC_HWFail']: - ret.errors.radarFault = True - if self.table_freeze is not None: - self.table_freeze.reset() + ignore_hw_fail = bool(getattr(self, "ignore_hw_fail", False)) + if nap_conf is not None: + try: + ignore_hw_fail = bool(nap_conf.radar_ignore_hw_fail) + except Exception: + pass + if 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 diff --git a/opendbc/car/tesla/tests/test_radar_interface.py b/opendbc/car/tesla/tests/test_radar_interface.py index 5445e4c5fa8..a793f3ae31f 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -202,3 +202,43 @@ def _frozen_cycle(): assert _frozen_cycle().errors.radarFault is False assert _frozen_cycle().errors.radarFault is False assert _frozen_cycle().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_still_faults_on_frozen_table_with_sgufail(self, monkeypatch): + radar = _make_bosch_interface() + 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() + + def _frozen_cycle(): + 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([]) + + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().errors.radarFault is False + assert _frozen_cycle().errors.radarFault is True From 0894437f52f0782ec2c00608ededd20565512366 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sun, 23 Aug 2026 12:57:47 +0000 Subject: [PATCH 06/12] tesla_preap: honor ignore-HWFail on freeze+SGUFail NAPRadarIgnoreHwFail left the frozen-table+SGUFail lockout in place, and a compiled params_pyx miss treated ignore as False. Resolve the flag live from nap_conf, Params, then the on-disk file. --- opendbc/car/tesla/radar_interface.py | 62 ++++++++--- .../car/tesla/tests/test_radar_interface.py | 103 +++++++++++++----- 2 files changed, 118 insertions(+), 47 deletions(-) diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index e153ab57a1b..7ce5c35ed5b 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -23,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: @@ -128,13 +161,9 @@ def __init__(self, CP): # 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) - try: - self.ignore_hw_fail = bool(nap_conf.radar_ignore_hw_fail) - except Exception: - self.ignore_hw_fail = False else: self.radar_offset = 0.0 - self.ignore_hw_fail = False + self.ignore_hw_fail = _resolve_ignore_hw_fail() def update(self, can_msgs): @@ -152,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 @@ -172,17 +206,11 @@ def update(self, can_msgs): # 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). - if radar_status['RADC_HWFail']: - ignore_hw_fail = bool(getattr(self, "ignore_hw_fail", False)) - if nap_conf is not None: - try: - ignore_hw_fail = bool(nap_conf.radar_ignore_hw_fail) - except Exception: - pass - if not ignore_hw_fail: - ret.errors.radarFault = True - if self.table_freeze is not None: - self.table_freeze.reset() + # Freeze+SGUFail is the same lockout family — honor the toggle there too. + 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 @@ -227,7 +255,7 @@ def update(self, can_msgs): 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 frozen and bool(self.rcp.vl['TeslaRadarSguInfo']['RADC_SGUFail']) and not ignore_hw_fail: 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 a793f3ae31f..e02f674e73b 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -79,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() @@ -175,16 +183,9 @@ def test_sgufail_with_frozen_table_is_radar_fault(self): 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() - def _frozen_cycle(): - 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([]) - - assert _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is True + 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() @@ -192,16 +193,9 @@ def test_frozen_table_without_sgufail_is_not_fault(self): 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() - def _frozen_cycle(): - 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([]) - - assert _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is False + 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() @@ -223,7 +217,7 @@ def test_ignore_hw_fail_leaves_sensor_dirty_temp_unavailable(self, monkeypatch): assert result.errors.radarFault is False assert result.errors.radarUnavailableTemporary is True - def test_ignore_hw_fail_still_faults_on_frozen_table_with_sgufail(self, monkeypatch): + def test_ignore_hw_fail_does_not_fault_on_frozen_table_with_sgufail(self, monkeypatch): radar = _make_bosch_interface() monkeypatch.setattr(radar_interface_module, "nap_conf", SimpleNamespace(radar_ignore_hw_fail=True)) radar.rcp.vl["TeslaRadarSguInfo"]["RADC_HWFail"] = 1 @@ -232,13 +226,62 @@ def test_ignore_hw_fail_still_faults_on_frozen_table_with_sgufail(self, monkeypa 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() - def _frozen_cycle(): - 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([]) + 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_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 _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is False - assert _frozen_cycle().errors.radarFault is True + assert result.errors.radarFault is True From e512a07cef506ca812e67d8bcc8f5b737e4f2d5f Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sun, 23 Aug 2026 22:07:13 +0000 Subject: [PATCH 07/12] tesla_preap: do not let IgnoreHwFail bypass freeze+SGUFail IgnoreHwFail was suppressing the frozen-table+SGUFail lockout, so stale tracks could become leadOne and slam aTarget. Keep the toggle for raw HWFail only; frozen && SGUFail still faults after ~5s. --- opendbc/car/tesla/radar_interface.py | 3 +-- opendbc/car/tesla/tests/test_radar_interface.py | 11 ++++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index 7ce5c35ed5b..c7c7e529746 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -206,7 +206,6 @@ def update(self, can_msgs): # 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). - # Freeze+SGUFail is the same lockout family — honor the toggle there too. if radar_status['RADC_HWFail'] and not ignore_hw_fail: ret.errors.radarFault = True if self.table_freeze is not None: @@ -255,7 +254,7 @@ def update(self, can_msgs): 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']) and not ignore_hw_fail: + if frozen and bool(self.rcp.vl['TeslaRadarSguInfo']['RADC_SGUFail']): 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 e02f674e73b..7a524ed758e 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,7 +65,6 @@ def _make_bosch_interface(): radar.pts = {} if hasattr(radar_interface_module, "BoschTrackLifecycle"): radar.bosch_tracks = radar_interface_module.BoschTrackLifecycle() - from opendbc.car.tesla.preap.radar_table_freeze import BoschTableFreezeWatch radar.table_freeze = BoschTableFreezeWatch(stable_cycles=3, min_points=4) radar.rcp = FakeRadarParser() return radar @@ -217,8 +217,9 @@ def test_ignore_hw_fail_leaves_sensor_dirty_temp_unavailable(self, monkeypatch): assert result.errors.radarFault is False assert result.errors.radarUnavailableTemporary is True - def test_ignore_hw_fail_does_not_fault_on_frozen_table_with_sgufail(self, monkeypatch): + def test_ignore_hw_fail_faults_on_frozen_table_with_sgufail(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 @@ -226,9 +227,9 @@ def test_ignore_hw_fail_does_not_fault_on_frozen_table_with_sgufail(self, monkey 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 + for _ in range(39): + assert _run_frozen_table(radar).errors.radarFault is False # HWFail is ignored. + 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() From ebd04ce3de1b9b1031cee886b94e5bfe1cdfd520 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Sun, 23 Aug 2026 22:52:55 +0000 Subject: [PATCH 08/12] tesla_preap: IgnoreHwFail drops frozen+SGU tracks instead of faulting MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Ignore still covers raw HWFail. Frozen+SGUFail no longer bricks engage when Ignore is on — those ghost tracks are dropped so they cannot become leadOne. Stock-safe radarFault remains when Ignore is off. --- opendbc/car/tesla/radar_interface.py | 8 +++++++- .../car/tesla/tests/test_radar_interface.py | 20 +++++++++++++++++-- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/opendbc/car/tesla/radar_interface.py b/opendbc/car/tesla/radar_interface.py index c7c7e529746..dbc10c28a4a 100644 --- a/opendbc/car/tesla/radar_interface.py +++ b/opendbc/car/tesla/radar_interface.py @@ -206,6 +206,8 @@ def update(self, can_msgs): # 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: @@ -255,7 +257,11 @@ def update(self, can_msgs): 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']): - ret.errors.radarFault = True + 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 7a524ed758e..17589943426 100644 --- a/opendbc/car/tesla/tests/test_radar_interface.py +++ b/opendbc/car/tesla/tests/test_radar_interface.py @@ -217,7 +217,7 @@ def test_ignore_hw_fail_leaves_sensor_dirty_temp_unavailable(self, monkeypatch): assert result.errors.radarFault is False assert result.errors.radarUnavailableTemporary is True - def test_ignore_hw_fail_faults_on_frozen_table_with_sgufail(self, monkeypatch): + 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)) @@ -228,7 +228,23 @@ def test_ignore_hw_fail_faults_on_frozen_table_with_sgufail(self, monkeypatch): radar.rcp.vl[f"RadarPoint{slot}_B"] = _point_b() for _ in range(39): - assert _run_frozen_table(radar).errors.radarFault is False # HWFail is ignored. + 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): From e59c8e11c2f42924c8330c23fa36d5b5ec0b9ea5 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Mon, 24 Aug 2026 22:45:46 +0000 Subject: [PATCH 09/12] tesla_preap: restore Tinkla 4WD nibble for VIN char 2/4 so this spare clears xwdValidity --- opendbc/safety/modes/tesla_preap.h | 15 ++++++++++++--- .../tests/test_tesla_preap_radar_donor.py | 19 ++++++++++++++----- 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index f706fbf151d..0d994f72b1b 100644 --- a/opendbc/safety/modes/tesla_preap.h +++ b/opendbc/safety/modes/tesla_preap.h @@ -301,9 +301,18 @@ 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); - // Do not OR Msg2A9_FourWheelDrive from VIN[7]=='2'/'4'. 2015 RWD 5YJSA1E2 - // has char 8=='2'; forcing 4WD on this chassis sprays Bosch yRel in motion. - // Empty-VIN passthrough kept chassis 2WD and was clean. Preserve 0x398 xWD. + // 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); } diff --git a/opendbc/safety/tests/test_tesla_preap_radar_donor.py b/opendbc/safety/tests/test_tesla_preap_radar_donor.py index 440484eede0..1181422f075 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_donor.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -6,7 +6,8 @@ PREAP_FLAG_RADAR_EMULATION = 2 AWD_VIN = "5YJSA1E42FF156789" # character 8 is '4' -JACK_RWD_VIN = "5YJSA1E25FF106153" # character 8 is '2' (2015 70 RWD) +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): @@ -40,23 +41,31 @@ def test_empty_vin_still_applies_position(self): 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_preserves_rwd_source_config(self): + 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("4285555310300010") + 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_jack_rwd_vin_char2_does_not_force_awd(self): - # Live car VIN. Old heuristic treated char 8=='2' as dual-motor. + 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): From 1e8af832d6aff8b83cec633f1ea504c08e17d3e2 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Tue, 25 Aug 2026 03:38:35 +0000 Subject: [PATCH 10/12] tesla_preap: add NAPRadarHud param key UI overlay toggle. Default off. --- opendbc/car/tesla/preap/nap_params.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/opendbc/car/tesla/preap/nap_params.py b/opendbc/car/tesla/preap/nap_params.py index de4b3a8520f..996622f2278 100644 --- a/opendbc/car/tesla/preap/nap_params.py +++ b/opendbc/car/tesla/preap/nap_params.py @@ -24,6 +24,7 @@ class NAPParamKeys: # Radar RADAR_ENABLED = "NAPRadarEnabled" + RADAR_HUD = "NAPRadarHud" RADAR_IGNORE_HW_FAIL = "NAPRadarIgnoreHwFail" RADAR_BEHIND_NOSECONE = "NAPRadarBehindNosecone" RADAR_OFFSET = "NAPRadarOffset" @@ -54,6 +55,7 @@ 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, From 604b9c497c2c51573c67379cd1cbbe20d2d3f9e2 Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Tue, 25 Aug 2026 14:37:03 +0000 Subject: [PATCH 11/12] tesla_preap: hold GTW until host VIN stream is complete Tinkla 0.6.6 did not talk on the radar bus until 0x560 was whole. We were forwarding this-car identity immediately, then switching contracts a second later. Host 0x560 is 10 Hz so the wait is short. --- opendbc/car/tesla/carcontroller.py | 7 ++--- opendbc/safety/modes/tesla_preap.h | 17 +++++++++++- .../safety/tests/libsafety/libsafety_py.py | 1 + .../tests/test_tesla_preap_radar_carconfig.py | 4 +++ .../tests/test_tesla_preap_radar_donor.py | 27 +++++++++++++++++++ 5 files changed, 52 insertions(+), 4 deletions(-) diff --git a/opendbc/car/tesla/carcontroller.py b/opendbc/car/tesla/carcontroller.py index 4dccc8bb75a..976501b7298 100644 --- a/opendbc/car/tesla/carcontroller.py +++ b/opendbc/car/tesla/carcontroller.py @@ -123,10 +123,11 @@ 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: always stream VIN/position/EPAS on 0x560 + # 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. - if nap_conf.radar_enabled and self.frame % 100 == 0: + # 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, diff --git a/opendbc/safety/modes/tesla_preap.h b/opendbc/safety/modes/tesla_preap.h index 0d994f72b1b..4f5c19f26f4 100644 --- a/opendbc/safety/modes/tesla_preap.h +++ b/opendbc/safety/modes/tesla_preap.h @@ -108,6 +108,7 @@ 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. @@ -155,6 +156,14 @@ 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; @@ -171,6 +180,7 @@ static bool preap_radar_donor_active(void) { 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]; @@ -354,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 @@ -512,6 +522,10 @@ uint8_t tesla_preap_radar_vin_feed_data(int 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 // ============================================ @@ -797,6 +811,7 @@ static safety_config tesla_preap_init(uint16_t param) { 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)' '; } diff --git a/opendbc/safety/tests/libsafety/libsafety_py.py b/opendbc/safety/tests/libsafety/libsafety_py.py index 420ccab4170..2e9ef72ad8e 100644 --- a/opendbc/safety/tests/libsafety/libsafety_py.py +++ b/opendbc/safety/tests/libsafety/libsafety_py.py @@ -80,6 +80,7 @@ class CANPacket: 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 5331ff98fc3..048616f3356 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_carconfig.py @@ -1,5 +1,6 @@ #!/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 @@ -31,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 index 1181422f075..65dc359437b 100644 --- a/opendbc/safety/tests/test_tesla_preap_radar_donor.py +++ b/opendbc/safety/tests/test_tesla_preap_radar_donor.py @@ -29,11 +29,38 @@ def setup_method(self): 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) From 4fd0b31abb76750609b572c2c4c99d580278277a Mon Sep 17 00:00:00 2001 From: jjackbrandt Date: Fri, 28 Aug 2026 20:27:26 +0000 Subject: [PATCH 12/12] tesla_preap: prompt regen under-delivery earlier The driver needs more time to add friction brake once regen cannot deliver the request. Fire at a shallower regen command, a smaller shortfall, and 0.5s of evidence instead of 0.8s. --- opendbc/car/tesla/preap/carcontroller.py | 37 ++++++++++--------- .../tesla/preap/tests/test_pedal_authority.py | 20 +++++++--- 2 files changed, 33 insertions(+), 24 deletions(-) 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/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):