diff --git a/opendbc_repo/opendbc/car/tesla/preap/nap_conf.py b/opendbc_repo/opendbc/car/tesla/preap/nap_conf.py index ad64af23e978a2..8d77ecd4868cf5 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/nap_conf.py +++ b/opendbc_repo/opendbc/car/tesla/preap/nap_conf.py @@ -39,6 +39,7 @@ 'radar_donor_vin': '', 'radar_epas_type': 0, 'radar_position': 0, + 'radar_upside_down': False, } @@ -133,6 +134,15 @@ def _save(self): pass def _get(self, key, default): + try: + if os.path.exists(CONFIG_FILE): + mtime = os.path.getmtime(CONFIG_FILE) + if getattr(self, "_last_mtime", 0) != mtime: + self._last_mtime = mtime + with open(CONFIG_FILE) as f: + self._cache.update(json.load(f)) + except Exception: + pass return self._cache.get(key, default) def _put(self, key, value): @@ -337,6 +347,14 @@ def radar_offset(self): def radar_offset(self, value): self._put_param_float(NAPParamKeys.RADAR_OFFSET, 'radar_offset', value) + @property + def radar_upside_down(self): + return self._get('radar_upside_down', False) + + @radar_upside_down.setter + def radar_upside_down(self, value): + self._put('radar_upside_down', bool(value)) + @property def pedal_calib_min(self): return self._get_param_float(NAPParamKeys.PEDAL_CALIB_MIN, 'pedal_calib_min', -3.0) diff --git a/opendbc_repo/opendbc/car/tesla/radar_interface.py b/opendbc_repo/opendbc/car/tesla/radar_interface.py index 87c1c5c3c11ec9..1446532b8bed1b 100644 --- a/opendbc_repo/opendbc/car/tesla/radar_interface.py +++ b/opendbc_repo/opendbc/car/tesla/radar_interface.py @@ -248,6 +248,8 @@ def update(self, can_msgs): # Parse track data self.pts[i].dRel = msg_a['LongDist'] self.pts[i].yRel = msg_a['LatDist'] + self.radar_offset + if nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False): + self.pts[i].yRel = -self.pts[i].yRel self.pts[i].vRel = msg_a['LongSpeed'] self.pts[i].aRel = msg_a['LongAccel'] self.pts[i].yvRel = msg_b['LatSpeed'] @@ -274,9 +276,9 @@ def _parse_bosch_track(self, msg_a, msg_b): return BoschTrackObservation( d_rel=msg_a['LongDist'], - y_rel=msg_a['LatDist'] + self.radar_offset, + y_rel=-(msg_a['LatDist'] + self.radar_offset) if (nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False)) else (msg_a['LatDist'] + self.radar_offset), v_rel=msg_a['LongSpeed'], a_rel=msg_a['LongAccel'], - yv_rel=msg_b['LatSpeed'], + yv_rel=-msg_b['LatSpeed'] if (nap_conf is not None and getattr(nap_conf, 'radar_upside_down', False)) else msg_b['LatSpeed'], measured=bool(msg_a['Meas']), ) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/nap.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/nap.py index 0d99637a88ab3f..38d654b278f1ca 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/nap.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/nap.py @@ -224,6 +224,19 @@ def _build_radar_items(self): tr("Let's you engage when Bosch raises HWFail even though tracks are still live."), dest=self._radar_items) + def on_upside_down(state): + from opendbc.car.tesla.preap.nap_conf import nap_conf + nap_conf.radar_upside_down = state + self._show_reboot_modal() + + from opendbc.car.tesla.preap.nap_conf import nap_conf + self._radar_items.append(toggle_item_sp( + tr("Invert Radar Orientation (Upside Down)"), + tr("Flip the radar's left/right axis. Use this if your radar is mounted upside down. Requires reboot."), + initial_state=nap_conf.radar_upside_down, + callback=on_upside_down, + )) + def on_radar_hud(state): self._params.put_bool(NAPParamKeys.RADAR_HUD, state) ui_state.radar_hud = state diff --git a/openpilot/sunnypilot/mads/mads.py b/openpilot/sunnypilot/mads/mads.py index 87d6824d52ea99..c429891e56177f 100644 --- a/openpilot/sunnypilot/mads/mads.py +++ b/openpilot/sunnypilot/mads/mads.py @@ -19,7 +19,7 @@ MadsSteeringModeOnBrake, persist_required_mads, read_steering_mode_param, resolve_mads_capabilities, unified_engagement_locked_off, ) -from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT +from openpilot.sunnypilot.mads.state import StateMachine, GEARS_ALLOW_PAUSED_SILENT, ENABLED_STATES State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState ButtonType = structs.CarState.ButtonEvent.Type @@ -32,6 +32,9 @@ IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput) HANDS_ON_PAUSE_LEVEL = 2 HANDS_ON_RESUME_US = 1_000_000 +# Same 200-sample bound already used for controlsMismatchLateral. Pending first +# grant uses it; a drop after grant is loss and disables immediately. +LATERAL_MISMATCH_MAX_COUNT = 200 UINT32_MASK = 0xFFFFFFFF @@ -80,6 +83,8 @@ def __init__(self, selfdrive): self._hands_on_steering_inhibited = False self._hands_on_clear_timing = False self._hands_on_clear_ts = 0 + self._lateral_permission_acquired = False + self._lateral_permission_acquire_counter = 0 # Required-MADS platforms consume only the frozen typed snapshot. if persist_required_mads(self.params, self.CP_SP): @@ -171,6 +176,8 @@ def _reset_hands_on_clear_timer(self): def _hard_disable_from_pause(self) -> None: self._hands_on_steering_inhibited = False self._reset_hands_on_clear_timer() + self._lateral_permission_acquired = False + self._lateral_permission_acquire_counter = 0 if self.events_sp.has(EventNameSP.silentLkasDisable): self.events_sp.remove(EventNameSP.silentLkasDisable) if self.events_sp.has(EventNameSP.silentLkasEnable): @@ -205,6 +212,13 @@ def _update_hands_on_pause(self, CS: structs.CarState) -> None: dm_lock = bool( self.events.has(EventName.driverUnresponsive3) or self.events.has(EventName.driverDistracted3) ) + mads_enable = self.events_sp.has(EventNameSP.lkasEnable) or self.events_sp.has(EventNameSP.silentLkasEnable) + lat_requested = bool(self.enabled or self.state_machine.state in ENABLED_STATES or mads_enable) + if not lat_requested: + self._lateral_permission_acquired = False + self._lateral_permission_acquire_counter = 0 + + established_lat_lost = panda_lat_lost and self._lateral_permission_acquired hard_event = bool( epas_fault or self.events_sp.has(EventNameSP.lkasDisable) @@ -212,12 +226,20 @@ def _update_hands_on_pause(self, CS: structs.CarState) -> None: or (not cs_fresh) or (not pandas) or (not panda_fresh) - or panda_lat_lost + or established_lat_lost or dm_lock or self.events_sp.has(EventNameSP.controlsMismatchLateral) or self.events.contains(ET.IMMEDIATE_DISABLE) or self.events_sp.contains(ET.IMMEDIATE_DISABLE) ) + if lat_requested and not hard_event: + if panda_lat_lost: + self._lateral_permission_acquire_counter += 1 + if self._lateral_permission_acquire_counter >= LATERAL_MISMATCH_MAX_COUNT: + hard_event = True + else: + self._lateral_permission_acquired = True + self._lateral_permission_acquire_counter = 0 if hard_event: self._hard_disable_from_pause() return @@ -269,7 +291,7 @@ def data_sample(self): # When the safety and selfdrived do not agree on controls_allowed_lateral # we want to disengage sunnypilot. However the status from the panda goes through # another socket other than the CAN messages and one can arrive earlier than the other. - # Therefore we allow a mismatch for two samples, then we trigger the disengagement. + # Allow LATERAL_MISMATCH_MAX_COUNT control frames for that transport mismatch. if not self.active or self.selfdrive.enabled: self.lateral_mismatch_counter = 0 elif any(not ps.controlsAllowedLateral for ps in self.selfdrive.sm['pandaStates'] @@ -393,7 +415,7 @@ def update_events(self, CS: structs.CarState): if self.state_machine.state == State.paused: self.events_sp.add(EventNameSP.silentLkasEnable) - if self.lateral_mismatch_counter >= 200: + if self.lateral_mismatch_counter >= LATERAL_MISMATCH_MAX_COUNT: self.events_sp.add(EventNameSP.controlsMismatchLateral) self.events.remove(EventName.pcmDisable) diff --git a/openpilot/sunnypilot/mads/tests/test_preap_hands_on_pause.py b/openpilot/sunnypilot/mads/tests/test_preap_hands_on_pause.py index 77d184736c843c..7fc334a3a40b5c 100644 --- a/openpilot/sunnypilot/mads/tests/test_preap_hands_on_pause.py +++ b/openpilot/sunnypilot/mads/tests/test_preap_hands_on_pause.py @@ -7,7 +7,7 @@ from opendbc.sunnypilot.car.tesla.values import TeslaFlagsSP from openpilot.selfdrive.selfdrived.events import Events from openpilot.sunnypilot.selfdrive.selfdrived.events import EventsSP -from openpilot.sunnypilot.mads.mads import HANDS_ON_RESUME_US, ModularAssistiveDrivingSystem +from openpilot.sunnypilot.mads.mads import HANDS_ON_RESUME_US, LATERAL_MISMATCH_MAX_COUNT, ModularAssistiveDrivingSystem State = custom.ModularAssistiveDrivingSystem.ModularAssistiveDrivingSystemState EventName = log.OnroadEvent.EventName @@ -89,6 +89,26 @@ def make_mads(hands_on=True, capability=True): mads.state_machine.state = State.enabled return mads, sd +def _disengage(mads): + mads.enabled = False + mads.active = False + mads.state_machine.state = State.disabled + + +def _step_mads(mads, sd, cs, *, allowed=True, inhibited=False, panda_valid=True, enable=False, override=False): + ps = _panda(inhibited) + ps.controlsAllowedLateral = allowed + sd.sm = FakeSM([ps], panda_valid=panda_valid) + sd.events.clear() + sd.events_sp.clear() + if enable: + sd.events_sp.add(EventNameSP.lkasEnable) + if override: + sd.events.add(EventName.steerOverride) + mads.update(cs) + sd.CS_prev = cs + + class TestPreAPHandsOnPause(unittest.TestCase): def test_hands_on_level_pauses_not_steering_pressed(self): @@ -327,6 +347,98 @@ def test_generic_platform_unchanged(self): self.assertFalse(mads._hands_on_steering_inhibited) self.assertFalse(sd.events_sp.has(EventNameSP.silentLkasDisable)) + def test_first_pull_delayed_permission_50ms_does_not_false_disable(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + self.assertNotEqual(mads.state_machine.state, State.disabled) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + for _ in range(5): + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertNotEqual(mads.state_machine.state, State.disabled) + _step_mads(mads, sd, cs, allowed=True, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.overriding) + + def test_first_pull_delayed_permission_over_100ms_does_not_false_disable(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + for _ in range(15): + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertNotEqual(mads.state_machine.state, State.disabled) + _step_mads(mads, sd, cs, allowed=True, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.overriding) + + def test_never_granted_permission_hits_mismatch_bound(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + for _ in range(LATERAL_MISMATCH_MAX_COUNT - 2): + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertNotEqual(mads.state_machine.state, State.disabled) + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.disabled) + + def test_acquired_permission_then_lost_hard_disables(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=True, enable=True, override=True) + _step_mads(mads, sd, cs, allowed=True, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.overriding) + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.disabled) + + def test_pause_cannot_evade_acquisition_deadline(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + for _ in range(49): + _step_mads(mads, sd, cs, allowed=False, override=True) + self.assertNotEqual(mads.state_machine.state, State.disabled) + cs_pause = FakeCS(hands_on_level=2) + remaining = LATERAL_MISMATCH_MAX_COUNT - 50 + for _ in range(remaining - 1): + _step_mads(mads, sd, cs_pause, allowed=False, inhibited=True) + self.assertNotEqual(mads.state_machine.state, State.disabled) + _step_mads(mads, sd, cs_pause, allowed=False, inhibited=True) + self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.disabled) + + def test_stale_positive_grant_during_pending_hard_disables(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + for _ in range(10): + _step_mads(mads, sd, cs, allowed=False, override=True) + _step_mads(mads, sd, cs, allowed=True, override=True, panda_valid=False) + self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.disabled) + + def test_door_during_pending_acquisition_hard_disables(self): + mads, sd = make_mads() + _disengage(mads) + cs = FakeCS(hands_on_level=1, steering_pressed=True) + _step_mads(mads, sd, cs, allowed=False, enable=True, override=True) + self.assertFalse(sd.events_sp.has(EventNameSP.lkasDisable)) + _step_mads(mads, sd, FakeCS(hands_on_level=1, steering_pressed=True, door_open=True), + allowed=False, override=True) + self.assertTrue(sd.events_sp.has(EventNameSP.lkasDisable)) + self.assertEqual(mads.state_machine.state, State.disabled) if __name__ == "__main__": unittest.main() diff --git a/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/calibrate_pedal.py b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/calibrate_pedal.py index b73c6024801b6b..3826df84539335 100755 --- a/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/calibrate_pedal.py +++ b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/calibrate_pedal.py @@ -237,9 +237,9 @@ def check_safety(self) -> bool: return True def _advance_on_pedal_message(self) -> None: - if self.pedal_interceptor_state != 0: - return self.pedal_error_count = 0 + if self.status == 2 and self.pedal_interceptor_state != 0: + return if self.status == 2: self.pedal_zero_sum += self.pedal_interceptor_value diff --git a/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/transport.py b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/transport.py index 52cd58b04de224..b8a4bf51858c4a 100644 --- a/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/transport.py +++ b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/transport.py @@ -128,6 +128,7 @@ def can_send(self, addr: int, dat: bytes, bus: int) -> None: if self._mode in (SAFETY_ELM327, SAFETY_ALLOUTPUT) or self._mode != SAFETY_TESLA_PREAP: raise TransportError("pedal frames require teslaPreap calibration safety") try: + self.panda.send_heartbeat(False, False) self.panda.can_send(addr, dat, bus) except Exception as exc: raise TransportError(f"can_send failed: {exc}") from exc diff --git a/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/vin_learn_radar.py b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/vin_learn_radar.py new file mode 100755 index 00000000000000..c3a85ed810cd49 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/car/tesla/preap/tools/vin_learn_radar.py @@ -0,0 +1,162 @@ +#!/usr/bin/env python3 +import time +import struct +import threading +import argparse +from panda import Panda +from openpilot.common.params import Params + +# UDS Constants +TX_ID = 0x641 +RX_ID = 0x651 + +def tesla_radar_security_access_algorithm(seed_bytes): + if len(seed_bytes) != 4: + raise ValueError(f"security seed must be 4 bytes, got {len(seed_bytes)}") + seed_val = int.from_bytes(seed_bytes, byteorder="big") + k4 = (seed_val >> 5 & 8) | (seed_val >> 0xB & 4) | (seed_val >> 0x18 & 1) | (seed_val >> 1 & 2) + if seed_val & 0x20000 == 0: + k32 = ((seed_val & ~(0xFF << k4 & 0xFFFFFFFF)) << (0x20 - k4) & 0xFFFFFFFF) | (seed_val >> k4 & 0xFFFFFFFF) + else: + k32 = ((~(0xFF << k4 & 0xFFFFFFFF) << (0x20 - k4) & seed_val & 0xFFFFFFFF) >> (0x20 - k4) & 0xFFFFFFFF) | (seed_val << k4 & 0xFFFFFFFF) + k2 = (seed_val >> 4) & 2 | (seed_val >> 0x1F) + if k2 == 0: + key_int = k32 | seed_val + elif k2 == 1: + key_int = k32 & seed_val + elif k2 == 2: + key_int = k32 ^ seed_val + else: + key_int = k32 + key_int &= 0xFFFFFFFF + return struct.pack("!I", key_int) + +class StandaloneFlasher: + def __init__(self, vin: str): + self.vin = vin.ljust(17, '0')[:17].encode('ascii') + from openpilot.common.params import Params + self.position = int(Params().get("NAPRadarPosition") or 0) + self.p = Panda() + self.p.set_safety_mode(17) # SAFETY_ALLOUTPUT + self.p.can_clear(0xFFFF) + self.running = True + self.bus = 1 + + def gateway_thread(self): + # Transmit 0x2A9 and 0x2B9 so the radar can learn the VIN + while self.running: + # 0x2A9 config + msg_2a9 = bytearray(8) + msg_2a9[0] = 0x44 # carConfig (US, no air susp, P85) + # Add XWD bit if VIN says AWD (char 8 is '2' or '4') + drive = self.vin[7] + if drive in [b'2', b'4', ord('2'), ord('4')]: + msg_2a9[0] |= 0x08 + msg_2a9[1] = 0x82 # RWD, EPAS type 2 + msg_2a9[2] = 0x20 # AP1, ParkAssist + msg_2a9[4] = 0x01 | (self.position << 4) # ForwardRadarHW (Bosch) + Position + + # 0x2B9 VIN + idx = int(time.time() * 10) % 7 + msg_2b9 = bytearray(8) + msg_2b9[0] = idx + for i in range(7): + v_idx = idx * 7 + i + if v_idx < 17: + msg_2b9[i+1] = self.vin[v_idx] + + self.p.can_send(0x2A9, bytes(msg_2a9), self.bus) + self.p.can_send(0x2B9, bytes(msg_2b9), self.bus) + time.sleep(0.1) + + def uds_request(self, payload, expected_pci, timeout=2.0): + # Single frame + data = bytearray([len(payload)]) + bytearray(payload) + data = data.ljust(8, b'\x00') + self.p.can_send(TX_ID, bytes(data), self.bus) + + start = time.monotonic() + while time.monotonic() - start < timeout: + rx = self.p.can_recv() + for addr, dat, src in rx: + if src == self.bus and addr == RX_ID: + if dat[0] == expected_pci and dat[1] == payload[0] + 0x40: + return dat + # Negative response: 0x03 0x7F [Service] [NRC] + if dat[0] == 0x03 and dat[1] == 0x7F and dat[2] == payload[0]: + if dat[3] == 0x21: # Busy + return b'BUSY' + raise Exception(f"UDS Error: {dat.hex()}") + time.sleep(0.01) + raise TimeoutError(f"UDS Request timed out for {bytes(payload).hex()}") + + def run(self): + print("Starting gateway spoofing...") + gw = threading.Thread(target=self.gateway_thread) + gw.start() + + try: + print("Sending Tester Present...") + self.uds_request([0x3E, 0x00], 0x02) + + print("Switching to Default Session...") + self.uds_request([0x10, 0x01], 0x06) + + print("Switching to Extended Session...") + self.uds_request([0x10, 0x03], 0x06) + + print("Requesting Seed...") + resp = self.uds_request([0x27, 0x11], 0x06) + seed = resp[3:7] + print(f"Seed: {seed.hex()}") + + key = tesla_radar_security_access_algorithm(seed) + print(f"Key: {key.hex()}") + + print("Submitting Key...") + self.uds_request([0x27, 0x12] + list(key), 0x02) + + print("Starting VIN Learn Routine (0x0A03)...") + self.uds_request([0x31, 0x01, 0x0A, 0x03], 0x04) + + print("Waiting for VIN Learn to complete...") + for i in range(15): + time.sleep(2) + try: + resp = self.uds_request([0x31, 0x02, 0x0A, 0x03], 0x04) + if resp != b'BUSY': + break + except Exception as e: + print(f"Stop routine error (ignoring): {e}") + + print("Requesting Results...") + try: + resp = self.uds_request([0x31, 0x03, 0x0A, 0x03], 0x04) + print(f"Results: {resp.hex()}") + except Exception as e: + print(f"No results response, but routine finished: {e}") + print("VIN Learn Complete!") + + finally: + self.running = False + gw.join() + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--confirm", action="store_true") + args = parser.parse_args() + + params = Params() + vin = params.get("NAPRadarDonorVin") + if not vin or len(vin) != 17: + print("Error: NAPRadarDonorVin is not set or invalid length.") + return + + print(f"Starting VIN Learn for {vin}...") + flasher = StandaloneFlasher(vin) + flasher.run() + + + +if __name__ == "__main__": + main()