Restore calibrated Tesla Pre-AP pedal control

CREDITS.md

@@ -29,3 +29,7 @@ The optional ACC-target and fused-track receivers and DBC preserve firestar5683'
 ## Spotify Bluetooth output
 
 Spotify's Bluetooth output binds the comma's BlueALSA A2DP source to the selected speaker through a process-local ALSA configuration, leaving the system ALSA default, soundd and the warning speaker untouched. That approach is adapted from firestar5683's RoadScore Bluetooth output (`roadscore/prototype/bluetooth_output.py`, firestar5683/StarPilot `RoadScore` at `249eb88b3a397db847e4a595d7e23b7e85d329e5`). The Soloist player itself is never included: each user downloads it from Spotify, and the PulseAudio server that carries its audio is vendored unmodified from Ubuntu (see THIRD_PARTY_NOTICES.md).
+
+## Tesla Pre-AP pedal control
+
+The Pre-AP configuration, pedal codec/feedback, VirtualDAS feedforward table, grade compensation, torque-zero estimator and vehicle-specific gains preserve firestar5683's intentional Dom work, including `7837a3c1aab`, `aed0b7fdb9e9` and pinned `b220a37f4bd119e402225a7aa16a7cd84878d0af`. Original author attribution: firestar5683 <168790843+firestar5683@users.noreply.github.com>. Startup/calibration ownership, current upstream button/PID interfaces and native command bounds are adapted to the current StarPilot architecture; the old permissive native gateway is not transplanted. Existing source/license notices remain intact.

opendbc_repo/opendbc/car/tesla/carstate.py

@@ -161,6 +161,14 @@ class CarState(CarStateBase):
 
     return ret
 
+  def update_button_enable(self, buttonEvents):
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP and self.CP.openpilotLongitudinalControl:
+      if not self.preap.enableLongControl:
+        return False
+      if any(event.type == structs.CarState.ButtonEvent.Type.setCruise and not event.pressed for event in buttonEvents):
+        return True
+    return super().update_button_enable(buttonEvents)
+
   @staticmethod
   def get_can_parsers(CP):
     if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:

opendbc_repo/opendbc/car/tesla/preap/aol.py

@@ -4,25 +4,33 @@ from opendbc.car import structs
 from opendbc.car.tesla.values import CAR
 
 
-def qualified(cp, *, marked_only=False):
+def _stock_qualified(cp, *, marked_only=False):
   try:
     return bool(cp.brand == "tesla" and cp.carFingerprint == CAR.TESLA_MODEL_S_PREAP and cp.flags == 0 and
-      not cp.passive and not cp.notCar and not cp.dashcamOnly and cp.radarUnavailable and
+      not cp.passive and not cp.notCar and not cp.dashcamOnly and ((cp.safetyConfigs[0].safetyParam == 0 and cp.radarUnavailable) or
+       (cp.safetyConfigs[0].safetyParam in (2, 6) and not cp.radarUnavailable)) and
       cp.transmissionType != structs.CarParams.TransmissionType.manual and
       cp.fingerprintSource != structs.CarParams.FingerprintSource.fixed and
       cp.pcmCruise and not cp.openpilotLongitudinalControl and not cp.alphaLongitudinalAvailable and
       cp.steerControlType == structs.CarParams.SteerControlType.angle and cp.steerAtStandstill and
       math.isfinite(cp.wheelbase) and math.isclose(cp.wheelbase, 2.96, rel_tol=0.0, abs_tol=1e-6) and cp.steerRatio == 15.0 and len(cp.safetyConfigs) == 1 and
       cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreap and
-      cp.safetyConfigs[0].safetyParam == 0 and cp.alternativeExperience in ((32,) if marked_only else (0, 32)))
+      cp.safetyConfigs[0].safetyParam in (0, 2, 6) and cp.alternativeExperience in ((32,) if marked_only else (0, 32)))
   except (AttributeError, TypeError, ValueError, OverflowError, IndexError):
     return False
 
 
+def qualified(cp, *, marked_only=False):
+  from opendbc.car.tesla.preap.pedal_profile import qualified as pedal_qualified
+  return _stock_qualified(cp, marked_only=marked_only) or (pedal_qualified(cp) and (not marked_only or cp.alternativeExperience == 32))
+
+
 def temporary_restriction(cp, state):
   return bool(qualified(cp, marked_only=True) and state is not None and state.canValid and not state.canTimeout and
-    not state.steerFaultPermanent and state.gearShifter == structs.CarState.GearShifter.drive and
-    (state.brakePressed or state.steerFaultTemporary or state.standstill))
+    not state.steerFaultPermanent and not state.steeringDisengage and not state.doorOpen and not state.accFaulted and
+    (state.gearShifter in (structs.CarState.GearShifter.reverse, structs.CarState.GearShifter.neutral) or
+     state.gearShifter == structs.CarState.GearShifter.drive and
+     (state.brakePressed or state.steerFaultTemporary or state.standstill)))
 
 
 def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns, pending_cancel_ns=0):

opendbc_repo/opendbc/car/tesla/preap/constants.py

@@ -0,0 +1,16 @@
+ACCEL_PREAP_BP = [0.0, 1.3, 7.5, 15.0, 25.0, 40.0]
+ACCEL_PREAP_FOLLOW = [0.35, 0.55, 0.80, 0.75, 0.65, 0.50]
+ACCEL_PREAP_PROFILES = {
+  0: [0.3, 0.8, 1.1, 1.0, 0.85, 0.7],
+  1: [0.3, 0.7, 1.0, 0.9, 0.8, 0.65],
+  2: [0.3, 0.6, 0.9, 0.8, 0.7, 0.55],
+}
+PEDAL_LONG_K_BP = [0.0, 3.0, 6.0, 35.0]
+PEDAL_LONG_KP_V = [0.0, 0.0, 0.0, 0.0]
+PEDAL_LONG_KI_V = [0.05, 0.08, 0.10, 0.15]
+VDAS_INNER_K_BP = [0.0, 5.0, 35.0]
+VDAS_INNER_KP_V = [0.0, 0.0, 0.0]
+VDAS_INNER_KI_V = [0.3, 0.2, 0.15]
+VDAS_FUTURE_T_BP = [2.0, 5.0]
+VDAS_FUTURE_T_V = [0.30, 0.55]
+VDAS_AEGO_FILTER_RC = 0.25

opendbc_repo/opendbc/car/tesla/preap/ff_table_default.py

@@ -0,0 +1,10 @@
+SPEED_BP = [0.0, 5.0, 12.0, 20.0, 30.0, 40.0]
+ACCEL_BP = [-1.5, -1.0, -0.5, 0.0, 0.5, 1.0, 1.5, 2.0, 2.5]
+DEFAULT_TABLE = [
+  [-5.00, -3.33, -1.67, 0.00, 10.00, 20.00, 30.00, 40.00, 50.00],
+  [-5.00, -3.33, -1.67, 0.00, 11.60, 23.20, 34.80, 46.40, 58.00],
+  [-5.00, -3.33, -1.67, 0.00, 13.20, 26.40, 39.60, 52.80, 66.00],
+  [-5.00, -3.33, -1.67, 0.00, 14.80, 29.60, 44.40, 59.20, 74.00],
+  [-5.00, -3.33, -1.67, 0.00, 16.40, 32.80, 49.20, 65.60, 82.00],
+  [-5.00, -3.33, -1.67, 0.00, 18.00, 36.00, 54.00, 72.00, 90.00],
+]

opendbc_repo/opendbc/car/tesla/preap/pedal_contract.py

@@ -0,0 +1,131 @@
+"""Frozen working Dom pedal calibration and0x551/0x552 wire contract."""
+from dataclasses import dataclass
+import math
+import struct
+
+M1 = 0.050796813
+M2 = 0.101593626
+D = -22.85856576
+FEEDBACK_TIMEOUT_NS = 500_000_000
+
+
+def float32(value):
+  return struct.unpack('<f', struct.pack('<f', value))[0]
+
+
+@dataclass(frozen=True)
+class PedalCalibration:
+  factor: float
+  zero: float
+  bus: int
+
+  def valid(self):
+    try:
+      if (type(self.bus) is not int or self.bus not in (0, 2) or
+          not math.isfinite(self.factor) or abs(self.factor) <= 1e-6 or not math.isfinite(self.zero) or
+          self.factor != float32(self.factor) or self.zero != float32(self.zero)):
+        return False
+      # Preserve Dom's clipping at the electrical floor. A saturated endpoint
+      # may not create incompatible values for the two physical channels.
+      for di in (-5., 90.):
+        one = max(0, min(65534, int((self.physical(di) - D) / M1)))
+        two = max(0, min(65534, int((self.physical(di) - D) / M2)))
+        if not 0 <= one - 2 * two <= 1:
+          return False
+      return True
+    except (ValueError, TypeError, OverflowError, struct.error):
+      return False
+
+  def physical(self, di):
+    return self.zero + di / self.factor
+
+  def di(self, physical):
+    return (physical - self.zero) * self.factor
+
+
+def checksum(address, first_five):
+  return ((address & 255) + (address >> 8) + sum(first_five)) & 255
+
+
+def pedal_max(speed):
+  bp = (0., 5., 12., 20., 30., 40.)
+  values = (50., 58., 66., 74., 82., 90.)
+  if speed <= 0:
+    return values[0]
+  for index in range(1, len(bp)):
+    if speed <= bp[index]:
+      ratio = (speed - bp[index - 1]) / (bp[index] - bp[index - 1])
+      return values[index - 1] + ratio * (values[index] - values[index - 1])
+  return values[-1]
+
+
+class PedalCodec:
+  def __init__(self, calibration):
+    if type(calibration) is not PedalCalibration or not calibration.valid():
+      raise ValueError('invalid frozen Tesla pedal calibration')
+    self.calibration = calibration
+    self.counter = 0
+
+  def command(self, di=0., *, enabled=False):
+    if type(enabled) is not bool or not math.isfinite(di):
+      raise ValueError('invalid Tesla pedal demand')
+    one = two = 0
+    if enabled:
+      if not -5. <= di <= 90.:
+        raise ValueError('Tesla pedal DI outside original envelope')
+      physical = self.calibration.physical(di)
+      one = max(0, min(65534, int((physical - D) / M1)))
+      two = max(0, min(65534, int((physical - D) / M2)))
+      if not 0 <= one <= 65534 or not 0 <= two <= 65534 or abs(one - 2 * two) > 1:
+        raise ValueError('invalid dual-channel Tesla pedal command')
+    prefix = struct.pack('>HHB', one, two, (int(enabled) << 7) | self.counter)
+    self.counter = (self.counter + 1) & 15
+    return 0x551, prefix + bytes([checksum(0x551, prefix)]), self.calibration.bus
+
+
+class PedalFeedback:
+  def __init__(self, calibration):
+    if type(calibration) is not PedalCalibration or not calibration.valid():
+      raise ValueError('invalid frozen Tesla pedal calibration')
+    self.calibration = calibration
+    self.last_counter = None
+    self.source_ns = 0
+    self.last_observed_ns = 0
+    self.healthy = False
+    self.value = self.value2 = 0.
+    self.torque_level = 0.
+
+  def clear(self):
+    self.healthy = False
+    self.source_ns = 0
+    self.last_counter = None
+
+  def observe(self, payload, bus, source_ns):
+    if (type(payload) is not bytes or len(payload) != 6 or bus != self.calibration.bus or
+        type(source_ns) is not int or source_ns < self.last_observed_ns):
+      self.clear()
+      return False
+    self.last_observed_ns = source_ns
+    one, two, flags = struct.unpack('>HHB', payload[:5])
+    state, counter = flags >> 4, flags & 15
+    # Dom did not define a feedback checksum validator. Do not substitute the
+    # unrelated upstream pedal CRC. Keep state/counter/dual-channel provenance.
+    physical_one, physical_two = one * M1 + D, two * M2 + D
+    if (state != 0 or one == 65535 or two == 65535 or
+        abs(physical_one - physical_two) > 2 * M2):
+      self.clear()
+      return False
+    previous = self.last_counter
+    self.last_counter = counter
+    self.healthy = previous is not None and counter == ((previous + 1) & 15)
+    self.source_ns = source_ns if self.healthy else 0
+    self.value = self.calibration.di(physical_one)
+    self.value2 = self.calibration.di(physical_two)
+    return self.healthy
+
+  def ready(self, now_ns):
+    return bool(self.healthy and self.source_ns > 0 and self.source_ns <= now_ns and
+                now_ns - self.source_ns <= FEEDBACK_TIMEOUT_NS)
+
+  def gas_pressed(self):
+    return self.value > 2. or self.value2 > 2.

opendbc_repo/opendbc/car/tesla/preap/pedal_control.py

@@ -0,0 +1,57 @@
+"""Dom VirtualDAS pedal control with frozen calibration and current axis authority."""
+import math
+from opendbc.car.tesla.preap.pedal_contract import PedalCodec, pedal_max
+from opendbc.car.tesla.preap.pedal_dynamics import PedalZeroTorque
+from opendbc.car.tesla.preap.virtual_das import VirtualDAS
+
+
+class PreAPPedalControl:
+  def __init__(self, calibration):
+    self.codec = PedalCodec(calibration)
+    self.zero_torque = PedalZeroTorque()
+    self.vdas = VirtualDAS(zero_torque=self.zero_torque)
+    self.previous_di = 0.
+    self.was_requested = False
+    self.engage_frame = -1_000_000
+
+  def update(self, CC, CS, frame, now_ns):
+    if frame % 2:
+      return []
+    feedback = CS.pedal_feedback
+    healthy = feedback is not None and feedback.ready(now_ns)
+    requested = bool(healthy and CS.cruiseEnabled and CS.enableLongControl)
+    active = bool(requested and CC.enabled and CC.longActive and CS.out.canValid and not CS.out.canTimeout and
+                  not CS.out.gasPressed and not CS.out.brakePressed and not CS.out.accFaulted and
+                  not CS.out.doorOpen and not CS.out.steerFaultPermanent and not CS.out.steeringDisengage)
+    if requested != self.was_requested:
+      CS.preap_cc_cancel_needed = True
+    if requested and not self.was_requested:
+      self.engage_frame = frame
+      self.previous_di = min(pedal_max(CS.out.vEgo), max(-5., feedback.value))
+      self.vdas.reset(a_init=0., pedal_di_init=self.previous_di)
+    self.was_requested = requested
+    if not active:
+      self.previous_di = 0.
+      self.vdas.reset()
+      return [self.codec.command(enabled=False)]
+    values = (CC.actuators.accel, CS.out.vEgo, CS.out.aEgo, *CC.orientationNED)
+    if not all(math.isfinite(float(value)) for value in values):
+      self.previous_di = 0.
+      self.vdas.reset()
+      return [self.codec.command(enabled=False)]
+    self.zero_torque.update(feedback.torque_level, self.previous_di, CS.out.vEgo)
+    elapsed = frame - self.engage_frame
+    grace = elapsed < 50
+    demand = float(CC.actuators.accel)
+    if grace:
+      # Original50-control-frame positive engagement grace; no old generic tune rollback.
+      demand = max(0., min(demand, max(0., CS.preap_accel_max) * elapsed / 50.))
+    proposed = self.vdas.update(demand, CS.out.vEgo, self.previous_di, a_ego=CS.out.aEgo,
+                                freeze_integrator=grace, orientation_ned=list(CC.orientationNED))
+    # Apply hard domain last: original rate clamp could otherwise escape it
+    # when an initial driver observation lay above the speed envelope.
+    proposed = max(-5., min(pedal_max(CS.out.vEgo), proposed))
+    self.previous_di = proposed
+    if proposed <= -4.75 and not grace:
+      CS.pccEvent = 'pedalMaxRegen'
+    return [self.codec.command(proposed, enabled=True)]

opendbc_repo/opendbc/car/tesla/preap/pedal_dynamics.py

@@ -0,0 +1,33 @@
+"""Original Dom preAP dynamics constants and per-drive torquezero owner."""
+from opendbc.car.common.conversions import Conversions as CV
+
+PEDAL_DI_MIN = -5.
+PEDAL_DI_ZERO = 0.
+ACCEL_MAX = 2.5
+REGEN_MAX = -1.5
+PEDAL_BP = [0., 5., 12., 20., 30., 40.]
+PEDAL_MAX_VALUES = [50., 58., 66., 74., 82., 90.]
+PEDAL_RAMP_RATE_UP = 5.
+PEDAL_RAMP_RATE_DOWN = 2.5
+TORQUE_LEVEL_ACC = 0.
+TORQUE_LEVEL_DECEL = -30.
+ZERO_TORQUE_MIN_SPEED = 10. * CV.MPH_TO_MS
+
+
+class PedalZeroTorque:
+  def __init__(self):
+    self.value = PEDAL_DI_ZERO
+    self._best_torque = TORQUE_LEVEL_DECEL
+
+  def update(self, torque_level: float, current_pedal_di: float, v_ego: float) -> None:
+    if v_ego < ZERO_TORQUE_MIN_SPEED:
+      return
+
+    if TORQUE_LEVEL_DECEL < torque_level < TORQUE_LEVEL_ACC and abs(torque_level) < abs(self._best_torque):
+      self.value = current_pedal_di
+      self._best_torque = torque_level
+
+  def get(self, v_ego: float) -> float:
+    if v_ego < 5.0 * CV.MPH_TO_MS:
+      return PEDAL_DI_ZERO
+    return self.value

opendbc_repo/opendbc/car/tesla/preap/pedal_parser.py

@@ -0,0 +1,19 @@
+"""Actual selected-bus CANParser plus original-source pedal feedback provenance."""
+from opendbc.can import CANParser
+
+
+class PedalCANParser(CANParser):
+  def __init__(self, feedback):
+    # Dom's 0x552 format is validated by PedalFeedback, independently of
+    # generic interceptor DBC messages and their unrelated checksum rules.
+    super().__init__('tesla_can', [], feedback.calibration.bus)
+    self.feedback = feedback
+
+  def update(self, strings, sendcan=False):
+    if strings and not isinstance(strings[0], list | tuple):
+      strings = [strings]
+    for stamp, frames in strings:
+      for address, payload, bus in frames:
+        if address == 0x552 and bus == self.feedback.calibration.bus:
+          self.feedback.observe(bytes(payload), bus, int(stamp))
+    return super().update(strings, sendcan)

opendbc_repo/opendbc/car/tesla/preap/pedal_profile.py

@@ -0,0 +1,39 @@
+"""Finite pedal profiles through the existing preAP safety model39."""
+import math
+from opendbc.car import structs
+from opendbc.car.tesla.values import CAR
+from opendbc.car.tesla.preap.constants import ACCEL_PREAP_BP, ACCEL_PREAP_PROFILES
+
+
+def qualified(cp):
+  try:
+    word = int(cp.safetyConfigs[0].safetyParam)
+    return bool(cp.brand == 'tesla' and cp.carFingerprint == CAR.TESLA_MODEL_S_PREAP and
+      cp.flags == 0 and not cp.passive and not cp.notCar and not cp.dashcamOnly and
+      cp.openpilotLongitudinalControl and not cp.pcmCruise and not cp.alphaLongitudinalAvailable and
+      cp.transmissionType != structs.CarParams.TransmissionType.manual and
+      cp.fingerprintSource != structs.CarParams.FingerprintSource.fixed and len(cp.safetyConfigs) == 1 and
+      cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreap and word in (1, 3, 7) and
+      cp.radarUnavailable == (word == 1) and cp.steerControlType == structs.CarParams.SteerControlType.angle and
+      cp.steerAtStandstill and math.isfinite(cp.wheelbase) and math.isclose(cp.wheelbase, 2.96, rel_tol=0., abs_tol=1e-6) and
+      cp.steerRatio == 15. and cp.alternativeExperience in (0, 32))
+  except (AttributeError, TypeError, ValueError, IndexError, OverflowError):
+    return False
+
+
+def accel_max(speed, personality):
+  values = ACCEL_PREAP_PROFILES.get(personality, ACCEL_PREAP_PROFILES[1])
+  if speed <= ACCEL_PREAP_BP[0]:
+    return values[0]
+  for i in range(1, len(ACCEL_PREAP_BP)):
+    if speed <= ACCEL_PREAP_BP[i]:
+      r = (speed - ACCEL_PREAP_BP[i - 1]) / (ACCEL_PREAP_BP[i] - ACCEL_PREAP_BP[i - 1])
+      return values[i - 1] + r * (values[i] - values[i - 1])
+  return values[-1]
+
+
+def accel_limits(cp, speed, personality, existing):
+  if not qualified(cp) or not math.isfinite(speed):
+    return existing
+  # Current upstream/native limits always win if narrower than the original tune.
+  return max(existing[0], -1.5), min(existing[1], accel_max(speed, personality))

opendbc_repo/opendbc/car/tesla/preap/radar.py

@@ -0,0 +1,103 @@
+from opendbc.can import CANParser
+from opendbc.car import Bus, structs
+from opendbc.car.interfaces import RadarInterfaceBase
+from opendbc.car.tesla.values import CANBUS, DBC
+from opendbc.car.tesla.preap.radar_profile import qualified, valid_context
+
+_BOSCH_RADAR_STATUS_MSG = 769
+_BOSCH_RADAR_POINT_A_BASE = 784
+_BOSCH_RADAR_POINT_B_BASE = 785
+_BOSCH_RADAR_POINT_STRIDE = 3
+_BOSCH_RADAR_POINTS = 32
+_BOSCH_TRIGGER_MSG = _BOSCH_RADAR_POINT_B_BASE + ((_BOSCH_RADAR_POINTS - 1) * _BOSCH_RADAR_POINT_STRIDE)
+
+
+class PreAPRadarInterface(RadarInterfaceBase):
+  def __init__(self, CP, context=None, cp_bytes=None):
+    super().__init__(CP)
+
+    self.context_required = qualified(CP)
+    self.radar_off_can = not valid_context(CP, context, cp_bytes) or Bus.radar not in DBC[CP.carFingerprint]
+    self.updated_messages: set[int] = set()
+    self.track_id = 0
+    self.radar_offset = context.offset if not self.radar_off_can else 0.0
+
+    if self.radar_off_can:
+      self.rcp = None
+    else:
+      messages = [(_BOSCH_RADAR_STATUS_MSG, 8)]
+      for i in range(_BOSCH_RADAR_POINTS):
+        messages.append((_BOSCH_RADAR_POINT_A_BASE + (i * _BOSCH_RADAR_POINT_STRIDE), 8))
+        messages.append((_BOSCH_RADAR_POINT_B_BASE + (i * _BOSCH_RADAR_POINT_STRIDE), 8))
+      self.radar_message_addresses = frozenset(address for address, _ in messages)
+      self.rcp = CANParser(DBC[CP.carFingerprint][Bus.radar], messages, CANBUS.vehicle)
+
+    self.trigger_msg = _BOSCH_TRIGGER_MSG
+
+  def update(self, can_packets) -> structs.RadarDataT | None:
+    if self.radar_off_can or self.rcp is None:
+      result = super().update([])
+      if result is not None and self.context_required:
+        result.errors.canError = True
+        result.errors.radarUnavailableTemporary = True
+      return result
+
+    # Only the configured Bosch receiver messages on the radar bus require
+    # eight bytes. Preserve every unrelated bus/address packet unchanged.
+    radar_packets = [(stamp, [(address, data, bus) for address, data, bus in frames
+                             if bus != CANBUS.vehicle or address not in self.radar_message_addresses or len(data) == 8])
+                     for stamp, frames in can_packets]
+    vls = self.rcp.update(radar_packets)
+    self.updated_messages.update(vls)
+
+    if self.trigger_msg not in self.updated_messages:
+      return None
+
+    ret = structs.RadarData()
+    if not self.rcp.can_valid:
+      ret.errors.canError = True
+
+    radar_status = self.rcp.vl[_BOSCH_RADAR_STATUS_MSG]
+    ret.errors.radarFault = bool(radar_status["RADC_HWFail"])
+    ret.errors.radarUnavailableTemporary = False
+
+    current_points: set[int] = set()
+    for i in range(_BOSCH_RADAR_POINTS):
+      msg_a_id = _BOSCH_RADAR_POINT_A_BASE + (i * _BOSCH_RADAR_POINT_STRIDE)
+      msg_b_id = _BOSCH_RADAR_POINT_B_BASE + (i * _BOSCH_RADAR_POINT_STRIDE)
+
+      # Matching one-bit indices can also match a cached previous cycle.
+      # Require both halves since the last trigger before publishing a point.
+      if msg_a_id not in self.updated_messages or msg_b_id not in self.updated_messages:
+        continue
+
+      msg_a = self.rcp.vl[msg_a_id]
+      msg_b = self.rcp.vl[msg_b_id]
+      if msg_a["Index"] != msg_b["Index2"]:
+        continue
+
+      if not msg_a["Tracked"] or msg_a["LongDist"] <= 0.0 or msg_a["LongDist"] > 250.0 or msg_a["ProbExist"] < 50.0:
+        self.pts.pop(i, None)
+        continue
+
+      current_points.add(i)
+      if i not in self.pts:
+        self.pts[i] = structs.RadarData.RadarPoint()
+        self.pts[i].trackId = self.track_id
+        self.track_id += 1
+
+      point = self.pts[i]
+      point.dRel = msg_a["LongDist"]
+      point.yRel = msg_a["LatDist"] + self.radar_offset
+      point.vRel = msg_a["LongSpeed"]
+      point.deprecated.aRel = msg_a["LongAccel"]
+      point.deprecated.yvRel = msg_b["LatSpeed"]
+      point.deprecated.measured = bool(msg_a["Meas"])
+
+    for point_id in list(self.pts.keys()):
+      if point_id not in current_points:
+        del self.pts[point_id]
+
+    ret.points = list(self.pts.values())
+    self.updated_messages.clear()
+    return ret

opendbc_repo/opendbc/car/tesla/preap/radar_profile.py

@@ -0,0 +1,43 @@
+"""Finite radar identity and one immutable final-CarParams context."""
+from dataclasses import dataclass
+import hashlib
+import math
+import struct
+
+from capnp import KjException
+
+from opendbc.car import structs
+from opendbc.car.tesla.preap.aol import qualified as stock_qualified
+
+
+@dataclass(frozen=True)
+class RadarStartupContext:
+  version: int
+  safety_param: int
+  car_params_sha256: bytes
+  offset: float
+
+
+def qualified(cp):
+  try:
+    return bool(stock_qualified(cp) and not cp.radarUnavailable and cp.safetyConfigs[0].safetyParam in (2, 3, 6, 7))
+  except (AttributeError, TypeError, ValueError, IndexError, OverflowError):
+    return False
+
+
+def valid_context(cp, context, cp_bytes):
+  if (type(context) is not RadarStartupContext or type(context.version) is not int or context.version != 1 or
+      type(context.safety_param) is not int or not qualified(cp) or
+      type(cp_bytes) is not bytes or not 0 < len(cp_bytes) <= 1024 * 1024 or
+      context.safety_param != int(cp.safetyConfigs[0].safetyParam) or
+      type(context.car_params_sha256) is not bytes or len(context.car_params_sha256) != 32 or
+      hashlib.sha256(cp_bytes).digest() != context.car_params_sha256 or
+      type(context.offset) is not float or not math.isfinite(context.offset)):
+    return False
+  try:
+    if struct.unpack('<f', struct.pack('<f', context.offset))[0] != context.offset:
+      return False
+    with structs.CarParams.from_bytes(cp_bytes) as published:
+      return published.to_dict() == cp.to_dict()
+  except (AttributeError, ValueError, TypeError, OverflowError, RuntimeError, KjException):
+    return False

opendbc_repo/opendbc/car/tesla/preap/stock.py

@@ -41,6 +41,9 @@ class PreAPStockCarState(CarStateBase):
     self.pedal_speed_kph = 0.0
     self.di_cruise_state = "OFF"
     self.pccEvent = None
+    self.pedal_feedback = None
+    self.pedal_personality = 1
+    self.preap_accel_max = .3
     self.preap_lateral_authorized = False
     self.das_control = {}
     self.can_now_ns = self.physical_stw_stamp_ns = 0
@@ -88,18 +91,26 @@ def update_stock(cs, can_parsers):
   if speed_units is not None:
     cs.speed_units = speed_units
 
-  use_pedal = False
-  pedal_long_allowed = False
-  long_control_allowed = True
+  from opendbc.car.tesla.preap.pedal_profile import qualified as pedal_qualified, accel_max
+  use_pedal = pedal_qualified(cs.CP)
+  feedback = cs.pedal_feedback
+  feedback_ready = use_pedal and feedback is not None and feedback.ready(cp_chassis._last_update_nanos)
+  if use_pedal:
+    ret.gasPressed = bool(feedback is not None and feedback.gas_pressed())
+    if feedback is not None:
+      feedback.torque_level = float(cp_pt.vl['DI_torque1']['DI_torqueMotor'])
+    cs.preap_accel_max = accel_max(ret.vEgo, cs.pedal_personality)
+  pedal_long_allowed = feedback_ready
+  long_control_allowed = pedal_long_allowed if use_pedal else True
 
-  ret.cruiseState.available = cruise_state in ("STANDBY", "ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
+  ret.cruiseState.available = use_pedal or cruise_state in ("STANDBY", "ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
   if speed_units == "KPH":
     ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS, 1e-3)
   elif speed_units == "MPH":
     ret.cruiseState.speed = max(cp_chassis.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS, 1e-3)
   ret.cruiseState.standstill = False
   ret.standstill = ret.vEgoRaw < 0.1
-  ret.accFaulted = cruise_state == "FAULT"
+  ret.accFaulted = cruise_state == "FAULT" or (use_pedal and not feedback_ready)
 
   ret.gearShifter = GEAR_MAP[cs.can_define.dv["DI_torque2"]["DI_gear"].get(int(cp_chassis.vl["DI_torque2"]["DI_gear"]), "DI_GEAR_INVALID")]
   ret.doorOpen = any(int(cp_chassis.vl["GTW_carState"][door]) != 0 for door in _DOORS)
@@ -140,7 +151,13 @@ def update_stock(cs, can_parsers):
   )
 
   can_engage = cs.engagement.check_can_engage(ret.doorOpen, ret.gearShifter, ret.seatbeltUnlatched)
-  ret.cruiseState.enabled = can_engage and cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
+  if use_pedal and not feedback_ready:
+    cs.engagement.cruiseEnabled = cs.engagement.enableLongControl = cs.engagement.enableJustCC = False
+    cs.engagement.pending_enable = False
+  ret.cruiseState.enabled = can_engage and (cs.engagement.cruiseEnabled if use_pedal else
+    cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL"))
+  if use_pedal and cs.engagement.enableLongControl:
+    ret.cruiseState.speed = cs.engagement.pedal_speed_kph * CV.KPH_TO_MS
 
   cs.cruiseEnabled = cs.engagement.cruiseEnabled
   cs.enableLongControl = cs.engagement.enableLongControl
@@ -167,6 +184,7 @@ class PreAPStockCarController(CarControllerBase):
   def __init__(self, dbc_names, CP):
     super().__init__(dbc_names, CP)
     self.admitted = qualified(CP)
+    self.pedal_control = None
     self.codec = TeslaCANPreAP(CANPacker(dbc_names[Bus.party]))
     self.stock_cc = StockCCSpoofer()
     self.apply_angle_last = 0.0
@@ -180,7 +198,9 @@ class PreAPStockCarController(CarControllerBase):
       self.frame += 1
       return actuators, sends
     lat_active = CC.latActive and CS.hands_on_level < 3 and CS.preap_lateral_authorized
-    if CC.cruiseControl.cancel and CS.cruiseEnabled:
+    # Pedal ownership requires cancelling OEM cruise on every active cycle.
+    # Its physical cancel edge is handled by PreAPEngagement instead.
+    if self.pedal_control is None and CC.cruiseControl.cancel and CS.cruiseEnabled:
       CS.cruiseEnabled = CS.enableLongControl = CS.enableJustCC = False
       CS.pedal_speed_kph = 0.0
       CS.preap_cc_cancel_needed = True
@@ -194,6 +214,8 @@ class PreAPStockCarController(CarControllerBase):
       sends.append(self.codec.create_steering_control((self.frame // 2) % 16, self.apply_angle_last, lat_active))
       sends.append(self.codec.create_epas_control((self.frame // 2) % 16, 1))
     CS.pccEvent = None
+    if self.pedal_control is not None:
+      sends.extend(self.pedal_control.update(CC, CS, self.frame, now_nanos))
     sends.extend(self.stock_cc.update(CS, self.frame, self.codec, 0))
     if self.stock_cc.pcc_event:
       CS.pccEvent = self.stock_cc.pcc_event

opendbc_repo/opendbc/car/tesla/preap/stock_cc.py

@@ -30,6 +30,9 @@ class StockCCSpoofer:
     source_current = bool(CS.out.canValid and not CS.out.canTimeout and CS.msg_stw_actn_req and
                           0 <= CS.can_now_ns - CS.physical_stw_stamp_ns <= 300_000_000)
 
+    if getattr(CS, "pedal_feedback", None) is not None:
+      source_current = source_current and safe and not CS.out.accFaulted
+
     if getattr(CS, "preap_cc_cancel_needed", False):
       self.cancel_pending = True
       self.cancel_frame = frame

opendbc_repo/opendbc/car/tesla/preap/virtual_das.py

@@ -0,0 +1,139 @@
+import math
+
+from numpy import clip, interp
+
+from opendbc.car.common.filter_simple import FirstOrderFilter, HighPassFilter
+from opendbc.car.common.pid import PIDController
+from opendbc.car.tesla.preap.pedal_dynamics import PEDAL_RAMP_RATE_DOWN, PEDAL_RAMP_RATE_UP, PedalZeroTorque
+from opendbc.car.tesla.preap.constants import (
+  VDAS_AEGO_FILTER_RC,
+  VDAS_FUTURE_T_BP,
+  VDAS_FUTURE_T_V,
+  VDAS_INNER_K_BP,
+  VDAS_INNER_KI_V,
+  VDAS_INNER_KP_V,
+)
+from opendbc.car.tesla.preap.ff_table_default import ACCEL_BP, DEFAULT_TABLE, SPEED_BP
+from opendbc.car.tesla.preap.pedal_dynamics import ACCEL_MAX, PEDAL_BP, PEDAL_DI_MIN, PEDAL_MAX_VALUES, REGEN_MAX
+
+PID_ERROR_DEADBAND = 0.1
+GRAVITY = 9.81
+PITCH_LP_RC = 0.5
+PITCH_HP_RC1 = 0.1
+PITCH_HP_RC2 = 1.0
+MAX_PITCH_COMPENSATION = 1.5
+
+
+class GradeEstimator:
+  def __init__(self, dt: float = 0.02):
+    self.pitch_lp = FirstOrderFilter(0.0, PITCH_LP_RC, dt)
+    self.pitch_hp = HighPassFilter(0.0, PITCH_HP_RC1, PITCH_HP_RC2, dt)
+
+  def update(self, orientation_ned: list[float]) -> tuple[float, float]:
+    if len(orientation_ned) < 2:
+      return 0.0, 0.0
+    pitch = orientation_ned[1]
+    self.pitch_lp.update(pitch)
+    self.pitch_hp.update(pitch)
+    grade_accel = math.sin(self.pitch_lp.x) * GRAVITY
+    pitch_comp = float(clip(math.sin(self.pitch_hp.x) * GRAVITY, -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
+    return grade_accel, pitch_comp
+
+  def reset(self) -> None:
+    self.pitch_lp.x = 0.0
+    self.pitch_hp.x = 0.0
+    self.pitch_hp._f1.x = 0.0
+    self.pitch_hp._f2.x = 0.0
+
+
+class JerkLimiter:
+  def __init__(self, j_max: float = 2.5, dt: float = 0.02):
+    self.j_max = j_max
+    self.dt = dt
+    self.a_limited = 0.0
+
+  def update(self, a_cmd: float) -> float:
+    da_max = self.j_max * self.dt
+    self.a_limited += float(clip(a_cmd - self.a_limited, -da_max, da_max))
+    return self.a_limited
+
+  def reset(self, a_init: float = 0.0) -> None:
+    self.a_limited = a_init
+
+
+class FeedforwardModel:
+  def __init__(self):
+    self.speed_bp = list(SPEED_BP)
+    self.accel_bp = list(ACCEL_BP)
+    self.table = [list(row) for row in DEFAULT_TABLE]
+
+  def get(self, a_cmd: float, v_ego: float, zero_torque_di: float) -> float:
+    si = float(interp(v_ego, self.speed_bp, range(len(self.speed_bp))))
+    si_lo = int(si)
+    si_hi = min(si_lo + 1, len(self.speed_bp) - 1)
+    sf = si - si_lo
+    di_lo = float(interp(a_cmd, self.accel_bp, self.table[si_lo]))
+    di_hi = float(interp(a_cmd, self.accel_bp, self.table[si_hi]))
+    base_di = di_lo + sf * (di_hi - di_lo)
+
+    if a_cmd < 0:
+      blend = float(clip((a_cmd - REGEN_MAX) / (0.0 - REGEN_MAX), 0.0, 1.0))
+    else:
+      blend = float(1.0 - a_cmd / ACCEL_MAX)
+    return base_di + zero_torque_di * blend
+
+
+class VirtualDAS:
+  def __init__(self, dt: float = 0.02, zero_torque=None):
+    self.dt = dt
+    self.zero_torque = zero_torque if zero_torque is not None else PedalZeroTorque()
+    self.jerk_limiter = JerkLimiter(dt=dt)
+    self.ff_model = FeedforwardModel()
+    self.grade_estimator = GradeEstimator(dt=dt)
+    self.inner_pid = PIDController(
+      k_p=(VDAS_INNER_K_BP, VDAS_INNER_KP_V),
+      k_i=(VDAS_INNER_K_BP, VDAS_INNER_KI_V),
+      k_f=0.0,
+      pos_limit=PEDAL_RAMP_RATE_UP,
+      neg_limit=-PEDAL_RAMP_RATE_DOWN,
+      rate=1.0 / dt,
+    )
+    self.a_ego_filter = FirstOrderFilter(0.0, VDAS_AEGO_FILTER_RC, dt)
+    self.prev_a_ego_filtered = 0.0
+    self.prev_pedal_di = 0.0
+
+  def update(self, a_cmd: float, v_ego: float, prev_pedal_di: float, a_ego: float = 0.0,
+             freeze_integrator: bool = False, orientation_ned: list[float] | None = None) -> float:
+    a_limited = self.jerk_limiter.update(a_cmd)
+    grade_accel, pitch_comp = self.grade_estimator.update(orientation_ned or [])
+
+    ff_di = self._feedforward(a_limited, v_ego) + pitch_comp
+    a_ego_corrected = a_ego - grade_accel
+    a_ego_filtered = self.a_ego_filter.update(a_ego_corrected)
+    j_ego = (a_ego_filtered - self.prev_a_ego_filtered) / self.dt
+    self.prev_a_ego_filtered = a_ego_filtered
+
+    future_t = float(interp(v_ego, VDAS_FUTURE_T_BP, VDAS_FUTURE_T_V))
+    error = a_limited - (a_ego_filtered + j_ego * future_t)
+    if abs(error) < PID_ERROR_DEADBAND:
+      error = 0.0
+
+    pedal_di = ff_di + float(self.inner_pid.update(error, speed=v_ego, freeze_integrator=freeze_integrator))
+    max_pedal_value = float(interp(v_ego, PEDAL_BP, PEDAL_MAX_VALUES))
+    pedal_di = float(clip(pedal_di, PEDAL_DI_MIN, max_pedal_value))
+    pedal_di = float(clip(pedal_di, prev_pedal_di - PEDAL_RAMP_RATE_DOWN, prev_pedal_di + PEDAL_RAMP_RATE_UP))
+    self.prev_pedal_di = pedal_di
+    return pedal_di
+
+  def reset(self, a_init: float = 0.0, pedal_di_init: float = 0.0) -> None:
+    self.jerk_limiter.reset(a_init)
+    self.inner_pid.reset()
+    self.grade_estimator.reset()
+    self.a_ego_filter.x = 0.0
+    self.prev_a_ego_filtered = 0.0
+    self.prev_pedal_di = pedal_di_init
+
+  def _feedforward(self, a_cmd: float, v_ego: float) -> float:
+    zero_torque_di = self.zero_torque.get(v_ego)
+    max_pedal_value = float(interp(v_ego, PEDAL_BP, PEDAL_MAX_VALUES))
+    return float(clip(self.ff_model.get(a_cmd, v_ego, zero_torque_di), PEDAL_DI_MIN, max_pedal_value))

opendbc_repo/opendbc/car/tesla/radar_interface.py

@@ -1,6 +1,7 @@
 from opendbc.can import CANParser
 from opendbc.car import Bus, structs
 from opendbc.car.interfaces import RadarInterfaceBase
+from opendbc.car.tesla.preap.radar import PreAPRadarInterface
 from opendbc.car.tesla.radar_hw1 import HW1RadarInterface
 from opendbc.car.tesla.values import CAR, DBC
 
@@ -25,6 +26,9 @@ def get_radar_can_parser(CP):
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
+    if CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      self.preap = PreAPRadarInterface(CP)
+      return
     if CP.carFingerprint == CAR.TESLA_MODEL_S_HW1:
       self.hw1 = HW1RadarInterface(CP)
       return
@@ -34,13 +38,22 @@ class RadarInterface(RadarInterfaceBase):
     self.radar_off_can = CP.radarUnavailable
     self.rcp = get_radar_can_parser(CP)
 
-  def update(self, can_strings):
-    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_HW1:
-      return self.hw1.update(can_strings)
-    if self.radar_off_can or self.rcp is None:
-      return super().update(None)
+  def configure_preap_radar(self, context, cp_bytes):
+    if self.CP.carFingerprint != CAR.TESLA_MODEL_S_PREAP:
+      raise ValueError("Radar context is only valid for Tesla PREAP")
+    self.preap = PreAPRadarInterface(self.CP, context, cp_bytes)
+    if self.preap.radar_off_can and not self.CP.radarUnavailable:
+      raise ValueError("Invalid final Tesla radar startup context")
 
-    vls = self.rcp.update(can_strings)
+  def update(self, can_packets) -> structs.RadarDataT | None:
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP:
+      return self.preap.update(can_packets)
+    if self.CP.carFingerprint == CAR.TESLA_MODEL_S_HW1:
+      return self.hw1.update(can_packets)
+    if self.radar_off_can or self.rcp is None:
+      return super().update([])
+
+    vls = self.rcp.update(can_packets)
     self.updated_messages.update(vls)
 
     if self.trigger_msg not in self.updated_messages:

opendbc_repo/opendbc/car/tesla/tests/test_preap_radar.py

@@ -0,0 +1,76 @@
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car import structs
+from opendbc.car.tesla.tests.test_preap_stock import params
+from opendbc.car.tesla.radar_interface import RadarInterface
+
+
+def radar_params(offset=0.0, mount=False):
+  cp = params()
+  cp.radarUnavailable = False
+  cp.safetyConfigs[0].safetyParam = 6 if mount else 2
+  return cp
+
+
+def radar_interface(cp, offset=0.0, cp_bytes=None):
+  import hashlib
+  from opendbc.car.tesla.preap.radar_profile import RadarStartupContext
+  from openpilot.starpilot.car.tesla.radar_settings import offset_float32
+  ri = RadarInterface(cp)
+  raw = cp.to_bytes() if cp_bytes is None else cp_bytes
+  ri.configure_preap_radar(RadarStartupContext(1, int(cp.safetyConfigs[0].safetyParam),
+                                              hashlib.sha256(raw).digest(), offset_float32(offset)), raw)
+  return ri
+
+
+def frames(packer, *, tracked=True, probability=75.0, distance=30.0, index=0, measured=True, fault=False):
+  result = [packer.make_can_msg("TeslaRadarSguInfo", 1, {"RADC_HWFail": fault})]
+  for i in range(32):
+    result.append(packer.make_can_msg(f"RadarPoint{i}_A", 1, {
+      "Index": 0, "Tracked": tracked if i == 0 else False, "ProbExist": probability,
+      "LongDist": distance, "LatDist": 1.0, "LongSpeed": -2.0, "LongAccel": 0.5, "Meas": measured,
+    }))
+    result.append(packer.make_can_msg(f"RadarPoint{i}_B", 1, {"Index2": index, "LatSpeed": 0.25}))
+  return result
+
+
+@pytest.mark.parametrize("mount", (False, True))
+def test_actual_decoder_offset_metadata_and_cycle_trigger(mount):
+  cp = radar_params(0.25, mount)
+  raw = cp.to_bytes()
+  with structs.CarParams.from_bytes(raw) as saved:
+    ri = radar_interface(saved, 0.25, cp_bytes=raw)
+  packer = CANPacker("tesla_radar_bosch_generated")
+  ri.update([])
+  for tick in range(1, 5):
+    result = ri.update([(tick * 125_000_000, frames(packer))])
+  assert len(result.points) == 1
+  point = result.points[0]
+  assert point.dRel == 30.0 and point.yRel == 1.25 and point.vRel == -2.0
+  assert point.deprecated.measured and point.deprecated.aRel == 0.5 and point.deprecated.yvRel == 0.25
+  assert ri.update([(625_000_000, frames(packer)[:-1])]) is None
+
+
+@pytest.mark.parametrize("changes", ({"tracked": False}, {"probability": 46.875}, {"distance": 0.0},
+                                     {"distance": 251.0}, {"index": 1}))
+def test_original_index_probability_distance_and_track_retirement(changes):
+  ri, packer = radar_interface(radar_params()), CANPacker("tesla_radar_bosch_generated")
+  ri.update([])
+  for tick in range(1, 5):
+    result = ri.update([(tick * 125_000_000, frames(packer))])
+  assert len(result.points) == 1
+  result = ri.update([(625_000_000, frames(packer, **changes))])
+  assert not result.points
+
+
+def test_source_expiry_and_hardware_fault_are_published():
+  ri, packer = radar_interface(radar_params()), CANPacker("tesla_radar_bosch_generated")
+  ri.update([])
+  for tick in range(1, 5):
+    result = ri.update([(tick * 125_000_000, frames(packer, fault=True))])
+  assert result.errors.radarFault
+  trigger = packer.make_can_msg("RadarPoint31_B", 1, {"Index2": 0})
+  for step in range(5):
+    result = ri.update([(2_000_000_000 + step * 125_000_000, [trigger])])
+  assert result.errors.canError

opendbc_repo/opendbc/car/tesla/tests/test_preap_radar_receiver.py

@@ -0,0 +1,81 @@
+"""Finite actual parser/receiver negatives; never an upstream struct override."""
+import pytest
+
+from opendbc.can import CANPacker
+from opendbc.car.tesla.tests.test_preap_radar import frames, radar_params, radar_interface
+from openpilot.cereal import log, messaging
+
+
+@pytest.mark.parametrize('mount', (False, True))
+@pytest.mark.parametrize('fault', ('wrong_bus', 0, 1, 7, 9, 12, 16, 64))
+def test_bad_receiver_frames_do_not_refresh_bosch_cycle_or_hide_expiry(mount, fault):
+  cp = radar_params(mount=mount)
+  ri = radar_interface(cp, 0.25)
+  packer = CANPacker('tesla_radar_bosch_generated')
+  ri.update([])
+  for cycle in range(1, 5):
+    rr = ri.update([(cycle * 125_000_000, frames(packer))])
+  assert len(rr.points) == 1 and rr.points[0].yRel == 1.25
+  before = {a: dict(ri.preap.rcp.ts_nanos[a]) for a in ri.preap.radar_message_addresses}
+  bad = [(a, d, 2) if fault == 'wrong_bus' else (a, d[:fault] if fault < 8 else d + bytes(fault - 8), b)
+         for a, d, b in frames(packer)]
+  for cycle in range(5, 17):
+    assert ri.update([(cycle * 125_000_000, bad)]) is None
+  # Original decoder publishes only on the final B trigger. Do not invent an
+  # immediate error publication or immediate cached-point retirement.
+  trigger = packer.make_can_msg('RadarPoint31_B', 1, {'Index2': 0})
+  assert {a: dict(ri.preap.rcp.ts_nanos[a]) for a in ri.preap.radar_message_addresses} == before
+  # Honor the current parser's five-invalid-read debounce.
+  for step in range(5):
+    rr = ri.update([(2_125_000_000 + step * 125_000_000, [trigger])])
+  assert rr is not None and rr.errors.canError
+  event = messaging.new_message('radarTracks', valid=False)
+  event.radarTracks = rr
+  with log.Event.from_bytes(event.to_bytes()) as received:
+    assert received.which() == 'radarTracks' and not received.valid
+    assert received.radarTracks.errors.canError
+
+
+@pytest.mark.parametrize('mount', (False, True))
+def test_genuine_eight_byte_receiver_and_unrelated_frames_are_preserved(mount, monkeypatch):
+  ri = radar_interface(radar_params(mount=mount), 0.25)
+  packer = CANPacker('tesla_radar_bosch_generated')
+  actual_parser = ri.preap.rcp.update
+  observed = []
+
+  def inspect_then_parse(packets):
+    observed.extend(packets)
+    return actual_parser(packets)
+
+  monkeypatch.setattr(ri.preap.rcp, 'update', inspect_then_parse)
+  data = frames(packer) + [(0x310, bytes(12), 2), (0x700, bytes(64), 1), (0x301, bytes(3), 0)]
+  for cycle in range(1, 5):
+    packet = (cycle * 125_000_000, data)
+    rr = ri.update([packet])
+    assert observed[-1] == packet
+    assert all(before[1] is after[1] for before, after in zip(data, observed[-1][1], strict=True))
+  assert not rr.errors.canError and len(rr.points) == 1 and rr.points[0].yRel == 1.25
+
+
+@pytest.mark.parametrize('mount', (False, True))
+@pytest.mark.parametrize('missing', ('a', 'b', 'both', 'short_a', 'short_b', 'trigger_only'))
+def test_current_pair_is_required_before_republishing_a_point(mount, missing):
+  ri = radar_interface(radar_params(mount=mount), 0.25)
+  packer = CANPacker('tesla_radar_bosch_generated')
+  for cycle in range(1, 5):
+    rr = ri.update([(cycle * 125_000_000, frames(packer))])
+  assert len(rr.points) == 1
+  previous_id = rr.points[0].trackId
+  batch = frames(packer)
+  omitted = {'a': (784,), 'b': (785,), 'both': (784, 785),
+             'short_a': (784,), 'short_b': (785,)}.get(missing, ())
+  if missing == 'trigger_only':
+    batch = [batch[-1]]
+  elif missing.startswith('short'):
+    batch = [(a, d[:-1] if a in omitted else d, b) for a, d, b in batch]
+  else:
+    batch = [frame for frame in batch if frame[0] not in omitted]
+  rr = ri.update([(625_000_000, batch)])
+  assert not rr.points
+  rr = ri.update([(750_000_000, frames(packer))])
+  assert len(rr.points) == 1 and rr.points[0].trackId != previous_id

opendbc_repo/opendbc/car/tesla/tests/test_preap_startup.py

@@ -1,4 +1,5 @@
 import gc
+import json
 import time
 
 import pytest
@@ -12,7 +13,7 @@ from opendbc.car.tesla.fingerprints import FINGERPRINTS
 CARS = (CAR.TESLA_MODEL_S_PREAP,)
 
 
-def exercise_startup(identity, alpha, release, aol, monkeypatch, mapping=(0, 0), probe=None):
+def exercise_startup(identity, alpha, release, aol, monkeypatch, mapping=(0, 0), probe=None, radar_mode=None, radar_offset=0.0):
   from opendbc.car import car_helpers
   from openpilot.cereal import messaging
   from openpilot.common.params import Params
@@ -39,10 +40,18 @@ def exercise_startup(identity, alpha, release, aol, monkeypatch, mapping=(0, 0),
     if probe is not None:
       saved.put("CancelButtonControl", 3, block=True)
 
+    if radar_mode is not None:
+      from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY, encode
+      saved.put(DOCUMENT_KEY, json.loads(encode(radar_mode, radar_offset)), block=True)
     initial = CarInterface.get_params(identity, fingerprint, [], card_module.alpha_long_requested(saved, is_release=release), release, False)
     initial.carVin = "0" * 17
     initial.carFw = []
     assert initial.safetyConfigs[0].safetyParam == 0
+    if radar_mode is not None:
+      from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+      expected_preferences = VehicleStartupPreferences.read(saved, enabled=True)
+      expected_preferences.prepare(initial, fingerprints=fingerprint)
+      expected_preferences.finalize(initial)
     expected = initial.to_dict()
     explicit_owner = True
     assert not initial.openpilotLongitudinalControl and initial.pcmCruise and not initial.dashcamOnly
@@ -65,6 +74,12 @@ def exercise_startup(identity, alpha, release, aol, monkeypatch, mapping=(0, 0),
       assert policy_for(card.CP).fixed_cruise_buttons
       assert card.aol_qualified
       assert int(card.CP.safetyConfigs[0].safetyModel.raw) == 39
+      if radar_mode is not None:
+        assert card.tesla_preap_radar_context is not None
+        assert card.RI.preap.radar_offset == radar_offset
+        # Settings mutation cannot change the immutable decoder context.
+        saved.put(DOCUMENT_KEY, json.loads(encode("nosecone" if radar_mode == "front" else "front", radar_offset + 1)), block=True)
+        assert card.RI.preap.radar_offset == radar_offset
       assert card.aol_card_intent.explicit_latch is explicit_owner
       assert not card.aol_card_intent.allowed_latch
       assert card.aol_card_intent.settings.main_action == 0

opendbc_repo/opendbc/car/tesla/tests/test_preap_stock.py

@@ -1,6 +1,7 @@
 import pytest
 
 from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.can_definitions import CanData
 from opendbc.car.tesla.interface import CarInterface
 from opendbc.car.tesla.values import CAR
 from opendbc.car.tesla.preap.aol import qualified
@@ -23,8 +24,9 @@ def test_stock_factory_and_serialized_profile(alpha, release):
     assert qualified(decoded)
 
 
-@pytest.mark.parametrize("key", ("NAPPedalEnabled", "NAPRadarEnabled", "NAPRadarBehindNosecone"))
-@pytest.mark.parametrize("raw", (b"1", b"invalid", b""))
+@pytest.mark.parametrize("key,raw", (("NAPPedalEnabled", b"1"), ("NAPPedalEnabled", b"invalid"), ("NAPPedalEnabled", b""),
+  ("NAPRadarEnabled", b"invalid"), ("NAPRadarEnabled", b""),
+  ("NAPRadarBehindNosecone", b"1"), ("NAPRadarBehindNosecone", b"invalid"), ("NAPRadarBehindNosecone", b"")))
 def test_unsupported_hardware_configuration_never_falls_back_to_stock(key, raw, tmp_path):
   from openpilot.starpilot.car.tesla.preap_preferences import stock_configuration, prepare_stock
 
@@ -120,7 +122,7 @@ def test_controller_attempted_cancel_echo_requires_fresh_native_reconciliation()
   assert emitted[0] & 63 == 1
   previous_physical_stamp = ci.CS.preap.physical_stw_stamp_ns
   now += 10_000_000
-  frames = [(address, emitted if address == 0x45 else data, bus)
+  frames = [CanData(address, emitted if address == 0x45 else data, bus)
             for address, data, bus in stream.frames(tick + 1)]
   cs = ci.update([(now, frames)])
   assert ci.CS.preap.cancel_echo_stamp_ns == now

opendbc_repo/opendbc/car/tesla/values.py

@@ -71,6 +71,7 @@ class CAR(Platforms):
       Bus.party: 'tesla_can',
       Bus.pt: 'tesla_can',
       Bus.chassis: 'tesla_can',
+      Bus.radar: 'tesla_radar_bosch_generated',
     },
   )
 

opendbc_repo/opendbc/safety/modes/tesla_preap.h

@@ -1,7 +1,11 @@
 #pragma once
 #include "opendbc/safety/declarations.h"
+#include "tesla_preap_pedal.h"
 
 static bool tesla_preap_admitted = false;
+static bool tesla_preap_pedal_long_armed = false;
+static bool tesla_preap_pedal_pull_seen = false;
+static uint32_t tesla_preap_pedal_pull_ts = 0U;
 static bool tesla_preap_doors_open = true;
 static int tesla_preap_gear = 0;
 static int tesla_preap_lever_previous = 2;
@@ -26,6 +30,11 @@ static uint32_t tesla_preap_stw_ts = 0U;
 static uint32_t tesla_preap_echo_ts = 0U;
 
 static void tesla_preap_clear(void) {
+  if (tesla_preap_pedal_configured) { pcm_cruise_check(false); }
+  tesla_preap_pedal_long_armed = false;
+  tesla_preap_pedal_pull_seen = false;
+  tesla_preap_pedal_tx_seen = false;
+  tesla_preap_pedal_tx_enabled = false;
   tesla_preap_engage_seen = false;
   tesla_preap_pending_physical = false;
   tesla_preap_lever_previous = 2;
@@ -58,6 +67,8 @@ static uint8_t tesla_preap_crc(const CANPacket_t *msg) {
 }
 
 
+#include "tesla_preap_radar.h"
+
 static uint32_t tesla_preap_checksum(const CANPacket_t *msg) { return msg->data[7]; }
 static uint32_t tesla_preap_compute_checksum(const CANPacket_t *msg) { return tesla_preap_crc(msg); }
 
@@ -83,7 +94,8 @@ static uint8_t tesla_preap_request_mask(void) {
   uint8_t request = 0U;
   const bool expired = tesla_preap_requested &&
     (safety_get_ts_elapsed(microsecond_timer_get(), aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US);
-  if (expired || relay_malfunction || !aol_rx_healthy() || !tesla_preap_sources_ready(false)) {
+  if (expired || relay_malfunction || !aol_rx_healthy() || !tesla_preap_sources_ready(false) ||
+      (tesla_preap_pedal_configured && !tesla_preap_pedal_feedback_ready(microsecond_timer_get()))) {
     tesla_preap_pending_physical = false;
     tesla_preap_clear();
   } else if (!heartbeat_engaged) {
@@ -95,18 +107,47 @@ static uint8_t tesla_preap_request_mask(void) {
       tesla_preap_engage_seen = true;
       tesla_preap_pending_physical = false;
     }
-    request = aol_host_axis_mask & 1U;
+    request = aol_host_axis_mask & (tesla_preap_pedal_configured ? 3U : 1U);
   }
   return request;
 }
 
 static uint8_t tesla_preap_permission(void) {
   const uint8_t request = tesla_preap_request_mask();
-  return (tesla_preap_engage_seen && (tesla_preap_gear == 4) && !tesla_preap_doors_open &&
-    !steering_disengage && !tesla_preap_eps_temporary) ? request : 0U;
+  const bool lateral_ready = tesla_preap_engage_seen && (tesla_preap_gear == 4) && !tesla_preap_doors_open &&
+    !steering_disengage && !tesla_preap_eps_temporary;
+  uint8_t permission = lateral_ready ? (request & 1U) : 0U;
+  const bool ordinary_ready = tesla_preap_engage_seen && (tesla_preap_gear == 4) && !tesla_preap_doors_open &&
+    !steering_disengage && !tesla_preap_eps_fault;
+  if (tesla_preap_pedal_configured && tesla_preap_pedal_long_armed && tesla_preap_pedal_tx_seen && ordinary_ready && controls_allowed && !brake_pressed && !gas_pressed &&
+      tesla_preap_sources_ready(true) && tesla_preap_pedal_feedback_ready(microsecond_timer_get())) {
+    permission |= request & 2U;
+  }
+  return permission;
 }
 
 static void tesla_preap_optional_rx(const CANPacket_t *msg) {
+  tesla_preap_radar_rx(msg);
+  if ((msg->addr == 0x552U) && tesla_preap_pedal_configured && (msg->bus == tesla_preap_pedal_bus)) {
+    bool ready = false;
+    if (GET_LEN(msg) == 6U) {
+      ready = tesla_preap_pedal_feedback(msg->data, msg->bus, microsecond_timer_get());
+    } else {
+      tesla_preap_pedal_feedback_healthy = false;
+      tesla_preap_pedal_feedback_seen = false;
+    }
+    gas_pressed = !ready || (tesla_preap_pedal_feedback_di > 2.0F);
+    if (!ready) {
+      controls_allowed = false;
+      tesla_preap_clear();
+    } else if (gas_pressed) {
+      // Preserve the existing engaged mode through gas override, but require a
+      // real zero/disabled command before any resumed active demand.
+      tesla_preap_pedal_tx_seen = false;
+      tesla_preap_pedal_tx_enabled = false;
+    }
+  }
+
   if (msg_matches(msg, 0x201U, 0U, 5U)) {
     tesla_preap_belt_seen = true;
     tesla_preap_belt_ts = microsecond_timer_get();
@@ -115,12 +156,18 @@ static void tesla_preap_optional_rx(const CANPacket_t *msg) {
     tesla_preap_belt_latched = buckle == 1U;
     if (!tesla_preap_belt_latched) {
       controls_allowed = false;
+      if (tesla_preap_pedal_configured) { pcm_cruise_check(false); }
+      tesla_preap_pedal_long_armed = false;
+      tesla_preap_pedal_pull_seen = false;
+      tesla_preap_pedal_tx_seen = false;
+      tesla_preap_pedal_tx_enabled = false;
       if (!tesla_preap_axis || !tesla_preap_belt_valid) { tesla_preap_clear(); }
     }
   }
 }
 
 static void tesla_preap_rx_hook(const CANPacket_t *msg) {
+  if (msg->addr == 0x552U) { tesla_preap_optional_rx(msg); }
   if (msg->bus == 0U) {
     if (msg->addr == 0x370U) {
       const unsigned int angle_raw = ((msg->data[4] & 0x3FU) << 8U) | msg->data[5];
@@ -138,7 +185,7 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
       UPDATE_VEHICLE_SPEED(speed);
       vehicle_moving = speed > (0.5F * KPH_TO_MS);
     }
-    if (msg->addr == 0x108U) { gas_pressed = msg->data[6] != 0U; }
+    if ((msg->addr == 0x108U) && !tesla_preap_pedal_configured) { gas_pressed = msg->data[6] != 0U; }
     if (msg->addr == 0x20AU) {
       const unsigned int brake = (msg->data[0] >> 2U) & 3U;
       tesla_preap_brake_valid = (brake == 1U) || (brake == 2U);
@@ -154,8 +201,19 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
     }
     if ((tesla_preap_gear != 4) || tesla_preap_doors_open || steering_disengage || brake_pressed) {
       controls_allowed = false;
-      if ((tesla_preap_gear != 4) || tesla_preap_doors_open || steering_disengage) { tesla_preap_clear(); }
+      if (tesla_preap_pedal_configured) { pcm_cruise_check(false); }
+      tesla_preap_pedal_long_armed = false;
+      tesla_preap_pedal_pull_seen = false;
+      tesla_preap_pedal_tx_seen = false;
+      tesla_preap_pedal_tx_enabled = false;
+      // Known Reverse/Neutral withdraw every axis but retain an established
+      // physical arm. Permission still requires current Drive and source health.
+      const bool gear_paused = (tesla_preap_gear == 2) || (tesla_preap_gear == 3);
+      if (((tesla_preap_gear != 4) && !gear_paused) || tesla_preap_doors_open || steering_disengage) {
+        tesla_preap_clear();
+      }
     }
+    if (msg->addr != 0x45U) { tesla_preap_radar_rx(msg); }
     if (msg->addr == 0x45U) {
       bool echo = tesla_preap_echo_pending &&
         (safety_get_ts_elapsed(microsecond_timer_get(), tesla_preap_echo_ts) <= 100000U);
@@ -179,6 +237,14 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
           const int lever = msg->data[0] & 0x3FU;
           if (established_counter && (lever == 2) && (tesla_preap_lever_previous == 0) && tesla_preap_sources_ready(!tesla_preap_axis) &&
               (tesla_preap_gear == 4) && !tesla_preap_doors_open && !steering_disengage && (!brake_pressed || tesla_preap_axis)) {
+            if (tesla_preap_pedal_configured) {
+              const uint32_t now = microsecond_timer_get();
+              tesla_preap_pedal_long_armed = tesla_preap_pedal_pull_seen && !brake_pressed &&
+                tesla_preap_pedal_feedback_ready(now) &&
+                (safety_get_ts_elapsed(now, tesla_preap_pedal_pull_ts) < 500000U);
+              tesla_preap_pedal_pull_seen = true;
+              tesla_preap_pedal_pull_ts = now;
+            }
             if (tesla_preap_belt_latched) { pcm_cruise_check(true); }
             tesla_preap_engage_ts = microsecond_timer_get();
             tesla_preap_engage_seen = !tesla_preap_axis;
@@ -190,6 +256,7 @@ static void tesla_preap_rx_hook(const CANPacket_t *msg) {
           } else {
           }
           tesla_preap_lever_previous = lever;
+          tesla_preap_radar_rx(msg);
         }
       }
     }
@@ -200,7 +267,16 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) {
   const AngleSteeringLimits limits = {.max_angle = 3600, .angle_deg_to_can = 10, .frequency = 50U};
   const AngleSteeringParams vm = {.slip_factor = -0.0005666F, .steer_ratio = 15.0F, .wheelbase = 2.96F};
   bool accepted = false;
-  if (msg->addr == 0x214U) {
+  if (msg->addr == 0x551U) {
+    const bool allowed = tesla_preap_admitted && tesla_preap_pedal_long_armed && tesla_preap_sources_ready(true) &&
+      (tesla_preap_gear == 4) && !tesla_preap_doors_open && !brake_pressed && !steering_disengage &&
+      !tesla_preap_eps_fault && get_longitudinal_allowed();
+    return tesla_preap_pedal_tx(msg->data, msg->bus, microsecond_timer_get(),
+      (float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, allowed);
+  }
+  if (msg->bus == 1U) {
+    accepted = tesla_preap_radar_tx(msg);
+  } else if (msg->addr == 0x214U) {
     const unsigned int sum = 0x14U + 2U + msg->data[0] + msg->data[1];
     accepted = tesla_preap_admitted && ((msg->data[0] & 7U) <= 1U) &&
       ((msg->data[0] & 248U) == 0U) && ((msg->data[1] & 240U) == 0U) &&
@@ -209,7 +285,8 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) {
     const int lever = msg->data[0] & 63U;
     accepted = tesla_preap_admitted && tesla_preap_stw_seen && tesla_preap_sources_ready(true) &&
       (safety_get_ts_elapsed(microsecond_timer_get(), tesla_preap_stw_ts) <= 300000U) &&
-      ((lever == 1) || ((lever == 16) && tesla_preap_engage_seen && !brake_pressed)) &&
+      ((lever == 1) || ((lever == 16) && tesla_preap_engage_seen && !brake_pressed &&
+       (tesla_preap_gear == 4) && !tesla_preap_doors_open && !steering_disengage && !tesla_preap_eps_temporary)) &&
       ((msg->data[0] & 192U) == 64U) && (tesla_preap_crc(msg) == msg->data[7]) &&
       ((msg->data[6] >> 4U) == (((tesla_preap_stw[6] >> 4U) + 1U) & 15U));
     for (int i = 1; i < 6; i++) { accepted = accepted && (msg->data[i] == tesla_preap_stw[i]); }
@@ -242,9 +319,15 @@ static bool tesla_preap_fwd_hook(int bus, int addr) {
 }
 
 static safety_config tesla_preap_init(uint16_t param) {
-  tesla_preap_admitted = (param == 0U) && ((alternative_experience == 0) || (alternative_experience == 32));
+  const bool pedal = ((param == 1U) || (param == 3U) || (param == 7U)) && tesla_preap_pedal_profile_init(param);
+  if ((param != 1U) && (param != 3U) && (param != 7U)) { tesla_preap_pedal_staged_ready = false; }
+  tesla_preap_admitted = (((param == 0U) || (param == 2U) || (param == 6U)) || pedal) &&
+    ((alternative_experience == 0) || (alternative_experience == 32));
   tesla_preap_axis = alternative_experience == 32;
   tesla_preap_reset();
+  tesla_preap_radar_enabled = tesla_preap_admitted && ((param & 2U) != 0U);
+  tesla_preap_radar_nosecone = (param & 4U) != 0U;
+  tesla_preap_radar_ticket = NULL;
   tesla_preap_doors_open = true;
   tesla_preap_brake_valid = false;
   tesla_preap_eps_fault = true;
@@ -256,6 +339,8 @@ static safety_config tesla_preap_init(uint16_t param) {
     {0x488, 0, 4, .check_relay = false, .disable_static_blocking = true},
     {0x214, 0, 3, .check_relay = false, .disable_static_blocking = true},
     {0x45, 0, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x551, 0, 6, .check_relay = false, .disable_static_blocking = true},
+    {0x551, 2, 6, .check_relay = false, .disable_static_blocking = true},
   };
   if (tesla_preap_admitted && tesla_preap_axis) {
     static const AolSafetyPolicy policy = {
@@ -265,6 +350,26 @@ static safety_config tesla_preap_init(uint16_t param) {
     };
     aol_policy = &policy;
   }
+  static const CanMsg radar_tx[] = {
+    {0x488, 0, 4, .check_relay = false, .disable_static_blocking = true},
+    {0x214, 0, 3, .check_relay = false, .disable_static_blocking = true},
+    {0x45, 0, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x551, 0, 6, .check_relay = false, .disable_static_blocking = true},
+    {0x551, 2, 6, .check_relay = false, .disable_static_blocking = true},
+    {0x219, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x109, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x149, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x159, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x209, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x2d9, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x2b9, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x2a9, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x199, 1, 8, .check_relay = false, .disable_static_blocking = true},
+    {0x129, 1, 6, .check_relay = false, .disable_static_blocking = true},
+    {0x1a9, 1, 5, .check_relay = false, .disable_static_blocking = true},
+    {0x119, 1, 6, .check_relay = false, .disable_static_blocking = true},
+    {0x169, 1, 8, .check_relay = false, .disable_static_blocking = true},
+  };
   static RxCheck rx[] = {
     {.msg = {{0x370, 0, 8, 25U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
     {.msg = {{0x108, 0, 8, 100U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
@@ -274,8 +379,16 @@ static safety_config tesla_preap_init(uint16_t param) {
     {.msg = {{0x318, 0, 8, 10U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
     {.msg = {{0x45, 0, 8, 10U, .ignore_quality_flag = true, .ignore_counter = true}, {0}, {0}}},
     {.msg = {{0x155, 0, 8, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
+    {.msg = {{0x552, 0, 6, 50U, .ignore_quality_flag = true, .ignore_checksum = true, .ignore_counter = true}, {0}, {0}}},
   };
   safety_config config = BUILD_SAFETY_CFG(rx, tx);
+  if (pedal) {
+    config.rx_checks[config.rx_checks_len - 1].msg[0].bus = tesla_preap_pedal_bus;
+    config.rx_checks[config.rx_checks_len - 1].status = (RxStatus){0};
+  } else {
+    config.rx_checks_len--;
+  }
+  if (tesla_preap_radar_enabled) { SET_TX_MSGS(radar_tx, config); }
   if (!tesla_preap_admitted) { config.tx_msgs_len = 0; }
   return config;
 }

opendbc_repo/opendbc/safety/modes/tesla_preap_pedal.h

@@ -0,0 +1,223 @@
+#pragma once
+#include <stdint.h>
+#include <stdbool.h>
+
+static bool tesla_preap_pedal_finite(float value) {
+  union { float value; uint32_t bits; } word = {.value = value};
+  return (word.bits & 0x7F800000U) != 0x7F800000U;
+}
+static float tesla_preap_pedal_abs(float value) { return value < 0.0F ? -value : value; }
+
+// Fork-local startup USB envelope. No CAN packet configures these values.
+#define TESLA_PREAP_PEDAL_CONFIG_WORDS 24U
+static uint16_t tesla_preap_pedal_staged[24];
+static uint16_t tesla_preap_pedal_active_words[24];
+static unsigned int tesla_preap_pedal_next_word = 0U;
+static bool tesla_preap_pedal_staged_ready = false;
+static bool tesla_preap_pedal_configured = false;
+static float tesla_preap_pedal_factor = 0.0F;
+static float tesla_preap_pedal_zero = 0.0F;
+static unsigned int tesla_preap_pedal_bus = 0U;
+static bool tesla_preap_pedal_feedback_seen = false;
+static bool tesla_preap_pedal_feedback_healthy = false;
+static uint8_t tesla_preap_pedal_feedback_counter = 0U;
+static uint32_t tesla_preap_pedal_feedback_ts = 0U;
+static float tesla_preap_pedal_feedback_di = 0.0F;
+static unsigned int tesla_preap_pedal_feedback_raw_one = 0U;
+static unsigned int tesla_preap_pedal_feedback_raw_two = 0U;
+static bool tesla_preap_pedal_tx_seen = false;
+static bool tesla_preap_pedal_tx_enabled = false;
+static uint8_t tesla_preap_pedal_tx_counter = 0U;
+static unsigned int tesla_preap_pedal_last_raw_one = 0U;
+static unsigned int tesla_preap_pedal_last_raw_two = 0U;
+
+static float tesla_preap_pedal_word_float(const uint16_t *words) {
+  union { uint32_t bits; float value; } decoded;
+  decoded.bits = (uint32_t)words[0] | ((uint32_t)words[1] << 16U);
+  return decoded.value;
+}
+
+static bool tesla_preap_pedal_config_valid(const uint16_t *words) {
+  bool hash_present = false;
+  for (unsigned int i = 8U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) { hash_present |= words[i] != 0U; }
+  const float factor = tesla_preap_pedal_word_float(&words[4]);
+  const float zero = tesla_preap_pedal_word_float(&words[6]);
+  bool valid = (words[0] == 1U) && ((words[1] == 0U) || (words[1] == 2U)) &&
+    ((words[2] == 1U) || (words[2] == 3U) || (words[2] == 7U)) && (words[3] == 0xD051U) &&
+    tesla_preap_pedal_finite(factor) && (tesla_preap_pedal_abs(factor) > 0.000001F) && tesla_preap_pedal_finite(zero) && hash_present;
+  if (!valid) { return false; }
+  const float demands[2] = {-5.0F, 90.0F};
+  for (unsigned int d = 0U; d < 2U; d++) {
+    const float physical = zero + (demands[d] / factor) + 22.85856576F;
+    const float raw_one = physical / 0.050796813F, raw_two = physical / 0.101593626F;
+    const unsigned int one = raw_one < 0.0F ? 0U : raw_one > 65534.0F ? 65534U : (unsigned int)raw_one;
+    const unsigned int two = raw_two < 0.0F ? 0U : raw_two > 65534.0F ? 65534U : (unsigned int)raw_two;
+    const int difference = (int)one - (2 * (int)two);
+    valid &= difference >= 0 && difference <= 1;
+  }
+  return valid;
+}
+
+static inline bool tesla_preap_pedal_config_word(uint16_t index, uint16_t value, bool may_configure) {
+  if (!may_configure || tesla_preap_pedal_configured) { return false; }
+  if (index == 0U) {
+    tesla_preap_pedal_next_word = 0U;
+    tesla_preap_pedal_staged_ready = false;
+  }
+  if (index == 0xFFFFU) {
+    tesla_preap_pedal_staged_ready = (value == 0xC051U) &&
+      (tesla_preap_pedal_next_word == TESLA_PREAP_PEDAL_CONFIG_WORDS) &&
+      tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
+    return tesla_preap_pedal_staged_ready;
+  }
+  if ((index != tesla_preap_pedal_next_word) || (index >= TESLA_PREAP_PEDAL_CONFIG_WORDS)) {
+    tesla_preap_pedal_next_word = 0U;
+    tesla_preap_pedal_staged_ready = false;
+    return false;
+  }
+  tesla_preap_pedal_staged[index] = value;
+  tesla_preap_pedal_next_word++;
+  return true;
+}
+
+static inline unsigned int tesla_preap_pedal_config_read(uint8_t *out) {
+  if (!tesla_preap_pedal_configured && !tesla_preap_pedal_staged_ready) { return 0U; }
+  const uint16_t *words = tesla_preap_pedal_configured ? tesla_preap_pedal_active_words : tesla_preap_pedal_staged;
+  for (unsigned int i = 0U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) {
+    out[2U * i] = words[i] & 0xFFU;
+    out[(2U * i) + 1U] = words[i] >> 8U;
+  }
+  return TESLA_PREAP_PEDAL_CONFIG_WORDS * 2U;
+}
+
+static void tesla_preap_pedal_mode_reset(uint16_t mode) {
+  tesla_preap_pedal_configured = false;
+  tesla_preap_pedal_feedback_seen = false;
+  tesla_preap_pedal_feedback_healthy = false;
+  tesla_preap_pedal_tx_seen = false;
+  tesla_preap_pedal_tx_enabled = false;
+  tesla_preap_pedal_last_raw_one = 0U;
+  tesla_preap_pedal_last_raw_two = 0U;
+  if (mode != 39U) {
+    tesla_preap_pedal_staged_ready = false;
+    tesla_preap_pedal_next_word = 0U;
+  }
+}
+
+static bool tesla_preap_pedal_profile_init(uint16_t param) {
+  const bool good = tesla_preap_pedal_staged_ready && (tesla_preap_pedal_staged[2] == param) &&
+    tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
+  tesla_preap_pedal_staged_ready = false;  // a startup capsule is consumed exactly once
+  if (good) {
+    for (unsigned int i = 0U; i < TESLA_PREAP_PEDAL_CONFIG_WORDS; i++) {
+      tesla_preap_pedal_active_words[i] = tesla_preap_pedal_staged[i];
+    }
+    tesla_preap_pedal_factor = tesla_preap_pedal_word_float(&tesla_preap_pedal_active_words[4]);
+    tesla_preap_pedal_zero = tesla_preap_pedal_word_float(&tesla_preap_pedal_active_words[6]);
+    tesla_preap_pedal_bus = tesla_preap_pedal_active_words[1];
+  }
+  tesla_preap_pedal_configured = good;
+  return good;
+}
+
+static float tesla_preap_pedal_di(unsigned int raw, float scale) {
+  return (((float)raw * scale) - 22.85856576F - tesla_preap_pedal_zero) * tesla_preap_pedal_factor;
+}
+
+static bool tesla_preap_pedal_feedback(const uint8_t *data, unsigned int bus, uint32_t now) {
+  if (!tesla_preap_pedal_configured || (bus != tesla_preap_pedal_bus)) { return false; }
+  const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
+  const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
+  const uint8_t counter = data[4] & 15U;
+  const bool monotonic = !tesla_preap_pedal_feedback_seen || ((uint32_t)(now - tesla_preap_pedal_feedback_ts) > 0U);
+  const bool channels = (one < 65535U) && (two < 65535U) &&
+    (tesla_preap_pedal_abs(((float)one * 0.050796813F) - ((float)two * 0.101593626F)) <= (2.0F * 0.101593626F));
+  const bool valid = ((data[4] >> 4U) == 0U) && channels && monotonic;
+  const bool next = tesla_preap_pedal_feedback_seen && valid &&
+    (counter == ((tesla_preap_pedal_feedback_counter + 1U) & 15U));
+  tesla_preap_pedal_feedback_healthy = next;
+  tesla_preap_pedal_feedback_seen = valid;
+  tesla_preap_pedal_feedback_counter = counter;
+  tesla_preap_pedal_feedback_ts = now;
+  const float a = tesla_preap_pedal_di(one, 0.050796813F);
+  const float b = tesla_preap_pedal_di(two, 0.101593626F);
+  tesla_preap_pedal_feedback_raw_one = one;
+  tesla_preap_pedal_feedback_raw_two = two;
+  tesla_preap_pedal_feedback_di = (a > b) ? a : b;
+  return next;
+}
+
+static bool tesla_preap_pedal_feedback_ready(uint32_t now) {
+  return tesla_preap_pedal_configured && tesla_preap_pedal_feedback_healthy &&
+    ((uint32_t)(now - tesla_preap_pedal_feedback_ts) <= 500000U);
+}
+
+static float tesla_preap_pedal_max_di(float speed) {
+  const float bp[6] = {0.0F, 5.0F, 12.0F, 20.0F, 30.0F, 40.0F};
+  const float values[6] = {50.0F, 58.0F, 66.0F, 74.0F, 82.0F, 90.0F};
+  float result = values[5];
+  for (unsigned int i = 1U; i < 6U; i++) {
+    if (speed <= bp[i]) {
+      const float bounded = (speed < 0.0F) ? 0.0F : speed;
+      result = values[i - 1U] + ((bounded - bp[i - 1U]) / (bp[i] - bp[i - 1U])) * (values[i] - values[i - 1U]);
+      break;
+    }
+  }
+  return result;
+}
+
+static bool tesla_preap_pedal_channel_bounds(unsigned int raw, unsigned int previous, float scale, float speed) {
+  const float first = (tesla_preap_pedal_zero + (-5.0F / tesla_preap_pedal_factor) + 22.85856576F) / scale;
+  const float second = (tesla_preap_pedal_zero + (tesla_preap_pedal_max_di(speed) / tesla_preap_pedal_factor) + 22.85856576F) / scale;
+  // Match the working Dom codec's0..65534 clamp, including a valid floor
+  // saturation. Config admission separately rejects incompatible channel tops.
+  const float bounded_first = first < 0.0F ? 0.0F : first > 65534.0F ? 65534.0F : first;
+  const float bounded_second = second < 0.0F ? 0.0F : second > 65534.0F ? 65534.0F : second;
+  const float low = bounded_first < bounded_second ? bounded_first : bounded_second;
+  const float high = bounded_first > bounded_second ? bounded_first : bounded_second;
+  // One ADC count covers Float32-vs-host quotient rounding without a DI-based
+  // tolerance growing with calibration factor.
+  const unsigned int floor_low = (unsigned int)low;
+  const unsigned int lower = floor_low > 0U ? floor_low - 1U : 0U;
+  const unsigned int upper = (unsigned int)high < 65534U ? (unsigned int)high + 1U : 65534U;
+  const float unit = tesla_preap_pedal_abs(tesla_preap_pedal_factor) * scale;
+  const float up = 5.0F / unit, down = 2.5F / unit;
+  const unsigned int rate_up = up >= 65534.0F ? 65534U : (unsigned int)up + 1U;
+  const unsigned int rate_down = down >= 65534.0F ? 65534U : (unsigned int)down + 1U;
+  const int delta = tesla_preap_pedal_factor > 0.0F ? (int)raw - (int)previous : (int)previous - (int)raw;
+  return raw >= lower && raw <= upper && delta <= (int)rate_up && delta >= -(int)rate_down;
+}
+
+static bool tesla_preap_pedal_tx(const uint8_t *data, unsigned int bus, uint32_t now,
+                                 float speed, bool longitudinal_allowed) {
+  if (!tesla_preap_pedal_configured || (bus != tesla_preap_pedal_bus)) { return false; }
+  const unsigned int one = ((unsigned int)data[0] << 8U) | data[1];
+  const unsigned int two = ((unsigned int)data[2] << 8U) | data[3];
+  unsigned int sum = 0x51U + 5U;
+  for (unsigned int i = 0U; i < 5U; i++) { sum += data[i]; }
+  const bool enabled = (data[4] & 128U) != 0U;
+  const uint8_t counter = data[4] & 15U;
+  bool good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) &&
+    (!tesla_preap_pedal_tx_seen || (counter == ((tesla_preap_pedal_tx_counter + 1U) & 15U)));
+  if (!enabled) {
+    // A valid zero/disabled command may resynchronize a dropped TX counter.
+    good = ((data[4] & 112U) == 0U) && ((sum & 255U) == data[5]) && (one == 0U) && (two == 0U);
+  } else {
+    const int difference = (int)one - (2 * (int)two);
+    const unsigned int previous = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_one : tesla_preap_pedal_feedback_raw_one;
+    const unsigned int previous_two = tesla_preap_pedal_tx_enabled ? tesla_preap_pedal_last_raw_two : tesla_preap_pedal_feedback_raw_two;
+    good &= tesla_preap_pedal_tx_seen && longitudinal_allowed && tesla_preap_pedal_feedback_ready(now) &&
+      (tesla_preap_pedal_feedback_di <= 2.0F) && tesla_preap_pedal_finite(speed) && speed >= 0.0F &&
+      difference >= 0 && difference <= 1 && one < 65535U && two < 65535U &&
+      tesla_preap_pedal_channel_bounds(one, previous, 0.050796813F, speed) &&
+      tesla_preap_pedal_channel_bounds(two, previous_two, 0.101593626F, speed);
+    if (good) { tesla_preap_pedal_last_raw_one = one; tesla_preap_pedal_last_raw_two = two; }
+  }
+  if (good) {
+    tesla_preap_pedal_tx_seen = true;
+    tesla_preap_pedal_tx_counter = counter;
+    tesla_preap_pedal_tx_enabled = enabled;
+  }
+  // Rejected TX does not alter the last accepted rate/counter baseline.
+  return good;
+}

opendbc_repo/opendbc/safety/modes/tesla_preap_radar.h

@@ -0,0 +1,124 @@
+#pragma once
+#include "opendbc/safety/can_tx.h"
+
+static bool tesla_preap_radar_enabled = false;
+static bool tesla_preap_radar_nosecone = false;
+static const CANPacket_t *tesla_preap_radar_ticket = NULL;
+
+static bool tesla_preap_radar_tx(const CANPacket_t *msg) {
+  const CANPacket_t *expected = tesla_preap_radar_ticket;
+  tesla_preap_radar_ticket = NULL;
+  bool accepted = tesla_preap_radar_enabled && (expected != NULL) &&
+    (msg->bus == 1U) && (msg->addr == expected->addr) &&
+    (GET_LEN(msg) == GET_LEN(expected)) && !msg->fd && !msg->extended && !msg->returned && !msg->rejected;
+  if (expected != NULL) {
+    for (unsigned int i = 0U; i < GET_LEN(expected); i++) {
+      accepted = accepted && (msg->data[i] == expected->data[i]);
+    }
+  }
+  return accepted;
+}
+
+static uint32_t tesla_preap_radar_word(const CANPacket_t *msg, unsigned int offset) {
+  return (uint32_t)msg->data[offset] | ((uint32_t)msg->data[offset + 1U] << 8U) |
+    ((uint32_t)msg->data[offset + 2U] << 16U) | ((uint32_t)msg->data[offset + 3U] << 24U);
+}
+
+static void tesla_preap_radar_put(CANPacket_t *msg, unsigned int offset, uint32_t value) {
+  for (unsigned int i = 0U; i < 4U; i++) { msg->data[offset + i] = (uint8_t)(value >> (8U * i)); }
+}
+
+static void tesla_preap_radar_sum(CANPacket_t *msg) {
+  unsigned int sum = (msg->addr & 255U) + (msg->addr >> 8U);
+  for (unsigned int i = 0U; i < (GET_LEN(msg) - 1U); i++) { sum += msg->data[i]; }
+  msg->data[GET_LEN(msg) - 1U] = (uint8_t)(sum & 255U);
+}
+
+static void tesla_preap_radar_emit(CANPacket_t *packet) {
+  if (tesla_preap_radar_enabled && !relay_malfunction && (tesla_preap_radar_ticket == NULL)) {
+    can_set_checksum(packet);
+    tesla_preap_radar_ticket = packet;
+    can_send(packet, 1U, false);
+    tesla_preap_radar_ticket = NULL;
+  }
+}
+
+static void tesla_preap_radar_copy(const CANPacket_t *source, uint32_t destination) {
+  CANPacket_t packet = {0};
+  packet.addr = destination;
+  packet.bus = 1U;
+  packet.data_len_code = source->data_len_code;
+  for (unsigned int i = 0U; i < GET_LEN(source); i++) { packet.data[i] = source->data[i]; }
+  tesla_preap_radar_emit(&packet);
+}
+
+static void tesla_preap_radar_rx(const CANPacket_t *msg) {
+  if (tesla_preap_radar_enabled && (msg->bus == 0U) && !msg->fd && !msg->extended && !msg->returned && !msg->rejected) {
+    if (msg_matches(msg, 0x45U, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x219U); }
+    else if (msg_matches(msg, 0x108U, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x109U); }
+    else if (msg_matches(msg, 0x145U, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x149U); }
+    else if (msg_matches(msg, 0x20AU, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x159U); }
+    else if (msg_matches(msg, 0x308U, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x209U); }
+    else if (msg_matches(msg, 0x30AU, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x2D9U); }
+    else if (msg_matches(msg, 0x405U, 0U, 8U)) { tesla_preap_radar_copy(msg, 0x2B9U); }
+    else if (msg_matches(msg, 0x398U, 0U, 8U)) {
+      CANPacket_t packet = {0};
+      packet.addr = 0x2A9U;
+      packet.bus = 1U;
+      packet.data_len_code = 8U;
+      const uint32_t low = (tesla_preap_radar_word(msg, 0U) & 0xFFFFF33FU) | 0x100U | 0x440U;
+      const uint32_t high = (tesla_preap_radar_word(msg, 4U) & 0xCFFF0F0FU) | 0x10000000U |
+        (tesla_preap_radar_nosecone ? 16U : 0U);
+      tesla_preap_radar_put(&packet, 0U, low);
+      tesla_preap_radar_put(&packet, 4U, high);
+      tesla_preap_radar_sum(&packet);
+      tesla_preap_radar_emit(&packet);
+    } else if (msg_matches(msg, 0xEU, 0U, 8U) && (tesla_preap_crc(msg) == msg->data[7])) {
+      CANPacket_t packet = *msg;
+      packet.bus = 1U;
+      packet.addr = 0x199U;
+      uint32_t low = tesla_preap_radar_word(msg, 0U);
+      uint32_t high = tesla_preap_radar_word(msg, 4U);
+      if (((low >> 16U) & 0xFF3FU) == 0xFF3FU) {
+        low = (low & 0x00C0FFFFU) | (0x20U << 16U);
+        high = (high & 0x00FFFFF0U) | 4U;
+        tesla_preap_radar_put(&packet, 0U, low);
+        tesla_preap_radar_put(&packet, 4U, high);
+        packet.data[7] = tesla_preap_crc(&packet);
+      }
+      tesla_preap_radar_emit(&packet);
+    } else if (msg_matches(msg, 0x115U, 0U, 6U)) {
+      tesla_preap_radar_copy(msg, 0x129U);
+      CANPacket_t packet = {0};
+      packet.addr = 0x1A9U;
+      packet.bus = 1U;
+      packet.data_len_code = 5U;
+      const unsigned int counter = msg->data[4] >> 4U;
+      tesla_preap_radar_put(&packet, 0U, 0x000C0000U | ((uint32_t)counter << 28U));
+      packet.data[4] = (uint8_t)((0x38U + 12U + (counter << 4U)) & 255U);
+      tesla_preap_radar_emit(&packet);
+    } else if (msg_matches(msg, 0x118U, 0U, 6U)) {
+      tesla_preap_radar_copy(msg, 0x119U);
+      const unsigned int raw_speed = (tesla_preap_radar_word(msg, 0U) >> 16U) & 4095U;
+      uint32_t speed = 8191U;
+      if (raw_speed != 4095U) {
+        const int mph_x100 = (int)raw_speed * 5 - 2500;
+        const int kph_x100 = mph_x100 * 1609 / 1000;
+        speed = (kph_x100 < 0) ? 0U : ((uint32_t)(kph_x100 / 4) & 8191U);
+      }
+      CANPacket_t packet = {0};
+      packet.addr = 0x169U;
+      packet.bus = 1U;
+      packet.data_len_code = 8U;
+      const uint32_t low = speed | (speed << 13U) | (speed << 26U);
+      const uint32_t high = ((speed >> 6U) | (speed << 7U) | ((uint32_t)(msg->data[4] & 15U) << 20U)) & 0x00FFFFFFU;
+      tesla_preap_radar_put(&packet, 0U, low);
+      tesla_preap_radar_put(&packet, 4U, high);
+      unsigned int checksum = 0x76U;
+      for (unsigned int i = 0U; i < 7U; i++) { checksum += packet.data[i]; }
+      packet.data[7] = (uint8_t)(checksum & 255U);
+      tesla_preap_radar_emit(&packet);
+    } else {
+    }
+  }
+}

opendbc_repo/opendbc/safety/safety.h

@@ -463,6 +463,7 @@ static void reset_sample(struct sample_t *sample) {
 }
 
 int set_safety_hooks(uint16_t mode, uint16_t param) {
+  tesla_preap_pedal_mode_reset(mode);
   const safety_hook_config safety_hook_registry[] = {
     {SAFETY_SILENT, &nooutput_hooks},
     {SAFETY_HONDA_NIDEC, &honda_nidec_hooks},
@@ -554,7 +555,10 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
         current_safety_param = 0;
         continue;
       }
-      if ((mode == SAFETY_TESLA_PREAP) && (param != 0U)) {
+      const bool preap_pedal_ready = ((param == 1U) || (param == 3U) || (param == 7U)) &&
+        tesla_preap_pedal_staged_ready && (tesla_preap_pedal_staged[2] == param) &&
+        tesla_preap_pedal_config_valid(tesla_preap_pedal_staged);
+      if ((mode == SAFETY_TESLA_PREAP) && (param != 0U) && (param != 2U) && (param != 6U) && !preap_pedal_ready) {
         current_hooks = &nooutput_hooks;
         current_safety_mode = SAFETY_NOOUTPUT;
         current_safety_param = 0;

opendbc_repo/opendbc/safety/tests/libsafety/safety.c

@@ -420,3 +420,13 @@ bool safety_test_volvo_cma_cold_state_zeroed(void) {
   }
   return cleared;
 }
+
+// Test transport adapter only; enforces the actual board startup mode guard.
+bool tesla_preap_test_config_word(uint16_t index, uint16_t value) {
+  const bool non_car = (current_safety_mode == SAFETY_SILENT) || (current_safety_mode == SAFETY_NOOUTPUT) ||
+    (current_safety_mode == SAFETY_ELM327);
+  return tesla_preap_pedal_config_word(index, value, non_car && !controls_allowed && !heartbeat_engaged);
+}
+unsigned int tesla_preap_test_config_read(uint8_t *out) {
+  return tesla_preap_pedal_config_read(out);
+}

opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py

@@ -0,0 +1,86 @@
+"""Actual registered mode39 bounds; no forced controlsAllowed or native callback bypass."""
+import pytest
+from opendbc.car.tesla.tests.preap_stream import PreAPStream
+from opendbc.car.tesla.preap.pedal_contract import PedalCalibration, PedalCodec
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.starpilot.car.tesla.tests.pedal_fixture import configure_native, feedback_frame
+
+
+def warm(bus, word):
+  safety = libsafety_py.libsafety
+  calibration = PedalCalibration(1., 0., bus)
+  configure_native(safety, calibration, word, b'actual-registered-profile-test')
+  stream = PreAPStream()
+  priming = PedalCodec(calibration)
+  now = 0
+  for tick in range(1, 91):
+    now = 1_000_000 + tick * 10000
+    safety.set_timer(now)
+    safety.set_aol_test_heartbeat(tick >= 65)
+    frames = stream.frames(tick, lever=2 if tick in (65, 75) else 0, cruise=1)
+    if tick % 2 == 0:
+      frames.append(feedback_frame(calibration, tick // 2))
+    for address, data, source in frames:
+      assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, source, data))
+    safety.safety_tick()
+    safety.aol_set_host_request(3 if tick >= 65 else 0)
+    if tick % 2 == 0:
+      assert transmit(safety, priming.command(enabled=False))
+    if 65 <= tick < 75:
+      assert not (safety.aol_get_permission_mask() & 2), 'one physical pull is lateral only'
+  assert safety.safety_config_valid() and safety.get_controls_allowed()
+  assert safety.aol_get_permission_mask() == 3
+  return safety, calibration, now
+
+
+def transmit(safety, frame):
+  address, data, bus = frame
+  return bool(safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, data)))
+
+
+@pytest.mark.parametrize('bus', [0, 2])
+@pytest.mark.parametrize('word', [1, 3, 7])
+def test_actual_registered_priming_rate_absolute_bus_and_counter_bounds(bus, word):
+  safety, calibration, now = warm(bus, word)
+  codec = PedalCodec(calibration)
+  assert transmit(safety, codec.command(enabled=False))
+  for di in range(0, 73, 4):
+    assert transmit(safety, codec.command(float(di), enabled=True))
+  valid = codec.command(74., enabled=True)
+  # At80kph original speed-profile max is75.78DI; this77DI request is
+  # within+5 rate from72 but outside absolute native demand.
+  too_high = PedalCodec(calibration)
+  too_high.counter = valid[1][4] & 15
+  assert not transmit(safety, too_high.command(77., enabled=True))
+  assert transmit(safety, valid), 'rejected TX must not corrupt accepted counter/rate state'
+  replay = valid
+  assert not transmit(safety, replay)
+  address, data, source = codec.command(74., enabled=True)
+  assert not transmit(safety, (address, data, 2 - source))
+  broken = bytearray(data)
+  broken[5] ^= 1
+  assert not transmit(safety, (address, bytes(broken), source))
+  assert transmit(safety, (address, data, source))
+  safety.set_timer(now + 500001)
+  assert not transmit(safety, codec.command(74., enabled=True))
+  assert transmit(safety, codec.command(enabled=False)), 'zero/disabled must remain available to withdraw/resynchronize'
+
+
+@pytest.mark.parametrize('fault', ['state', 'channels', 'counter', 'short'])
+def test_actual_feedback_fault_withdraws_then_needs_fresh_physical_rearm(fault):
+  safety, calibration, now = warm(2, 1)
+  codec = PedalCodec(calibration)
+  assert transmit(safety, codec.command(enabled=False))
+  assert transmit(safety, codec.command(0., enabled=True))
+  counter = 45 if fault == 'counter' else 46
+  frame = feedback_frame(calibration, counter, state=1 if fault == 'state' else 0,
+                         bad_channel=fault == 'channels')
+  if fault == 'short':
+    frame = frame[0], frame[1][:-1], frame[2]
+  safety.set_timer(now + 1000)
+  address, data, bus = frame
+  safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, data))
+  assert not safety.get_controls_allowed()
+  assert not (safety.aol_get_permission_mask() & 2)
+  assert not transmit(safety, codec.command(0., enabled=True))
+  assert transmit(safety, codec.command(enabled=False))

opendbc_repo/opendbc/safety/tests/test_tesla_preap_radar.py

@@ -0,0 +1,155 @@
+import unittest
+
+from opendbc.safety.tests.common import make_msg
+from opendbc.safety.tests.libsafety import libsafety_py
+from opendbc.safety.tests.libsafety.libsafety_py import new_CANPacket
+from opendbc.safety.tests import test_tesla_preap_stock as stock_fixture
+
+
+class TestTeslaPreapRadar(unittest.TestCase):
+  SOURCES = {0x45: (8, (0x219,)), 0x108: (8, (0x109,)), 0x145: (8, (0x149,)),
+             0x20A: (8, (0x159,)), 0x308: (8, (0x209,)), 0x30A: (8, (0x2D9,)),
+             0x405: (8, (0x2B9,)), 0x398: (8, (0x2A9,)), 0xE: (8, (0x199,)),
+             0x115: (6, (0x129, 0x1A9)), 0x118: (6, (0x119, 0x169))}
+
+  def setUp(self):
+    self.safety = libsafety_py.libsafety
+    self.safety.init_tests()
+    self.safety.set_alternative_experience(32)
+    self.safety.set_safety_hooks(39, 2)
+    self.safety.set_timer(1_000_000)
+    self.safety.reset_recorded_can()
+
+  def tearDown(self):
+    self.safety.init_tests()
+    self.safety.set_alternative_experience(0)
+    self.safety.set_safety_hooks(19, 0)
+
+  def source(self, address, counter=0):
+    length = self.SOURCES[address][0]
+    data = bytearray(length)
+    if address == 0x45:
+      data[0] = 64
+      data[6] = counter << 4
+    if address in (0x45, 0xE):
+      data[-1] = stock_fixture.TestTeslaPreapStock.crc(data[:-1])
+    return make_msg(0, address, dat=bytes(data))
+
+  def recorded(self):
+    packets = []
+    for index in range(self.safety.get_recorded_can_count()):
+      packet = new_CANPacket()
+      self.assertTrue(self.safety.get_recorded_can(index, packet))
+      packets.append(packet)
+    return packets
+
+  def test_all_sources_exact_outputs_and_no_host_injection(self):
+    for parameter in (2, 6):
+      for address, (_, destinations) in self.SOURCES.items():
+        with self.subTest(parameter=parameter, address=address):
+          self.setUp()
+          self.safety.set_safety_hooks(39, parameter)
+          self.safety.safety_rx_hook(self.source(address))
+          packets = self.recorded()
+          self.assertEqual(tuple(p[0].addr for p in packets), destinations)
+          for packet in packets:
+            self.assertEqual(packet[0].bus, 1)
+            self.assertFalse(packet[0].fd)
+            self.assertFalse(self.safety.safety_tx_hook(packet))
+          self.safety.safety_tick()
+          self.assertEqual(self.safety.get_recorded_can_count(), len(destinations))
+
+  def test_wrong_bus_length_fd_and_crc_cannot_mint_ticket(self):
+    for address in self.SOURCES:
+      for fault in ('bus', 'length', 'fd', 'extended', 'returned', 'rejected'):
+        with self.subTest(address=address, fault=fault):
+          self.setUp()
+          packet = self.source(address)
+          if fault == 'length':
+            packet = make_msg(0, address, dat=bytes(self.SOURCES[address][0] - 1))
+          elif fault == 'bus':
+            packet[0].bus = 1
+          else:
+            setattr(packet[0], fault, 1)
+          self.safety.safety_rx_hook(packet)
+          self.assertEqual(self.safety.get_recorded_can_count(), 0)
+    for address in (0x45, 0xE):
+      self.setUp()
+      packet = self.source(address)
+      packet[0].data[7] ^= 1
+      self.safety.safety_rx_hook(packet)
+      self.assertEqual(self.safety.get_recorded_can_count(), 0)
+
+  def test_physical_counter_and_accepted_echo_are_not_gateway_sources(self):
+    fixture = stock_fixture.TestTeslaPreapStock()
+    fixture.setUp()
+    self.safety.set_safety_hooks(39, 2)
+    fixture.arm()
+    self.safety.reset_recorded_can()
+    duplicate = fixture.stalk(2)
+    self.safety.safety_rx_hook(duplicate)
+    self.assertEqual(self.safety.get_recorded_can_count(), 0)
+    fixture.arm()
+    echo = fixture.stalk(1, (fixture.counter + 1) % 16)
+    self.assertTrue(self.safety.safety_tx_hook(echo))
+    self.safety.reset_recorded_can()
+    self.safety.safety_rx_hook(echo)
+    self.assertEqual(self.safety.get_recorded_can_count(), 0)
+    fixture.feed()
+    self.assertIn(0x219, [p[0].addr for p in self.recorded()])
+
+  def test_relay_failure_consumes_ticket_and_fresh_source_can_retry(self):
+    self.safety.set_relay_malfunction(True)
+    self.safety.safety_rx_hook(self.source(0x145))
+    self.assertEqual(self.safety.get_recorded_can_count(), 0)
+    self.safety.set_relay_malfunction(False)
+    self.assertFalse(self.safety.safety_tx_hook(make_msg(1, 0x149, dat=bytes(8))))
+    self.safety.safety_rx_hook(self.source(0x145))
+    self.assertEqual(self.safety.get_recorded_can_count(), 1)
+    self.assertFalse(self.safety.safety_tx_hook(self.recorded()[0]))
+
+  def test_exact_all_16bit_profiles_and_stock_gateway_invariant(self):
+    for parameter in range(65536):
+      self.safety.set_safety_hooks(39, parameter)
+      self.safety.reset_recorded_can()
+      self.safety.safety_rx_hook(self.source(0x145))
+      self.assertEqual(self.safety.get_recorded_can_count(), int(parameter in (2, 6)), parameter)
+
+  def test_mount_and_counter_transforms_have_independent_literal_bytes(self):
+    for parameter, expected in ((2, bytes.fromhex('40050000000000f0')),
+                                (6, bytes.fromhex('4005000010000000'))):
+      self.setUp()
+      self.safety.set_safety_hooks(39, parameter)
+      self.safety.safety_rx_hook(self.source(0x398))
+      packet = self.recorded()[0]
+      self.assertEqual(bytes(packet[0].data[0:8]), expected[:8])
+    self.setUp()
+    self.safety.safety_rx_hook(make_msg(0, 0x115, dat=bytes.fromhex('00000000a000')))
+    packet = self.recorded()[1]
+    self.assertEqual(bytes(packet[0].data[0:5]), bytes.fromhex('00000ca0e4'))
+
+  def test_exact_alternative_experience_and_no_timer_replay(self):
+    for parameter in (2, 6):
+      for experience in (0, 1, 16, 32, 33):
+        self.safety.init_tests()
+        self.safety.set_alternative_experience(experience)
+        self.safety.set_safety_hooks(39, parameter)
+        self.safety.reset_recorded_can()
+        self.safety.set_timer(1_000_000)
+        self.safety.safety_rx_hook(self.source(0x145))
+        self.assertEqual(self.safety.get_recorded_can_count(), int(experience in (0, 32)))
+        self.safety.set_timer(3_000_000)
+        self.safety.safety_tick()
+        self.assertEqual(self.safety.get_recorded_can_count(), int(experience in (0, 32)))
+
+  def test_full_transport_queue_cannot_leave_reusable_ticket(self):
+    for _ in range(32):
+      self.safety.safety_rx_hook(self.source(0x145))
+    self.assertEqual(self.safety.get_recorded_can_count(), 32)
+    self.safety.safety_rx_hook(self.source(0x145))
+    self.assertEqual(self.safety.get_recorded_can_count(), 32)
+    self.assertFalse(self.safety.safety_tx_hook(make_msg(1, 0x149, dat=bytes(8))))
+    self.safety.reset_recorded_can()
+    self.safety.safety_rx_hook(self.source(0x145))
+    self.assertEqual(self.safety.get_recorded_can_count(), 1)
+    self.assertFalse(self.safety.safety_tx_hook(self.recorded()[0]))

opendbc_repo/opendbc/safety/tests/test_tesla_preap_stock.py

@@ -191,11 +191,11 @@ class TestTeslaPreapStock(unittest.TestCase):
     self.assertFalse(self.safety.safety_tx_hook(make_msg(0, 0x488, dat=bytes([64, 0, 64, 0]))))
     self.assertFalse(self.safety.safety_tx_hook(make_msg(1, 0x488, dat=bytes([64, 0, 64, 12]))))
 
-  def test_all_16bit_parameters_reject_every_nonzero_profile(self):
+  def test_all_16bit_parameters_accept_only_stock_and_radar_profiles(self):
     command = self.steering(control_type=0)
     for parameter in range(65536):
       self.safety.set_safety_hooks(39, parameter)
-      self.assertEqual(bool(self.safety.safety_tx_hook(command)), parameter == 0, parameter)
+      self.assertEqual(bool(self.safety.safety_tx_hook(command)), parameter in (0, 2, 6), parameter)
 
   def test_required_sources_loss_wrong_bus_length_and_fresh_rearm(self):
     for address in (0x370, 0x108, 0x118, 0x20A, 0x368, 0x318, 0x45, 0x155, 0x201):
@@ -252,6 +252,51 @@ class TestTeslaPreapStock(unittest.TestCase):
           self.assertEqual(self.safety.safety_fwd_hook(bus, address), -1)
           self.assertTrue(self.safety.safety_test_selected_fwd(bus, address))
 
+  def test_reverse_neutral_pause_retains_only_existing_physical_arm(self):
+    for gear in (2, 3):
+      with self.subTest(gear=gear):
+        self.setUp()
+        self.arm()
+        self.feed(gear=gear)
+        self.assertFalse(self.safety.get_controls_allowed())
+        self.safety.aol_set_host_request(0)
+        self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+        self.safety.aol_set_host_request(1)
+        self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+        self.assertFalse(self.safety.safety_tx_hook(self.stalk(16, (self.counter + 1) % 16)))
+        self.feed()
+        self.safety.aol_set_host_request(1)
+        self.assertEqual(self.safety.aol_get_permission_mask(), 1)
+        self.assertFalse(self.safety.get_controls_allowed())
+        self.feed(gear=gear, lever=1)
+        self.feed()
+        self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+
+  def test_reverse_does_not_relax_expiry_fault_or_cold_arm_rules(self):
+    for gear in (0, 1, 7):
+      self.setUp()
+      self.arm()
+      self.feed(gear=gear)
+      self.feed()
+      self.safety.aol_set_host_request(1)
+      self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+    self.setUp()
+    for _ in range(10):
+      self.feed(gear=2)
+    self.feed(gear=2, lever=2)
+    self.safety.set_aol_test_heartbeat(True)
+    self.safety.aol_set_host_request(1)
+    self.feed()
+    self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+    self.arm()
+    self.feed(gear=2)
+    self.time += 1_100_000
+    self.safety.set_timer(self.time)
+    self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+    self.feed()
+    self.safety.aol_set_host_request(1)
+    self.assertEqual(self.safety.aol_get_permission_mask(), 0)
+
 
 class TestTeslaPreapIgnition(unittest.TestCase):
   def setUp(self):

openpilot/cereal/custom.capnp

@@ -92,6 +92,28 @@ struct StarPilotCarControl @0x81c2f05a394cf4af {
   }
 
   phoneGps @3 :StarPilotCarState.Gps;  # sourceMonoTime is comma receive time minus the phone's fix age
+
+
+  struct PreapRadarStartup {
+    version @0 :UInt16;
+    kind @1 :UInt16;
+    carParamsSha256 @2 :Data;
+    safetyModel @3 :UInt16;
+    safetyParam @4 :UInt16;
+    lateralOffset @5 :Float32;
+  }
+
+  struct PreapPedalStartup {
+    version @0 :UInt16;
+    kind @1 :UInt16;
+    carParamsSha256 @2 :Data;
+    safetyModel @3 :UInt16;
+    safetyParam @4 :UInt16;
+    factor @5 :Float32;  # original DI units per physical pedal unit
+    zero @6 :Float32;  # original physical pedal unit offset
+    pedalBus @7 :UInt16;
+    sourceMonoTime @8 :UInt64;  # CLOCK_MONOTONIC ns from actual Card startup
+  }
 }
 struct StarPilotCarParams @0xaedffd8f31e7b55d {
   alternativeExperience @0 :Int16;

openpilot/common/params_keys.h

@@ -312,7 +312,15 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"ConditionalModeConfig", {PERSISTENT | DONT_LOG, JSON}},
     {"ConditionalManualState", {PERSISTENT | DONT_LOG, JSON}},
     {"VehicleSelection", {PERSISTENT | DONT_LOG, JSON}},
+    {"NAPAdaptiveAccel", {PERSISTENT, BOOL, "1"}},
     {"NAPPedalEnabled", {PERSISTENT, BOOL, "0"}},
+    {"TeslaPreAPRadarConfig", {PERSISTENT, JSON}},
+    {"NAPRadarOffset", {PERSISTENT, FLOAT, "0.0"}},
+    {"NAPPedalCalibDone", {PERSISTENT, BOOL, "0"}},
+    {"NAPPedalCanBus", {PERSISTENT, INT, "2"}},
+    {"NAPPedalCalibFactor", {PERSISTENT, FLOAT, "1.0"}},
+    {"NAPPedalCalibZero", {PERSISTENT, FLOAT, "0.0"}},
+    {"TeslaPreAPPedalStartup", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION | DONT_LOG, BYTES}},
     {"NAPRadarEnabled", {PERSISTENT, BOOL, "0"}},
     {"NAPRadarBehindNosecone", {PERSISTENT, BOOL, "0"}},
     {"SafeMode", {PERSISTENT | DONT_LOG, BOOL, "0"}},

openpilot/selfdrive/car/card.py

@@ -418,6 +418,11 @@ class Car:
     # Write CarParams for controls and radard
     self.vehicle_startup.seal_publication()
     cp_bytes = self.CP.to_bytes()
+    from openpilot.starpilot.car.tesla.pedal_startup import install as install_preap_pedal
+    install_preap_pedal(self.CI, self.CP, cp_bytes, startup_preferences.tesla_preap_pedal, self.params)
+    from openpilot.starpilot.car.tesla.radar_context import install_startup_context
+    self.tesla_preap_radar_context = install_startup_context(
+      self.RI, self.CP, cp_bytes, startup_preferences.tesla_preap_radar)
     self.params.put("CarParams", cp_bytes, block=True)
     put_cache(self.params, "CarParamsCache", self.CP)
     put_cache(self.params, "CarParamsPersistent", self.CP)
@@ -981,6 +986,12 @@ class Car:
                              sequence=self.slc_cruise_event_id, car_state_stamp=int(cs_send.logMonoTime),
                              valid=bool(cs_send.valid and not CS.canTimeout))
     companion = None
+    if self.CP.brand == 'tesla' and self.CP.openpilotLongitudinalControl and qualified_tesla_preap(self.CP):
+      internal = self.CI.CS.preap
+      companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))
+      companion.starpilotCarState.sourceCarStateMonoTime = int(cs_send.logMonoTime)
+      companion.starpilotCarState.pedalLongActive = bool(internal.enableLongControl)
+      companion.starpilotCarState.pedalMaxRegen = internal.pccEvent == 'pedalMaxRegen'
     steering_authority = getattr(self.CI.CS, 'steering_authority', None)
     if self.CP.brand == 'gm' and steering_authority is not None and steering_authority.enabled:
       companion = messaging.new_message('starpilotCarState', valid=bool(cs_send.valid), logMonoTime=int(cs_send.logMonoTime))

openpilot/selfdrive/controls/controlsd.py

@@ -278,6 +278,9 @@ class Controls:
     # accel PID loop
     publish_takeoff_state = getattr(self.LoC, 'takeoff_starting', False)
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
+    from opendbc.car.tesla.preap.pedal_profile import accel_limits as preap_pedal_limits
+    pid_accel_limits = preap_pedal_limits(self.CP, CS.vEgo,
+      getattr(self.sm['selfdriveState'].personality, 'raw', 1), pid_accel_limits)
     longitudinal_output = self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits,
                                           context=self.longitudinal_inputs.context(CC.longActive))
     actuators.accel = final_output(longitudinal_output)

openpilot/selfdrive/controls/lib/longitudinal_planner.py

@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_accel_
 from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.common.swaglog import cloudlog
 from openpilot.starpilot.navigation.intent import cruise_ceiling as navigation_ceiling
+from openpilot.starpilot.car.tesla.preap_adaptive import startup_owner as preap_adaptive_owner
 from openpilot.starpilot.longitudinal.vehicle_policy import forecast_should_stop, planner_cost_owner, far_follow_owner
 from openpilot.starpilot.longitudinal.cruise_ceiling import CruiseCeiling, CurveCeiling, select_cruise_ceiling, select_curve_ceiling
 from openpilot.starpilot.longitudinal.profile_runtime import (AppliedProfile, ProfileSmoother, ProfileTuning, SelectedProfileTuning,
@@ -127,6 +128,7 @@ class LongitudinalPlanner:
     self.vehicle_cost_owner = planner_cost_owner(CP)
     self.lead_takeoff = LeadTakeoff()
     self.vehicle_far_follow = far_follow_owner(CP, dt)
+    self.preap_adaptive = preap_adaptive_owner(CP)
 
   def update(self, sm, *, cruise_ceiling: CruiseCeiling | None = None,
              profile_tuning: ProfileTuning | None = None, curve_ceiling: CurveCeiling | None = None,
@@ -254,6 +256,13 @@ class LongitudinalPlanner:
     acceleration_cap = self.last_profile.acceleration_max if self.last_profile is not None else None
     if selected_acceleration is not None:
       acceleration_cap = selected_acceleration
+    preap_acceleration_cap = None
+    if self.preap_adaptive is not None:
+      preap_acceleration_cap = self.preap_adaptive.sample(
+        sm, now_ns=sample_now_ns, drive_id=drive_id, upper=ACCEL_MAX if acceleration_cap is None else acceleration_cap,
+        follow_seconds=selected_follow, active=profile_eligible and not reset_state)
+      if preap_acceleration_cap is not None:
+        acceleration_cap = preap_acceleration_cap
     self.mpc.update(sm['radarState'], personality=sm['selfdriveState'].personality,
                     follow_seconds=follow if follow is not None else
                     (self.last_profile.follow_seconds if self.last_profile is not None else None),
@@ -376,6 +385,9 @@ class LongitudinalPlanner:
       global_floor = coast_plan.brake_floor
       cruise_profile = None
       slc_floor = None
+    if preap_acceleration_cap is not None:
+      selected_acceleration = (preap_acceleration_cap if selected_acceleration is None else
+                               min(selected_acceleration, preap_acceleration_cap))
     self.a_cruise = get_cruise_accel(sm['selfdriveState'].experimentalMode, v_cruise, v_ego,
                                      self.a_cruise, steer_angle_without_offset, self.CP, self.dt,
                                      accel_coast, self.allow_throttle, cruise_profile, slc_floor,
@@ -440,6 +452,8 @@ class LongitudinalPlanner:
         target=float(output_a_target), previous_target=float(a_prev), follow_seconds=float(self.mpc.params[0, 4]),
         blocked=bool(self.output_should_stop or self.fcw or self.force_stop_plan.forcing or
                      self.force_stop_plan.approach_distance_m > 0 or self.mpc.solution_status != 0))
+    if preap_acceleration_cap is not None:
+      output_a_target = min(output_a_target, preap_acceleration_cap)
     self.output_a_target = np.clip(output_a_target, ACCEL_MIN, ACCEL_MAX)
 
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.output_a_target + a_prev) / 2.0

openpilot/selfdrive/pandad/SConscript

@@ -31,3 +31,6 @@ if arch != "Darwin":
       '-Wl,--wrap=_ZN9PubMaster4sendEPKcR14MessageBuilder',
     ])
     power_test.Program('tests/test_peripheral_state', ['tests/test_peripheral_state.cc', power_test.Object('tests/peripheral_panda_safety.o', 'panda_safety.cc')], LIBS=[panda] + libs)
+
+  if GetOption('extras') or any(target.startswith('openpilot/selfdrive/pandad/tests/test_preap_pedal_startup') for target in COMMAND_LINE_TARGETS):
+    env.Program('tests/test_preap_pedal_startup', ['tests/test_preap_pedal_startup.cc', env.Object('tests/preap_panda_safety.o', 'panda_safety.cc')], LIBS=[panda] + libs + ['dl'])

openpilot/selfdrive/pandad/panda.cc

@@ -192,6 +192,24 @@ void Panda::send_heartbeat(bool engaged) {
   handle->control_write(0xf3, engaged, 0);
 }
 
+bool Panda::matches_preap_pedal(const std::array<uint16_t, 24> &words) {
+  std::array<uint8_t, 48> actual{}, expected{};
+  for (size_t i = 0; i < words.size(); ++i) {
+    expected[2 * i] = words[i] & 255;
+    expected[2 * i + 1] = words[i] >> 8;
+  }
+  const int count = handle->control_read(0xb8, 0, 0, actual.data(), actual.size());
+  return count == static_cast<int>(actual.size()) && actual == expected;
+}
+
+bool Panda::stage_preap_pedal(const std::array<uint16_t, 24> &words) {
+  for (size_t i = 0; i < words.size(); ++i) {
+    if (handle->control_write(0xb7, static_cast<uint16_t>(i), words[i]) < 0) return false;
+  }
+  if (handle->control_write(0xb7, 0xffff, 0xc051) < 0) return false;
+  return matches_preap_pedal(words);
+}
+
 bool Panda::set_aol_axis_request(uint8_t axis_mask) {
   return handle->control_write(0xf5, axis_mask & 0x7U, 0) >= 0;
 }

openpilot/selfdrive/pandad/panda.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <cstdint>
+#include <array>
 #include <ctime>
 #include <functional>
 #include <list>
@@ -81,6 +82,8 @@ public:
   void enable_deepsleep();
   void send_heartbeat(bool engaged);
   bool set_aol_axis_request(uint8_t axis_mask);
+  bool stage_preap_pedal(const std::array<uint16_t, 24> &words);
+  bool matches_preap_pedal(const std::array<uint16_t, 24> &words);
   void set_can_speed_kbps(uint16_t bus, uint16_t speed);
   void set_can_fd_auto(uint16_t bus, bool enabled);
   void set_data_speed_kbps(uint16_t bus, uint16_t speed);

openpilot/selfdrive/pandad/panda_safety.cc

@@ -1,21 +1,33 @@
 #include "selfdrive/pandad/pandad.h"
+#include "selfdrive/pandad/preap_pedal_wire.h"
 #include "openpilot/cereal/messaging/messaging.h"
 #include "common/swaglog.h"
 
 void PandaSafety::configureSafetyMode(bool is_onroad) {
+  // A Card restart/config change or native reset withdraws before any restage.
+  if (is_onroad && safety_configured_ && preap_pedal_words_ &&
+      (read_preap_pedal_capsule(params_.getParamPath("TeslaPreAPPedalStartup")) != preap_pedal_capsule_ ||
+       params_.get("CarParams") != preap_pedal_cp_bytes_ || pandas_.size() != 1 ||
+       !pandas_[0]->matches_preap_pedal(*preap_pedal_words_))) {
+    for (auto *panda : pandas_) panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT, 0U);
+    safety_configured_ = false;
+    preap_pedal_words_.reset();
+  }
   if (is_onroad && !safety_configured_) {
     updateMultiplexingMode();
 
     auto car_params = fetchCarParams();
     if (!car_params.empty()) {
       LOGW("got %lu bytes CarParams", car_params.size());
-      setSafetyMode(car_params);
-      safety_configured_ = true;
+      safety_configured_ = setSafetyMode(car_params);
     }
   } else if (!is_onroad) {
     initialized_ = false;
     safety_configured_ = false;
     log_once_ = false;
+    preap_pedal_words_.reset();
+    preap_pedal_capsule_.clear();
+    preap_pedal_cp_bytes_.clear();
   }
 }
 
@@ -55,13 +67,26 @@ std::string PandaSafety::fetchCarParams() {
   return params_.get("CarParams");
 }
 
-void PandaSafety::setSafetyMode(const std::string &params_string) {
+bool PandaSafety::setSafetyMode(const std::string &params_string) {
   AlignedBuffer aligned_buf;
   capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size()));
   cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
 
   auto safety_configs = car_params.getSafetyConfigs();
   uint16_t alternative_experience = car_params.getAlternativeExperience();
+  const bool pedal = car_params.getBrand() == "tesla" && car_params.getCarFingerprint() == "TESLA_MODEL_S_PREAP" &&
+    car_params.getOpenpilotLongitudinalControl();
+  std::optional<PreapPedalWords> pedal_words;
+  std::string pedal_capsule;
+  if (pedal) {
+    pedal_capsule = read_preap_pedal_capsule(params_.getParamPath("TeslaPreAPPedalStartup"));
+    pedal_words = preap_pedal_words(pedal_capsule, params_string, car_params);
+    if (pandas_.size() != 1 || !pedal_words) {
+      for (auto *panda : pandas_) panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT, 0U);
+      return false;
+    }
+  }
+
 
   alternative_experience_ = alternative_experience;
   safety_config_count_ = safety_configs.size();
@@ -76,8 +101,20 @@ void PandaSafety::setSafetyMode(const std::string &params_string) {
     safety_slots_.emplace_back(model, param);
     LOGW("Panda %zu: setting safety model: %d, param: %d, alternative experience: %d", i, (int)model, param, alternative_experience);
     pandas_[i]->set_alternative_experience(alternative_experience);
+    if (pedal && !pandas_[i]->stage_preap_pedal(*pedal_words)) {
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT, 0U);
+      return false;
+    }
     pandas_[i]->set_safety_model(model, param);
+    if (pedal && !pandas_[i]->matches_preap_pedal(*pedal_words)) {
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT, 0U);
+      return false;
+    }
   }
+  preap_pedal_words_ = pedal_words;
+  preap_pedal_capsule_ = pedal ? pedal_capsule : std::string();
+  preap_pedal_cp_bytes_ = pedal ? params_string : std::string();
+  return true;
 }
 
 Panda *PandaSafety::aolOwner() {

openpilot/selfdrive/pandad/pandad.h

@@ -17,7 +17,7 @@ public:
 private:
   void updateMultiplexingMode();
   std::string fetchCarParams();
-  void setSafetyMode(const std::string &params_string);
+  bool setSafetyMode(const std::string &params_string);
 
   bool initialized_ = false;
   bool log_once_ = false;
@@ -27,5 +27,8 @@ private:
   std::vector<std::pair<cereal::CarParams::SafetyModel, uint16_t>> safety_slots_;
   uint16_t alternative_experience_ = 0;
   size_t safety_config_count_ = 0;
+  std::optional<std::array<uint16_t, 24>> preap_pedal_words_;
+  std::string preap_pedal_capsule_;
+  std::string preap_pedal_cp_bytes_;
   Params params_;
 };

openpilot/selfdrive/pandad/preap_pedal_wire.h

@@ -0,0 +1,78 @@
+#pragma once
+#include <array>
+#include <cmath>
+#include <cstring>
+#include <optional>
+#include <string>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include "selfdrive/pandad/preap_sha256.h"
+#include "openpilot/cereal/gen/cpp/custom.capnp.h"
+#include "openpilot/cereal/messaging/messaging.h"
+#include "selfdrive/pandad/aol_protocol.h"
+
+using PreapPedalWords = std::array<uint16_t, 24>;
+
+inline std::string read_preap_pedal_capsule(const std::string &path) {
+  const int fd = open(path.c_str(), O_RDONLY | O_NONBLOCK | O_NOFOLLOW);
+  if (fd < 0) return {};
+  struct stat before{}, after{}, current{};
+  std::array<char, 512> bytes{};
+  const bool sized = fstat(fd, &before) == 0 && S_ISREG(before.st_mode) && before.st_size >= 16 && before.st_size <= 512;
+  const ssize_t count = sized ? read(fd, bytes.data(), static_cast<size_t>(before.st_size)) : -1;
+  const bool stable = fstat(fd, &after) == 0 && lstat(path.c_str(), &current) == 0 && S_ISREG(current.st_mode) &&
+                      before.st_dev == after.st_dev && before.st_ino == after.st_ino &&
+                      before.st_dev == current.st_dev && before.st_ino == current.st_ino &&
+                      before.st_size == after.st_size && before.st_size == current.st_size &&
+                      before.st_mtime == after.st_mtime && before.st_mtime == current.st_mtime;
+  close(fd);
+  if (!sized || !stable || count != before.st_size) return {};
+  return std::string(bytes.data(), static_cast<size_t>(count));
+}
+
+inline std::optional<PreapPedalWords> preap_pedal_words(const std::string &raw,
+                                                       const std::string &cp_bytes,
+                                                       cereal::CarParams::Reader cp) {
+  if (raw.size() < 16 || raw.size() > 512 || raw.size() % 8 || cp_bytes.empty() || cp_bytes.size() > 1024 * 1024 ||
+      cp.getBrand() != "tesla" || cp.getCarFingerprint() != "TESLA_MODEL_S_PREAP" ||
+      cp.getPassive() || cp.getNotCar() || cp.getDashcamOnly() || !cp.getOpenpilotLongitudinalControl() ||
+      cp.getPcmCruise() || cp.getAlphaLongitudinalAvailable() || cp.getFlags() != 0 ||
+      (cp.getAlternativeExperience() != 0 && cp.getAlternativeExperience() != 32) ||
+      cp.getSafetyConfigs().size() != 1 || cp.getSafetyConfigs()[0].getSafetyModel() != cereal::CarParams::SafetyModel::TESLA_PREAP ||
+      cp.getTransmissionType() == cereal::CarParams::TransmissionType::MANUAL ||
+      cp.getFingerprintSource() == cereal::CarParams::FingerprintSource::FIXED ||
+      cp.getSteerControlType() != cereal::CarParams::SteerControlType::ANGLE || !cp.getSteerAtStandstill() ||
+      !std::isfinite(cp.getWheelbase()) || std::fabs(cp.getWheelbase() - 2.96F) > 1e-6F || cp.getSteerRatio() != 15.0F) return std::nullopt;
+  const uint16_t word = cp.getSafetyConfigs()[0].getSafetyParam();
+  if ((word != 1 && word != 3 && word != 7) || cp.getRadarUnavailable() != (word == 1)) return std::nullopt;
+  uint32_t segments = 0, words = 0;
+  memcpy(&segments, raw.data(), 4);
+  memcpy(&words, raw.data() + 4, 4);
+  if (segments != 0 || words > 63 || raw.size() != 8U + static_cast<size_t>(words) * 8U) return std::nullopt;
+  try {
+    AlignedBuffer aligned;
+    capnp::ReaderOptions options;
+    options.traversalLimitInWords = 63;
+    options.nestingLimit = 4;
+    capnp::FlatArrayMessageReader reader(aligned.align(raw.data(), raw.size()), options);
+    const auto context = reader.getRoot<cereal::StarPilotCarControl::PreapPedalStartup>();
+    const auto hash = context.getCarParamsSha256();
+    const auto actual_hash = preap_sha256(cp_bytes);
+    const float factor = context.getFactor(), zero = context.getZero();
+    const uint16_t bus = context.getPedalBus();
+    if (context.getVersion() != 1 || context.getKind() != 5 || context.getSafetyModel() != 39 ||
+        context.getSafetyParam() != word || hash.size() != 32 || memcmp(hash.begin(), actual_hash.data(), 32) != 0 ||
+        context.getSourceMonoTime() == 0 || context.getSourceMonoTime() > aol_monotonic_ns() ||
+        !std::isfinite(factor) || std::fabs(factor) <= 1e-6F || !std::isfinite(zero) || (bus != 0 && bus != 2)) return std::nullopt;
+    PreapPedalWords result{1, bus, word, 0xD051};
+    uint32_t fbits = 0, zbits = 0;
+    memcpy(&fbits, &factor, 4); memcpy(&zbits, &zero, 4);
+    result[4] = fbits & 65535; result[5] = fbits >> 16;
+    result[6] = zbits & 65535; result[7] = zbits >> 16;
+    for (size_t i = 0; i < 16; ++i) result[8 + i] = hash[2 * i] | (static_cast<uint16_t>(hash[2 * i + 1]) << 8);
+    return result;
+  } catch (const kj::Exception &) {
+    return std::nullopt;
+  }
+}

openpilot/selfdrive/pandad/preap_sha256.h

@@ -0,0 +1,53 @@
+#pragma once
+#include <array>
+#include <cstdint>
+#include <string_view>
+
+// SHA-256 over the bounded serialized startup CP; no external runtime library.
+inline std::array<uint8_t, 32> preap_sha256(std::string_view input) {
+  constexpr std::array<uint32_t, 64> k = {
+    0x428a2f98U, 0x71374491U, 0xb5c0fbcfU, 0xe9b5dba5U, 0x3956c25bU, 0x59f111f1U, 0x923f82a4U, 0xab1c5ed5U,
+    0xd807aa98U, 0x12835b01U, 0x243185beU, 0x550c7dc3U, 0x72be5d74U, 0x80deb1feU, 0x9bdc06a7U, 0xc19bf174U,
+    0xe49b69c1U, 0xefbe4786U, 0x0fc19dc6U, 0x240ca1ccU, 0x2de92c6fU, 0x4a7484aaU, 0x5cb0a9dcU, 0x76f988daU,
+    0x983e5152U, 0xa831c66dU, 0xb00327c8U, 0xbf597fc7U, 0xc6e00bf3U, 0xd5a79147U, 0x06ca6351U, 0x14292967U,
+    0x27b70a85U, 0x2e1b2138U, 0x4d2c6dfcU, 0x53380d13U, 0x650a7354U, 0x766a0abbU, 0x81c2c92eU, 0x92722c85U,
+    0xa2bfe8a1U, 0xa81a664bU, 0xc24b8b70U, 0xc76c51a3U, 0xd192e819U, 0xd6990624U, 0xf40e3585U, 0x106aa070U,
+    0x19a4c116U, 0x1e376c08U, 0x2748774cU, 0x34b0bcb5U, 0x391c0cb3U, 0x4ed8aa4aU, 0x5b9cca4fU, 0x682e6ff3U,
+    0x748f82eeU, 0x78a5636fU, 0x84c87814U, 0x8cc70208U, 0x90befffaU, 0xa4506cebU, 0xbef9a3f7U, 0xc67178f2U,
+  };
+  std::array<uint32_t, 8> hash = {0x6a09e667U, 0xbb67ae85U, 0x3c6ef372U, 0xa54ff53aU,
+                                  0x510e527fU, 0x9b05688cU, 0x1f83d9abU, 0x5be0cd19U};
+  const auto rotate = [](uint32_t value, unsigned bits) { return (value >> bits) | (value << (32U - bits)); };
+  const uint64_t bits = static_cast<uint64_t>(input.size()) * 8U;
+  const size_t blocks = input.size() / 64U + ((input.size() % 64U < 56U) ? 1U : 2U);
+  for (size_t block = 0; block < blocks; ++block) {
+    std::array<uint32_t, 64> words{};
+    for (size_t byte = 0; byte < 64U; ++byte) {
+      const size_t position = block * 64U + byte;
+      uint8_t value = 0;
+      if (position < input.size()) value = static_cast<uint8_t>(input[position]);
+      else if (position == input.size()) value = 0x80U;
+      else if (block == blocks - 1U && byte >= 56U) value = static_cast<uint8_t>(bits >> ((63U - byte) * 8U));
+      words[byte / 4U] |= static_cast<uint32_t>(value) << ((3U - byte % 4U) * 8U);
+    }
+    for (size_t i = 16; i < words.size(); ++i) {
+      const uint32_t s0 = rotate(words[i - 15], 7) ^ rotate(words[i - 15], 18) ^ (words[i - 15] >> 3);
+      const uint32_t s1 = rotate(words[i - 2], 17) ^ rotate(words[i - 2], 19) ^ (words[i - 2] >> 10);
+      words[i] = words[i - 16] + s0 + words[i - 7] + s1;
+    }
+    auto [a, b, c, d, e, f, g, h] = hash;
+    for (size_t i = 0; i < words.size(); ++i) {
+      const uint32_t s1 = rotate(e, 6) ^ rotate(e, 11) ^ rotate(e, 25);
+      const uint32_t choose = (e & f) ^ (~e & g);
+      const uint32_t t1 = h + s1 + choose + k[i] + words[i];
+      const uint32_t s0 = rotate(a, 2) ^ rotate(a, 13) ^ rotate(a, 22);
+      const uint32_t majority = (a & b) ^ (a & c) ^ (b & c);
+      h = g; g = f; f = e; e = d + t1; d = c; c = b; b = a; a = t1 + s0 + majority;
+    }
+    hash[0] += a; hash[1] += b; hash[2] += c; hash[3] += d;
+    hash[4] += e; hash[5] += f; hash[6] += g; hash[7] += h;
+  }
+  std::array<uint8_t, 32> result{};
+  for (size_t i = 0; i < result.size(); ++i) result[i] = static_cast<uint8_t>(hash[i / 4U] >> ((3U - i % 4U) * 8U));
+  return result;
+}

openpilot/selfdrive/pandad/tests/test_aol_protocol.cc

@@ -158,11 +158,12 @@ int main() {
   }
   for (uint32_t word = 0U; word <= 65535U; word++) {
     assert(mazda_aol_param(static_cast<uint16_t>(word)) == (word == 0U));
-    assert(tesla_preap_aol_param(static_cast<uint16_t>(word)) == (word == 0U));
+    const bool preap_expected = (word == 0U) || (word == 1U) || (word == 2U) || (word == 3U) || (word == 6U) || (word == 7U);
+    assert(tesla_preap_aol_param(static_cast<uint16_t>(word)) == preap_expected);
     auto preap = status();
     preap.safety_mode = 39U;
     preap.safety_param = static_cast<uint16_t>(word);
-    assert(aol_capable(preap, 39U, VEHICLE_REGISTRY) == (word == 0U));
+    assert(aol_capable(preap, 39U, VEHICLE_REGISTRY) == preap_expected);
     assert(!aol_capable(preap, 35U, VEHICLE_REGISTRY));
   }
   constexpr uint16_t ford_words[] = {8U, 9U, 10U, 11U, 12U, 13U, 18U, 19U, 32U, 33U, 66U, 67U};

openpilot/selfdrive/pandad/tests/test_preap_pedal_startup.cc

@@ -0,0 +1,191 @@
+#include <cstdlib>
+#include <cstring>
+#include <dlfcn.h>
+#include <memory>
+#include <stdexcept>
+#include <unistd.h>
+#include <vector>
+#include "common/tests/native_test.h"
+#include "selfdrive/pandad/pandad.h"
+#include "selfdrive/pandad/preap_pedal_wire.h"
+
+struct NativePedal {
+  void *library = nullptr;
+  int (*hooks)(uint16_t, uint16_t);
+  int (*mode)();
+  int (*param)();
+  void (*alternative)(int);
+  void (*heartbeat)(bool);
+  bool (*word)(uint16_t, uint16_t);
+  unsigned int (*read)(uint8_t *);
+  template<class T> T symbol(const char *name) {
+    auto result = reinterpret_cast<T>(dlsym(library, name));
+    if (result == nullptr) throw std::runtime_error(name);
+    return result;
+  }
+  NativePedal() {
+    const char *path = getenv("TESLA_PEDAL_LIBSAFETY");
+    if (path == nullptr || (library = dlopen(path, RTLD_NOW | RTLD_LOCAL)) == nullptr)
+      throw std::runtime_error("TESLA_PEDAL_LIBSAFETY must name the actual source-matched native library");
+    hooks = symbol<decltype(hooks)>("set_safety_hooks");
+    mode = symbol<decltype(mode)>("get_current_safety_mode");
+    param = symbol<decltype(param)>("get_current_safety_param");
+    alternative = symbol<decltype(alternative)>("set_alternative_experience");
+    heartbeat = symbol<decltype(heartbeat)>("set_aol_test_heartbeat");
+    word = symbol<decltype(word)>("tesla_preap_test_config_word");
+    read = symbol<decltype(read)>("tesla_preap_test_config_read");
+    heartbeat(false); CHECK(hooks(0, 0) == 0);
+  }
+  ~NativePedal() { heartbeat(false); hooks(0, 0); dlclose(library); }
+};
+
+class NativePedalHandle : public PandaCommsHandle {
+public:
+  NativePedal &native;
+  bool old_firmware = false;
+  std::vector<uint16_t> modes;
+  size_t word_writes = 0;
+  explicit NativePedalHandle(NativePedal &api) : native(api) { hw_serial = "actual-preap-native"; }
+  int control_write(uint8_t request, uint16_t p1, uint16_t p2, unsigned int) override {
+    if (request == 0xdc) { modes.push_back(p1); return native.hooks(p1, p2) == 0 ? 0 : -1; }
+    if (request == 0xdf) { native.alternative(p1); return 0; }
+    if (request == 0xb7) {
+      ++word_writes;
+      if (!old_firmware) (void)native.word(p1, p2);
+      return 0;  // Like the board handler, write completion is not an admission ack.
+    }
+    return 0;
+  }
+  int control_read(uint8_t request, uint16_t, uint16_t, unsigned char *data, uint16_t length, unsigned int) override {
+    if (request == 0xb8 && length == 48) return old_firmware ? 0 : static_cast<int>(native.read(data));
+    if (request == 0xd2 && length == sizeof(health_t)) {
+      health_t health{};
+      health.safety_mode_pkt = native.mode(); health.safety_param_pkt = native.param();
+      health.alternative_experience_pkt = 32;
+      memcpy(data, &health, length); return length;
+    }
+    return -1;
+  }
+  int bulk_write(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
+  int bulk_read(unsigned char, unsigned char *, int, unsigned int) override { return 0; }
+  void cleanup() override {}
+};
+class NativePedalPanda : public Panda {
+public:
+  explicit NativePedalPanda(std::unique_ptr<NativePedalHandle> h) :
+    Panda(std::move(h), cereal::PandaState::PandaType::RED_PANDA, 0) {}
+};
+
+std::string serialized(capnp::MallocMessageBuilder &builder) {
+  const auto words = capnp::messageToFlatArray(builder);
+  const auto bytes = words.asBytes();
+  return std::string(reinterpret_cast<const char *>(bytes.begin()), bytes.size());
+}
+std::string capsule(const std::string &cp_bytes, uint16_t bus, uint16_t word, float factor,
+                    uint16_t version = 1, uint16_t kind = 5, bool wrong_hash = false) {
+  capnp::MallocMessageBuilder message;
+  auto context = message.initRoot<cereal::StarPilotCarControl::PreapPedalStartup>();
+  context.setVersion(version); context.setKind(kind); context.setSafetyModel(39); context.setSafetyParam(word);
+  context.setPedalBus(bus); context.setFactor(factor); context.setZero(0.0F); context.setSourceMonoTime(aol_monotonic_ns());
+  auto hash = preap_sha256(cp_bytes);
+  if (wrong_hash) hash[0] ^= 1;
+  context.setCarParamsSha256(kj::arrayPtr(hash.data(), hash.size()));
+  return serialized(message);
+}
+
+void independent_sha256_vectors() {
+  const std::vector<std::pair<size_t, std::string>> vectors = {
+    {0, "e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855"},
+    {1, "6e340b9cffb37a989ca544e6bb780a2c78901d3fb33738768511a30617afa01d"},
+    {3, "ae4b3280e56e2faf83f414a6e3dabe9d5fbe18976544c05fed121accb85b53fc"},
+    {55, "463eb28e72f82e0a96c0a4cc53690c571281131f672aa229e0d45ae59b598b59"},
+    {56, "da2ae4d6b36748f2a318f23e7ab1dfdf45acdc9d049bd80e59de82a60895f562"},
+    {57, "2fe741af801cc238602ac0ec6a7b0c3a8a87c7fc7d7f02a3fe03d1c12eac4d8f"},
+    {63, "29af2686fd53374a36b0846694cc342177e428d1647515f078784d69cdb9e488"},
+    {64, "fdeab9acf3710362bd2658cdc9a29e8f9c757fcf9811603a8c447cd1d9151108"},
+    {65, "4bfd2c8b6f1eec7a2afeb48b934ee4b2694182027e6d0fc075074f2fabb31781"},
+    {119, "da18797ed7c3a777f0847f429724a2d8cd5138e6ed2895c3fa1a6d39d18f7ec6"},
+    {120, "f52b23db1fbb6ded89ef42a23ce0c8922c45f25c50b568a93bf1c075420bbb7c"},
+    {127, "92ca0fa6651ee2f97b884b7246a562fa71250fedefe5ebf270d31c546bfea976"},
+    {128, "471fb943aa23c511f6f72f8d1652d9c880cfa392ad80503120547703e56a2be5"},
+    {129, "5099c6a56203f9687f7d33f4bfdf576d31dc91f6b695ecea38b2770c87631135"},
+    {1024, "785b0751fc2c53dc14a4ce3d800e69ef9ce1009eb327ccf458afe09c242c26c9"},
+    {1048576, "fbbab289f7f94b25736c58be46a994c441fd02552cc6022352e3d86d2fab7c83"},
+  };
+  constexpr char hex[] = "0123456789abcdef";
+  for (const auto &[size, expected] : vectors) {
+    std::string input(size, '\0');
+    for (size_t i = 0; i < size; ++i) input[i] = static_cast<char>(i & 255U);
+    const auto hash = preap_sha256(input);
+    std::string actual;
+    for (uint8_t byte : hash) { actual += hex[byte >> 4]; actual += hex[byte & 15U]; }
+    CHECK(actual == expected);
+  }
+}
+
+void actual_panda_safety_to_native_startup() {
+  independent_sha256_vectors();
+  const char *old = getenv("OPENPILOT_PREFIX");
+  const std::string original_prefix = old ? old : "";
+  const std::string prefix = "test-preap-pedal-startup-" + std::to_string(getpid());
+  setenv("OPENPILOT_PREFIX", prefix.c_str(), 1);
+  Params params;
+  struct Restore {
+    Params &params; std::string previous;
+    ~Restore() {
+      for (const auto *key : {"CarParams", "TeslaPreAPPedalStartup", "FirmwareQueryDone", "ControlsReady", "ObdMultiplexingChanged"}) params.remove(key);
+      if (previous.empty()) unsetenv("OPENPILOT_PREFIX"); else setenv("OPENPILOT_PREFIX", previous.c_str(), 1);
+    }
+  } restore{params, original_prefix};
+  NativePedal native;
+  auto device = std::make_unique<NativePedalHandle>(native);
+  auto *handle = device.get();
+  NativePedalPanda panda(std::move(device));
+  for (const uint16_t bus : {0, 2}) for (const uint16_t word : {1, 3, 7}) {
+    capnp::MallocMessageBuilder cp_message;
+    auto cp = cp_message.initRoot<cereal::CarParams>();
+    cp.setBrand("tesla"); cp.setCarFingerprint("TESLA_MODEL_S_PREAP"); cp.setWheelbase(2.96F); cp.setSteerRatio(15.0F);
+    cp.setOpenpilotLongitudinalControl(true); cp.setPcmCruise(false); cp.setRadarUnavailable(word == 1);
+    cp.setSteerAtStandstill(true); cp.setSteerControlType(cereal::CarParams::SteerControlType::ANGLE);
+    cp.setTransmissionType(cereal::CarParams::TransmissionType::AUTOMATIC);
+    cp.setFingerprintSource(cereal::CarParams::FingerprintSource::CAN); cp.setAlternativeExperience(32);
+    auto safety = cp.initSafetyConfigs(1);
+    safety[0].setSafetyModel(cereal::CarParams::SafetyModel::TESLA_PREAP); safety[0].setSafetyParam(word);
+    const std::string cp_bytes = serialized(cp_message);
+    params.put("CarParams", cp_bytes); params.putBool("FirmwareQueryDone", true); params.putBool("ControlsReady", true);
+    params.put("TeslaPreAPPedalStartup", capsule(cp_bytes, bus, word, 1.0F));
+    PandaSafety owner({&panda});
+    owner.configureSafetyMode(true);
+    CHECK(native.mode() == 39); CHECK(native.param() == word);
+    auto expected = preap_pedal_words(params.get("TeslaPreAPPedalStartup"), cp_bytes, cp.asReader());
+    CHECK(expected.has_value()); CHECK(panda.matches_preap_pedal(*expected));
+    CHECK(!native.word(0, 1));  // Actual car-mode mutation guard, not a mock.
+    // Native reset must force an actual new stage/echo instead of trusting cached CP.
+    CHECK(native.hooks(0, 0) == 0);
+    const auto before_reset = handle->word_writes;
+    owner.configureSafetyMode(true);
+    CHECK(native.mode() == 39); CHECK(handle->word_writes > before_reset);
+    // A source/configuration restart withdraws before switching calibration.
+    const auto before_change = handle->modes.size();
+    params.put("TeslaPreAPPedalStartup", capsule(cp_bytes, bus, word, 2.0F));
+    owner.configureSafetyMode(true);
+    CHECK(handle->modes.size() > before_change); CHECK(handle->modes[before_change] == static_cast<uint16_t>(cereal::CarParams::SafetyModel::NO_OUTPUT));
+    CHECK(native.mode() == 39);
+    // Both wrong CP provenance and unknown versions/kinds must remain NO_OUTPUT.
+    for (const auto &bad : {capsule(cp_bytes, bus, word, 1.0F, 1, 5, true),
+                            capsule(cp_bytes, bus, word, 1.0F, 2, 5), capsule(cp_bytes, bus, word, 1.0F, 1, 4)}) {
+      owner.configureSafetyMode(false); params.put("TeslaPreAPPedalStartup", bad);
+      const auto before_bad = handle->word_writes;
+      owner.configureSafetyMode(true);
+      CHECK(native.mode() == static_cast<int>(cereal::CarParams::SafetyModel::NO_OUTPUT));
+      CHECK(handle->word_writes == before_bad);
+    }
+    owner.configureSafetyMode(false);
+    params.put("TeslaPreAPPedalStartup", capsule(cp_bytes, bus, word, 1.0F));
+    handle->old_firmware = true;
+    owner.configureSafetyMode(true);
+    CHECK(native.mode() == static_cast<int>(cereal::CarParams::SafetyModel::NO_OUTPUT));
+    handle->old_firmware = false;
+  }
+}
+int main() { return run_native_test(actual_panda_safety_to_native_startup); }

openpilot/selfdrive/selfdrived/selfdrived.py

@@ -565,10 +565,12 @@ class SelfdriveD:
         intent_companion = getattr(self, 'aol_intent_companion', None)
         drained = intent_companion is None or self.drain_axis_intent(intent_companion)
         now_ns = self.aol_car_state_log_ns if REPLAY and self.aol_car_state_log_ns else time.monotonic_ns()
-        companion = current_intent(self.sm, car_state_ns=int(self.sm.logMonoTime['carState']), now_ns=now_ns,
+        companion = current_intent(self.sm, car_state_ns=self.aol_car_state_log_ns, now_ns=now_ns,
                                    previous=self.aol_last_intent, companion=intent_companion, drain=False) if drained else None
         session = companion.producerSessionId if companion is not None else None
-        self.tesla_stock_alerts = self.tesla_stock_consumer.poll(self.sm, now_ns=now_ns, session=session)
+        self.tesla_stock_alerts = self.tesla_stock_consumer.poll(
+          self.sm, now_ns=now_ns, session=session, car_state=CS,
+          source_ns=self.aol_car_state_log_ns, source_valid=self.conditional_car_state_valid)
 
     if self.calibrated_pose is not None and not self.CP.notCar:
       excessive_actuation = self.excessive_actuation_check.update(self.sm, CS, self.calibrated_pose)

openpilot/starpilot/car/tesla/aol.py

@@ -19,15 +19,18 @@ def policy_for(cp):
                           runtime_supported=admitted, normal_runtime_supported=admitted,
                           ordinary_axis_ack_required=qualified(cp, marked_only=True),
                           explicit_latch=admitted, physical_stalk_owner=admitted, fixed_cruise_buttons=admitted,
+                          full_axis_runtime_required=admitted and cp.openpilotLongitudinalControl,
                           alternative_experience_addition=32 if admitted else 0)
 
 
 def native_profile_supported(model, param):
-  return (model == 39 and param == 0) or (model == 10 and param in SCREEN_WORDS)
+  return (model == 39 and param in (0, 1, 2, 3, 6, 7)) or (model == 10 and param in SCREEN_WORDS)
 
 
 def native_accepts_cp(cp, model, param):
-  return (qualified(cp, marked_only=True) or screen_qualified(cp, marked_only=True)) and native_profile_supported(model, param)
+  return bool((qualified(cp, marked_only=True) or screen_qualified(cp, marked_only=True)) and
+              native_profile_supported(model, param) and len(cp.safetyConfigs) == 1 and
+              int(cp.safetyConfigs[0].safetyModel.raw) == model and int(cp.safetyConfigs[0].safetyParam) == param)
 
 
 def native_latch_rejected(cp, native):

openpilot/starpilot/car/tesla/aol_policy.h

@@ -3,7 +3,7 @@
 #include "selfdrive/pandad/aol_protocol.h"
 
 inline bool tesla_preap_aol_param(uint16_t param) {
-  return param == 0U;
+  return (param == 0U) || (param == 1U) || (param == 2U) || (param == 3U) || (param == 6U) || (param == 7U);
 }
 
 inline constexpr AolSafetyProfile TESLA_PREAP_AOL_PROFILE{39U, tesla_preap_aol_param, true};

openpilot/starpilot/car/tesla/pedal_events.py

@@ -0,0 +1,54 @@
+"""Original Pre-AP pedal chimes/regen prompt from the existing custom companion."""
+from openpilot.cereal import log
+from openpilot.selfdrive.selfdrived.events import Alert, ET, Priority, VisualAlert
+from opendbc.car.tesla.preap.pedal_profile import qualified
+
+
+class PedalStatusConsumer:
+  def __init__(self, cp):
+    self.enabled = qualified(cp)
+    self.session = None
+    self.previous = False
+    self.stamp = 0
+
+  def update(self, sm, *, now_ns, session, car_state=None, source_ns=None, source_valid=False):
+    try:
+      if not self.enabled or not session:
+        return []
+      cs = sm['carState'] if car_state is None else car_state
+      status = sm['starpilotCarState']
+      stamp = sm.logMonoTime['carState'] if source_ns is None else source_ns
+      valid = (sm.valid['carState'] and sm.alive['carState']) if car_state is None else source_valid
+      if (not valid or not sm.valid['starpilotCarState'] or not sm.alive['starpilotCarState'] or
+          not cs.canValid or cs.canTimeout or not 0 < stamp == sm.logMonoTime['starpilotCarState'] ==
+          status.sourceCarStateMonoTime <= now_ns <= stamp + 100_000_000):
+        return []
+      if session != self.session:
+        self.session, self.previous, self.stamp = session, False, 0
+      if stamp <= self.stamp:
+        return []
+      self.stamp = stamp
+      active = bool(cs.cruiseState.enabled and status.pedalLongActive)
+      alerts = []
+      if active != self.previous:
+        alerts.append(pedal_alert('engaged' if active else 'disengaged'))
+      if status.pedalMaxRegen and active:
+        alerts.append(pedal_alert('maxRegen'))
+      self.previous = active
+      return alerts
+    except (AttributeError, KeyError, TypeError, ValueError, RuntimeError):
+      return []
+
+
+def pedal_alert(kind):
+  regen = kind == 'maxRegen'
+  active = kind == 'engaged'
+  alert = Alert('Max Regen Being Used' if regen else 'Pedal Cruise Engaged' if active else 'Pedal Cruise Disengaged', '',
+    log.SelfdriveState.AlertStatus.userPrompt if regen else log.SelfdriveState.AlertStatus.normal,
+    log.SelfdriveState.AlertSize.small, Priority.HIGH if regen else Priority.LOW,
+    VisualAlert.steerRequired if regen else VisualAlert.none,
+    log.SelfdriveState.AudibleAlert.prompt if regen else log.SelfdriveState.AudibleAlert.engage if active else
+    log.SelfdriveState.AudibleAlert.disengage, 2. if regen else .8)
+  alert.event_type = ET.WARNING
+  alert.alert_type = f'teslaPreapPedal/{kind}/{ET.WARNING}'
+  return alert

openpilot/starpilot/car/tesla/pedal_preferences.py

@@ -0,0 +1,47 @@
+"""One complete frozen snapshot of the working Dom Pre-AP pedal settings."""
+from dataclasses import dataclass
+from opendbc.car.tesla.preap.pedal_contract import PedalCalibration, float32
+from openpilot.starpilot.saved_source import read_saved
+
+KEYS = ('NAPPedalEnabled', 'NAPPedalCalibDone', 'NAPPedalCanBus', 'NAPPedalCalibFactor', 'NAPPedalCalibZero')
+
+
+@dataclass(frozen=True)
+class PedalSettings:
+  enabled: bool = False
+  valid: bool = True
+  calibration: PedalCalibration | None = None
+  personality: int = 1
+
+
+def read_pedal_settings(params):
+  try:
+    first = read_saved(params, 'NAPPedalEnabled', 8)
+    if first in ((None, True), (b'0', True)):
+      return PedalSettings()
+    if first != (b'1', True):
+      return PedalSettings(valid=False)
+    # Detect concurrent multi-key edits; never read live settings in a controller.
+    left = tuple(read_saved(params, name, 64) for name in KEYS)
+    right = tuple(read_saved(params, name, 64) for name in KEYS)
+    if left != right or not all(valid for _, valid in left) or left[0] != (b'1', True):
+      return PedalSettings(enabled=True, valid=False)
+    raw = {name: value for name, (value, _) in zip(KEYS, left, strict=True)}
+    if raw['NAPPedalCalibDone'] != b'1':
+      return PedalSettings(enabled=True, valid=False)
+    bus_raw = raw['NAPPedalCanBus']
+    if bus_raw not in (None, b'0', b'2'):
+      return PedalSettings(enabled=True, valid=False)
+    factor = float32(float(raw['NAPPedalCalibFactor'] if raw['NAPPedalCalibFactor'] is not None else b'1.0'))
+    zero = float32(float(raw['NAPPedalCalibZero'] if raw['NAPPedalCalibZero'] is not None else b'0.0'))
+    calibration = PedalCalibration(factor, zero, 0 if bus_raw == b'0' else 2)
+    if not calibration.valid():
+      return PedalSettings(enabled=True, valid=False)
+    try:
+      personality_raw, personality_valid = read_saved(params, 'LongitudinalPersonality', 64)
+      personality = int(personality_raw) if personality_valid and personality_raw is not None else 1
+    except (OSError, TypeError, ValueError, OverflowError):
+      personality = 1
+    return PedalSettings(True, True, calibration, personality if personality in (0, 1, 2) else 1)
+  except (OSError, AttributeError, TypeError, ValueError, OverflowError):
+    return PedalSettings(enabled=True, valid=False)

openpilot/starpilot/car/tesla/pedal_startup.py

@@ -0,0 +1,68 @@
+"""Versioned custom nested startup capsule; exact final CP and frozen settings."""
+import hashlib
+import struct
+import time
+import capnp
+
+from opendbc.car import Bus, structs
+from opendbc.car.tesla.preap.pedal_contract import PedalCalibration, PedalFeedback
+from opendbc.car.tesla.preap.pedal_profile import qualified, accel_max
+from opendbc.car.tesla.preap.pedal_parser import PedalCANParser
+from opendbc.car.tesla.preap.pedal_control import PreAPPedalControl
+from openpilot.cereal import custom
+from openpilot.starpilot.car.tesla.pedal_preferences import PedalSettings
+
+KEY = 'TeslaPreAPPedalStartup'
+VERSION = 1
+KIND = 5
+MAX_BYTES = 512
+SCHEMA = custom.StarPilotCarControl.PreapPedalStartup
+
+
+def decode(cp, cp_bytes, raw):
+  if type(raw) is not bytes or not 16 <= len(raw) <= MAX_BYTES or len(raw) % 8 or not qualified(cp):
+    return None
+  segments, words = struct.unpack_from('<II', raw)
+  if segments != 0 or len(raw) != 8 + words * 8 or words > 63:
+    return None
+  try:
+    with SCHEMA.from_bytes(raw, traversal_limit_in_words=63, nesting_limit=4) as value:
+      calibration = PedalCalibration(float(value.factor), float(value.zero), int(value.pedalBus))
+      if (int(value.version) != VERSION or int(value.kind) != KIND or int(value.safetyModel) != 39 or
+          int(value.safetyParam) != int(cp.safetyConfigs[0].safetyParam) or not calibration.valid() or
+          len(value.carParamsSha256) != 32 or bytes(value.carParamsSha256) != hashlib.sha256(cp_bytes).digest() or
+          value.sourceMonoTime <= 0 or value.sourceMonoTime > time.monotonic_ns()):
+        return None
+    with structs.CarParams.from_bytes(cp_bytes) as published:
+      return calibration if published.to_dict() == cp.to_dict() else None
+  except (AttributeError, ValueError, TypeError, OverflowError, RuntimeError, capnp.KjException):
+    return None
+
+
+def install(ci, cp, cp_bytes, settings, params):
+  if not qualified(cp):
+    params.remove(KEY)
+    return None
+  if (type(settings) is not PedalSettings or not settings.valid or not settings.enabled or
+      settings.calibration is None or not settings.calibration.valid()):
+    raise ValueError('preAP pedal startup settings missing or invalid')
+  calibration = settings.calibration
+  msg = SCHEMA.new_message(version=VERSION, kind=KIND, safetyModel=39,
+    safetyParam=int(cp.safetyConfigs[0].safetyParam), carParamsSha256=hashlib.sha256(cp_bytes).digest(),
+    factor=calibration.factor, zero=calibration.zero, pedalBus=calibration.bus, sourceMonoTime=time.monotonic_ns())
+  raw = msg.to_bytes()
+  if decode(cp, cp_bytes, raw) != calibration:
+    raise ValueError('preAP pedal startup capsule failed roundtrip')
+  previous = getattr(ci, 'preap_pedal_cp_sha256', None)
+  digest = hashlib.sha256(cp_bytes).digest()
+  if previous is not None:
+    raise ValueError('preAP pedal startup context may only be installed once')
+  feedback = PedalFeedback(calibration)
+  ci.can_parsers[Bus.ap_party] = PedalCANParser(feedback)
+  ci.CS.preap.pedal_feedback = feedback
+  ci.CS.preap.preap_accel_max = accel_max(0., settings.personality)
+  ci.CS.preap.pedal_personality = settings.personality
+  ci.CC.preap.pedal_control = PreAPPedalControl(calibration)
+  ci.preap_pedal_cp_sha256 = digest
+  params.put(KEY, raw, block=True)
+  return calibration

openpilot/starpilot/car/tesla/preap_adaptive.py

@@ -0,0 +1,87 @@
+"""Pre-AP optional close-lead acceleration cap from Dom's intentional planner."""
+import math
+
+from opendbc.car import structs
+from opendbc.car.tesla.values import CAR
+
+SOURCE_MAX_AGE_NS = 250_000_000
+SOURCES = ('carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState', 'modelV2')
+SPEED_BP = (0.0, 1.3, 7.5, 15.0, 25.0, 40.0)
+FOLLOW_ACCEL = (0.35, 0.55, 0.80, 0.75, 0.65, 0.50)
+
+
+def profile_key(cp):
+  try:
+    if (cp.brand != 'tesla' or cp.carFingerprint != CAR.TESLA_MODEL_S_PREAP or cp.passive or cp.notCar or
+        cp.dashcamOnly or not cp.openpilotLongitudinalControl or cp.pcmCruise or cp.alphaLongitudinalAvailable or
+        cp.steerControlType != structs.CarParams.SteerControlType.angle or len(cp.safetyConfigs) != 1 or
+        cp.transmissionType == structs.CarParams.TransmissionType.manual or
+        cp.fingerprintSource == structs.CarParams.FingerprintSource.fixed):
+      return None
+    if (not math.isfinite(cp.wheelbase) or not math.isclose(cp.wheelbase, 2.96, rel_tol=0., abs_tol=1e-6) or
+        cp.steerRatio != 15.):
+      return None
+    cfg = cp.safetyConfigs[0]
+    if cfg.safetyModel != structs.CarParams.SafetyModel.teslaPreap or cfg.safetyParam not in (1, 3, 7):
+      return None
+    return (cp.brand, str(cp.carFingerprint), int(cfg.safetyParam), float(cp.wheelbase), float(cp.steerRatio))
+  except (AttributeError, IndexError, ValueError, TypeError, OverflowError):
+    return None
+
+
+def follow_limit(speed):
+  if speed <= SPEED_BP[0]:
+    return FOLLOW_ACCEL[0]
+  for index in range(1, len(SPEED_BP)):
+    if speed <= SPEED_BP[index]:
+      fraction = (speed - SPEED_BP[index - 1]) / (SPEED_BP[index] - SPEED_BP[index - 1])
+      return FOLLOW_ACCEL[index - 1] + fraction * (FOLLOW_ACCEL[index] - FOLLOW_ACCEL[index - 1])
+  return FOLLOW_ACCEL[-1]
+
+
+class PreapAdaptiveAccel:
+  def __init__(self, cp, enabled):
+    self.cp = cp
+    self.key = profile_key(cp)
+    self.enabled = type(enabled) is bool and enabled and self.key is not None
+
+  def sample(self, sm, *, now_ns, drive_id, upper, follow_seconds, active):
+    try:
+      if not self.enabled or not active or profile_key(self.cp) != self.key:
+        return None
+      if (type(now_ns) is not int or type(drive_id) is not int or not 0 < drive_id < now_ns or
+          not all(sm.valid[name] and sm.alive[name] and drive_id <= sm.logMonoTime[name] <= now_ns and
+                  now_ns - sm.logMonoTime[name] <= SOURCE_MAX_AGE_NS for name in SOURCES)):
+        return None
+      car, lead = sm['carState'], sm['radarState'].leadOne
+      if (not car.canValid or car.canTimeout or car.gasPressed or car.brakePressed or
+          car.gearShifter != structs.CarState.GearShifter.drive or car.doorOpen or car.steerFaultPermanent or
+          sm['controlsState'].forceDecel or not sm['selfdriveState'].enabled or not sm['carControl'].longActive or
+          not lead.present):
+        return None
+      speed, distance, upper, follow = float(car.vEgo), float(lead.dRel), float(upper), float(follow_seconds)
+      if (not all(math.isfinite(v) for v in (speed, distance, upper, follow, float(lead.vLead))) or
+          speed < 0 or distance <= 0 or upper < 0 or not .5 <= follow <= 3.):
+        return None
+      # Original get_safe_obstacle_distance uses COMFORT_BRAKE2.5/STOP_DISTANCE6.
+      safe_distance = speed * speed / 5.0 + follow * speed + 6.0
+      strength = max(0.0, min(1.0, 1.0 - (distance / max(safe_distance, 1.0) - 1.2) / 0.3))
+      if strength <= 0:
+        return None
+      return min(upper, upper * (1.0 - strength) + follow_limit(speed) * strength)
+    except (AttributeError, KeyError, IndexError, TypeError, ValueError, OverflowError):
+      return None
+
+
+def startup_owner(cp):
+  if profile_key(cp) is None:
+    return None
+  from openpilot.common.params import Params
+  from openpilot.starpilot.saved_source import read_saved
+  # Freeze the complete saved bool once; no live toggle read inside model updates.
+  try:
+    raw, valid = read_saved(Params(), 'NAPAdaptiveAccel', 8)
+    enabled = valid and raw in (None, b'1')
+  except (OSError, AttributeError, TypeError, ValueError):
+    enabled = False
+  return PreapAdaptiveAccel(cp, enabled)

openpilot/starpilot/car/tesla/preap_preferences.py

@@ -1,20 +1,49 @@
 from opendbc.car import get_safety_config, structs
 from opendbc.car.tesla.values import CAR
 from opendbc.car.tesla.preap.aol import qualified
-from openpilot.starpilot.saved_source import read_saved
-
-KEYS = ("NAPPedalEnabled", "NAPRadarEnabled", "NAPRadarBehindNosecone")
+from opendbc.car.tesla.preap.radar_profile import qualified as radar_qualified
+from openpilot.starpilot.car.tesla.radar_settings import read_radar_settings
 
 
-def stock_configuration(params):
+def stock_configuration(params, radar=None, pedal=None):
   try:
-    return all(read_saved(params, key, 8) in ((None, True), (b"0", True)) for key in KEYS)
+    from openpilot.starpilot.car.tesla.pedal_preferences import read_pedal_settings
+    pedal = read_pedal_settings(params) if pedal is None else pedal
+    radar = read_radar_settings(params) if radar is None else radar
+    return pedal.valid and radar.valid
   except (OSError, TypeError, ValueError):
     return False
 
 
-def prepare_stock(cp, admitted):
-  if cp.brand == "tesla" and cp.carFingerprint == CAR.TESLA_MODEL_S_PREAP and (not admitted or not qualified(cp)):
+def prepare_stock(cp, admitted, radar=None, pedal=None):
+  if cp.brand != "tesla" or cp.carFingerprint != CAR.TESLA_MODEL_S_PREAP:
+    return cp
+  if pedal is not None and pedal.enabled:
+    from opendbc.car.tesla.preap.pedal_profile import qualified as pedal_qualified
+    from opendbc.car.tesla.preap.constants import PEDAL_LONG_K_BP, PEDAL_LONG_KI_V
+    desired = 1 | ((6 if radar.behind_nosecone else 2) if radar is not None and radar.enabled else 0)
+    if (admitted and pedal.valid and pedal.calibration is not None and pedal.calibration.valid() and
+        (qualified(cp) or pedal_qualified(cp))):
+      cp.safetyConfigs[0].safetyParam = desired
+      cp.radarUnavailable = desired == 1
+      cp.openpilotLongitudinalControl = True
+      cp.pcmCruise = False
+      cp.alphaLongitudinalAvailable = False
+      cp.longitudinalTuning.kiBP = PEDAL_LONG_K_BP
+      cp.longitudinalTuning.kiV = PEDAL_LONG_KI_V
+      cp.longitudinalActuatorDelay = .4
+      if pedal_qualified(cp):
+        return cp
+    cp.dashcamOnly = True
+    cp.alternativeExperience = 0
+    cp.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]
+    return cp
+  if radar is not None and not radar.valid:
+    admitted = False
+  if admitted and radar is not None and radar.valid and radar.enabled and (qualified(cp) or radar_qualified(cp)):
+    cp.safetyConfigs[0].safetyParam = 6 if radar.behind_nosecone else 2
+    cp.radarUnavailable = False
+  if not admitted or not (radar_qualified(cp) if radar is not None and radar.enabled else qualified(cp)):
     cp.dashcamOnly = True
     cp.alternativeExperience = 0
     cp.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.noOutput)]

openpilot/starpilot/car/tesla/radar_context.py

@@ -0,0 +1,58 @@
+"""Reserved custom startup context; no live Params or core schema extension."""
+import hashlib
+import struct
+
+import capnp
+
+from openpilot.cereal import custom
+from opendbc.car.tesla.preap.radar_profile import RadarStartupContext, qualified, valid_context
+from openpilot.starpilot.car.tesla.radar_settings import RadarSettings, offset_float32
+
+VERSION = 1
+KIND = 4
+MAX_BYTES = 256
+MAX_WORDS = (MAX_BYTES - 8) // 8
+SCHEMA = custom.StarPilotCarControl.PreapRadarStartup
+
+
+def encode_context(cp, cp_bytes, context):
+  if not valid_context(cp, context, cp_bytes):
+    raise ValueError("Invalid Tesla radar startup context")
+  message = SCHEMA.new_message(version=VERSION, kind=KIND, carParamsSha256=context.car_params_sha256,
+                               safetyModel=39, safetyParam=context.safety_param, lateralOffset=context.offset)
+  raw = message.to_bytes()
+  if len(raw) > MAX_BYTES:
+    raise ValueError("Oversized Tesla radar startup context")
+  return raw
+
+
+def decode_context(cp, cp_bytes, raw):
+  if type(raw) is not bytes or not 16 <= len(raw) <= MAX_BYTES or len(raw) % 8:
+    return None
+  segments, words = struct.unpack_from('<II', raw)
+  if segments != 0 or words > MAX_WORDS or len(raw) != 8 + words * 8:
+    return None
+  try:
+    with SCHEMA.from_bytes(raw, traversal_limit_in_words=MAX_WORDS, nesting_limit=4) as value:
+      if int(value.version) != VERSION or int(value.kind) != KIND or int(value.safetyModel) != 39:
+        return None
+      context = RadarStartupContext(int(value.version), int(value.safetyParam), bytes(value.carParamsSha256), float(value.lateralOffset))
+    return context if valid_context(cp, context, cp_bytes) else None
+  except (ValueError, TypeError, OverflowError, RuntimeError, capnp.KjException):
+    return None
+
+
+def install_startup_context(ri, cp, cp_bytes, settings):
+  if not qualified(cp):
+    return None
+  word = int(cp.safetyConfigs[0].safetyParam)
+  if (type(settings) is not RadarSettings or type(settings.valid) is not bool or not settings.valid or
+      type(settings.enabled) is not bool or not settings.enabled or type(settings.behind_nosecone) is not bool or
+      word != ((6 if settings.behind_nosecone else 2) | int(cp.openpilotLongitudinalControl))):
+    raise ValueError("Tesla radar capability does not match its frozen startup settings")
+  context = RadarStartupContext(VERSION, word, hashlib.sha256(cp_bytes).digest(), offset_float32(settings.offset))
+  decoded = decode_context(cp, cp_bytes, encode_context(cp, cp_bytes, context))
+  if decoded is None:
+    raise ValueError("Tesla radar startup context did not survive serialization")
+  ri.configure_preap_radar(decoded, cp_bytes)
+  return decoded

openpilot/starpilot/car/tesla/radar_settings.py

@@ -0,0 +1,74 @@
+from dataclasses import dataclass
+import json
+import math
+import struct
+
+from openpilot.starpilot.saved_source import read_saved
+
+DOCUMENT_KEY = "TeslaPreAPRadarConfig"
+MAX_BYTES = 1024
+MODES = ("off", "front", "nosecone")
+
+
+def offset_float32(value):
+  if isinstance(value, bool):
+    raise ValueError("Invalid radar offset")
+  value = struct.unpack("f", struct.pack("f", float(value)))[0]
+  if not math.isfinite(value):
+    raise ValueError("Invalid radar offset")
+  return value
+
+
+@dataclass(frozen=True)
+class RadarSettings:
+  enabled: bool
+  behind_nosecone: bool
+  offset: float
+  valid: bool
+  sources: tuple = ()
+
+
+def encode(mode, offset):
+  if mode not in MODES:
+    raise ValueError("Invalid radar mode")
+  return json.dumps({"version": 1, "mode": mode, "offset": offset_float32(offset)},
+                    sort_keys=True, separators=(",", ":"), allow_nan=False).encode()
+
+
+def read_radar_settings(params):
+  sources = ()
+  try:
+    document, readable = read_saved(params, DOCUMENT_KEY, MAX_BYTES)
+    sources = ((DOCUMENT_KEY, document),)
+    if not readable:
+      return RadarSettings(False, False, 0.0, False, sources)
+    if document is not None:
+      def unique_pairs(pairs):
+        result = {}
+        for key, value in pairs:
+          if key in result:
+            raise ValueError("Duplicate radar configuration key")
+          result[key] = value
+        return result
+
+      value = json.loads(document, object_pairs_hook=unique_pairs)
+      if (not isinstance(value, dict) or set(value) != {"version", "mode", "offset"} or
+          type(value["version"]) is not int or value["version"] != 1 or value["mode"] not in MODES or
+          type(value["offset"]) not in (int, float)):
+        raise ValueError("Invalid radar configuration")
+      offset = offset_float32(value["offset"])
+      return RadarSettings(value["mode"] != "off", value["mode"] == "nosecone", offset, True, sources)
+    enabled, enabled_ok = read_saved(params, "NAPRadarEnabled", 8)
+    mount, mount_ok = read_saved(params, "NAPRadarBehindNosecone", 8)
+    sources += (("NAPRadarEnabled", enabled), ("NAPRadarBehindNosecone", mount))
+    flags_valid = bool(enabled_ok and mount_ok and enabled in (None, b"0", b"1") and mount in (None, b"0", b"1"))
+    if not flags_valid or (mount == b"1" and enabled != b"1"):
+      return RadarSettings(False, False, 0.0, False, sources)
+    if enabled != b"1":
+      return RadarSettings(False, False, 0.0, True, sources)
+    raw, offset_ok = read_saved(params, "NAPRadarOffset", 64)
+    sources += (("NAPRadarOffset", raw),)
+    offset = offset_float32(0.0 if raw is None else float(raw))
+    return RadarSettings(True, mount == b"1", offset, bool(offset_ok), sources)
+  except (OSError, TypeError, ValueError, OverflowError):
+    return RadarSettings(False, False, 0.0, False, sources)

openpilot/starpilot/car/tesla/stock_events.py

@@ -17,6 +17,8 @@ def cp_fingerprint(cp):
 
 def publish_stock_status(pm, cp, internal, *, session, sequence, car_state_stamp, valid):
   from openpilot.cereal import messaging
+  if cp.openpilotLongitudinalControl:
+    return
   binding = cp_fingerprint(cp)
   if binding is None:
     return
@@ -36,17 +38,22 @@ def publish_stock_status(pm, cp, internal, *, session, sequence, car_state_stamp
 
 class StockCruiseConsumer:
   def __init__(self, cp):
+    from openpilot.starpilot.car.tesla.pedal_events import PedalStatusConsumer
+    self.pedal = PedalStatusConsumer(cp)
     self.binding = cp_fingerprint(cp)
     self.session = None
     self.sequence = -1
     self.previous = False
     self.pending = []
     self.socket = None
-    if self.binding is not None:
+    if self.binding is not None and not self.pedal.enabled:
       from openpilot.cereal import messaging
       self.socket = messaging.sub_sock('slcCruiseEvent', conflate=False)
 
-  def poll(self, sm, *, now_ns, session):
+  def poll(self, sm, *, now_ns, session, car_state=None, source_ns=None, source_valid=False):
+    if self.pedal.enabled:
+      return self.pedal.update(sm, now_ns=now_ns, session=session, car_state=car_state,
+                               source_ns=source_ns, source_valid=source_valid)
     from openpilot.cereal import messaging
     for _ in range(128):
       envelope = messaging.recv_one_or_none(self.socket)
@@ -55,26 +62,29 @@ class StockCruiseConsumer:
       if envelope.which() == 'slcCruiseEvent' and str(envelope.slcCruiseEvent.kind) == 'teslaStockCruise':
         self.pending.append(envelope)
     self.pending = self.pending[-16:]
-    current_stamp = sm.logMonoTime['carState']
+    current_stamp = sm.logMonoTime['carState'] if source_ns is None else source_ns
     alerts = []
     for envelope in self.pending:
       if envelope.logMonoTime == current_stamp:
-        alerts.extend(self.update(sm, envelope=envelope, now_ns=now_ns, session=session))
+        alerts.extend(self.update(sm, envelope=envelope, now_ns=now_ns, session=session,
+                                  car_state=car_state, source_ns=source_ns, source_valid=source_valid))
     self.pending = [item for item in self.pending if current_stamp < item.logMonoTime <= now_ns + 100_000_000]
 
     return alerts
 
-  def update(self, sm, *, envelope, now_ns, session):
+  def update(self, sm, *, envelope, now_ns, session, car_state=None, source_ns=None, source_valid=False):
     try:
       record = envelope.slcCruiseEvent
       stock = record.teslaStockCruise
-      cs = sm['carState']
+      cs = sm['carState'] if car_state is None else car_state
+      current_stamp = sm.logMonoTime['carState'] if source_ns is None else source_ns
+      valid = (sm.valid['carState'] and sm.alive['carState']) if car_state is None else source_valid
       stamp = envelope.logMonoTime
-      if (self.binding is None or not envelope.valid or not sm.valid['carState'] or not sm.alive['carState'] or
+      if (self.binding is None or not envelope.valid or not valid or
           not cs.canValid or cs.canTimeout or str(record.kind) != 'teslaStockCruise' or
           stock.version != 1 or stock.cpFingerprint != self.binding or not session or
           record.producerSessionId != session or
-          not 0 < stock.carStateMonoTime == record.observedMonoTime == stamp == sm.logMonoTime['carState'] <= now_ns <= stamp + 100_000_000):
+          not 0 < stock.carStateMonoTime == record.observedMonoTime == stamp == current_stamp <= now_ns <= stamp + 100_000_000):
         return []
       if self.session != session:
         self.session, self.sequence, self.previous = session, -1, False

openpilot/starpilot/car/tesla/stock_intent.py

@@ -46,11 +46,12 @@ class TeslaPreapCardIntent(AolCardIntent):
     super().update(state.as_reader(), **kwargs)
     healthy = bool(CS.canValid and not CS.canTimeout)
     fault = bool(kwargs.get('fault_active') or CS.accFaulted or CS.steerFaultPermanent or CS.steeringDisengage or
-                 CS.doorOpen or CS.gearShifter != structs.CarState.GearShifter.drive)
+                 CS.doorOpen or CS.gearShifter not in (structs.CarState.GearShifter.drive,
+                   structs.CarState.GearShifter.reverse, structs.CarState.GearShifter.neutral))
     if not healthy or fault or cancel:
       self.allowed_latch = False
       self.pull_neutral = False
-    elif pull and not self._fault_inhibit:
+    elif pull and not self._fault_inhibit and CS.gearShifter == structs.CarState.GearShifter.drive:
       self.allowed_latch = True
       self.pause_lateral = False
       self._last_latch_edge_ns = kwargs.get('now_ns', 0)

openpilot/starpilot/car/tesla/tests/pedal_fixture.py

@@ -0,0 +1,85 @@
+"""Actual selected Card startup and guarded native pedal configuration helpers."""
+import hashlib
+import struct
+from contextlib import contextmanager
+from unittest.mock import patch
+
+from opendbc.car import gen_empty_fingerprint, structs
+from opendbc.car.tesla.fingerprints import FINGERPRINTS
+from opendbc.car.tesla.values import CAR
+from opendbc.car.tesla.preap.pedal_contract import M1, M2, D
+from openpilot.cereal import messaging
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.selfdrive.car.card import Car
+from openpilot.selfdrive.controls.controlsd import Controls
+
+
+def native_words(calibration, word, cp_bytes):
+  prefix = struct.pack('<HHHHff', 1, calibration.bus, word, 0xD051, calibration.factor, calibration.zero)
+  return struct.unpack('<24H', prefix + hashlib.sha256(cp_bytes).digest())
+
+
+def configure_native(safety, calibration, word, cp_bytes):
+  safety.init_tests()
+  assert safety.set_safety_hooks(0, 0) == 0
+  safety.set_aol_test_heartbeat(False)
+  safety.set_alternative_experience(32)
+  words = native_words(calibration, word, cp_bytes)
+  for i, value in enumerate(words):
+    assert safety.tesla_preap_test_config_word(i, value)
+  assert safety.tesla_preap_test_config_word(0xFFFF, 0xC051)
+  assert safety.set_safety_hooks(39, word) == 0
+  assert not safety.tesla_preap_test_config_word(0, 1), 'car-mode calibration mutation must fail'
+
+
+def feedback_frame(calibration, counter, *, di=0., state=0, wrong_bus=False, bad_channel=False):
+  physical = calibration.physical(di)
+  one = int((physical - D) / M1)
+  two = int((physical - D) / M2) + (200 if bad_channel else 0)
+  data = struct.pack('>HHB', one, two, ((state & 15) << 4) | (counter & 15)) + b'\x00'
+  # Byte5 feedback checksum was unvalidated/unspecified in the working Dom path.
+  # Do not invent the unrelated generic pedal's CRC in this actual fixture.
+  return 0x552, data, 2 - calibration.bus if wrong_bus else calibration.bus
+
+
+@contextmanager
+def actual_pedal_startup(bus, radar_mode, release, monkeypatch):
+  from opendbc.car import car_helpers
+  from openpilot.selfdrive.car import card as card_module
+  from openpilot.starpilot.car.tesla.pedal_startup import KEY, decode
+  for name, value in (('SIMULATION', '1'), ('REPLAY', '1'), ('AOL_REPLAY_RUNTIME', '1')):
+    monkeypatch.setenv(name, value)
+  fingerprint = gen_empty_fingerprint()
+  fingerprint[0] = dict(FINGERPRINTS[CAR.TESLA_MODEL_S_PREAP][0])
+  with OpenpilotPrefix():
+    params = Params()
+    for key, value in (('OpenpilotEnabledToggle', True), ('AlwaysOnLateral', False),
+                       ('NAPPedalEnabled', True), ('NAPPedalCalibDone', True), ('IsReleaseBranch', release)):
+      params.put_bool(key, value, block=True)
+    params.put('NAPPedalCanBus', bus, block=True)
+    params.put('NAPPedalCalibFactor', 1.0, block=True)
+    params.put('NAPPedalCalibZero', 0.0, block=True)
+    params.put('TeslaPreAPRadarConfig', {'version': 1, 'mode': radar_mode, 'offset': .25}, block=True)
+    observed = (CAR.TESLA_MODEL_S_PREAP, fingerprint, '0' * 17, [], structs.CarParams.FingerprintSource.can, True)
+    with patch.object(car_helpers, 'fingerprint', return_value=observed), \
+         patch.object(card_module, 'can_comm_callbacks', return_value=(lambda *args: [], lambda frames: None)), \
+         patch.object(card_module.messaging, 'recv_one_retry', return_value=messaging.new_message('can', 1)):
+      card = Car()
+    word = {'off': 1, 'front': 3, 'nosecone': 7}[radar_mode]
+    assert card.CP.openpilotLongitudinalControl and not card.CP.pcmCruise
+    assert card.CP.safetyConfigs[0].safetyParam == word and card.CP.alternativeExperience == 32
+    assert card.CP.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.teslaPreap
+    assert card.CP.radarUnavailable == (radar_mode == 'off')
+    cp_bytes = params.get('CarParams')
+    calibration = decode(card.CP, cp_bytes, params.get(KEY))
+    assert calibration is not None and calibration.bus == bus
+    assert card.CI.CS.preap.pedal_feedback.calibration == calibration
+    assert card.CI.CC.preap.pedal_control.codec.calibration == calibration
+    controls = Controls()
+    assert controls.CP.to_dict() == card.CP.to_dict()
+    # Later saved edits cannot change the per-drive parser/controller contract.
+    params.put('NAPPedalCalibFactor', 2.0, block=True)
+    params.put('NAPPedalCanBus', 2 - bus, block=True)
+    assert card.CI.CS.preap.pedal_feedback.calibration == calibration
+    yield card, controls, calibration, cp_bytes

openpilot/starpilot/car/tesla/tests/schema_fixture.py

@@ -0,0 +1,8 @@
+"""Compare complete compiled layouts, including opaque pointer default values."""
+def layout(schema):
+  result = schema.node.struct.to_dict()
+  for field in result.get('fields', []):
+    if 'slot' in field and next(iter(field['slot']['type'])) in ('struct', 'list', 'anyPointer', 'data'):
+      value = schema.fields[field['name']].proto.slot.defaultValue
+      field['slot']['defaultValue'] = {'schemaValueHex': value.as_builder().to_bytes().hex()}
+  return result

openpilot/starpilot/car/tesla/tests/test_pedal_callers.py

@@ -0,0 +1,168 @@
+"""Actual selected startup -> physical CAN -> Card/SD/Controls/Card.apply -> registered native."""
+import gc
+from unittest.mock import patch
+import pytest
+
+from opendbc.car import structs
+from opendbc.car.tesla.tests.preap_stream import PreAPStream
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.cereal import messaging
+from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+from openpilot.starpilot.aol.wire import SafetyState, PandaSlot, encode_safety
+from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
+from openpilot.starpilot.tests.test_tesla_screen_integration import selfdrive_step
+from openpilot.starpilot.car.tesla.tests.pedal_fixture import actual_pedal_startup, configure_native, feedback_frame
+
+
+@pytest.mark.parametrize('bus', [0, 2])
+@pytest.mark.parametrize('radar_mode', ['off', 'front', 'nosecone'])
+@pytest.mark.parametrize('fault', ['gas', 'brake', 'dropout', 'reverse', 'feedback_state', 'counter', 'channels', 'wrong_bus',
+                                   'eps', 'door', 'belt', 'eps_temporary'])
+def test_real_selected_pedal_fault_withdrawal_and_physical_reengagement(bus, radar_mode, fault, monkeypatch):
+  safety = libsafety_py.libsafety
+  release = safety.set_safety_hooks(structs.CarParams.SafetyModel.allOutput, 0) != 0
+  with actual_pedal_startup(bus, radar_mode, release, monkeypatch) as (card, controls, calibration, cp_bytes):
+    ci, word = card.CI, int(card.CP.safetyConfigs[0].safetyParam)
+    configure_native(safety, calibration, word, cp_bytes)
+    with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False):
+      sd = SelfdriveD(CP=card.CP)
+    stream = PreAPStream()
+    previous = structs.CarControl()
+    receipt = None
+    selfdriven, observed, diagnostics = {}, {}, {}
+    alerts_seen = set()
+    try:
+      for tick in range(310):
+        now = 2_000_000_000 + tick * 10_000_000
+        bad = 120 <= tick < (180 if fault in ('dropout', 'reverse', 'wrong_bus') else 140)
+        lever = 2 if tick in (65, 75, 220, 230) else 1 if tick == 275 else 0
+        incoming = stream.frames(tick, lever=lever, cruise=1,
+          brake=2 if bad and fault == 'brake' else 1, gear=2 if bad and fault == 'reverse' else 4,
+          doors=1 if bad and fault == 'door' else 0, belt=0 if bad and fault == 'belt' else 1,
+          eps_status=3 if bad and fault == 'eps' else 0 if bad and fault == 'eps_temporary' else 1,
+          eps_error=15 if bad and fault == 'eps' else 0)
+        if tick % 2 == 0 and not (bad and fault == 'dropout'):
+          counter = 59 if bad and fault == 'counter' else tick // 2
+          incoming.append(feedback_frame(calibration, counter, di=8. if bad and fault == 'gas' else 0.,
+            state=1 if bad and fault == 'feedback_state' else 0,
+            bad_channel=bad and fault == 'channels', wrong_bus=bad and fault == 'wrong_bus'))
+        safety.set_timer(now // 1000)
+        safety.set_aol_test_heartbeat(bool(card.aol_card_intent.allowed_latch or sd.enabled))
+        for address, data, source in incoming:
+          safety.safety_rx_hook(libsafety_py.make_CANPacket(address, source, data))
+        safety.safety_tick()
+        peers = []
+        cc = messaging.new_message('carControl', valid=True, logMonoTime=now)
+        cc.carControl = previous
+        peers.append(cc.as_reader())
+        peers.append(selfdriven['onroadEvents'] if selfdriven else
+                     messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now).as_reader())
+        panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now)
+        panda.pandaStates[0].safetyModel = card.CP.safetyConfigs[0].safetyModel
+        panda.pandaStates[0].safetyParam = word
+        panda.pandaStates[0].alternativeExperience = 32
+        panda.pandaStates[0].controlsAllowed = bool(safety.get_controls_allowed())
+        panda.pandaStates[0].safetyRxChecksInvalid = not bool(safety.safety_config_valid())
+        peers.append(panda.as_reader())
+        if receipt is not None:
+          peers.append(receipt.as_reader())
+        if selfdriven:
+          peers.extend(selfdriven[key] for key in ('aolAxisState', 'selfdriveState'))
+        card.sm.update_msgs(now / 1e9, peers)
+        card.CC_prev = previous
+        with patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']), \
+             patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(now, incoming)]), \
+             patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now), \
+             patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now), \
+             patch('openpilot.selfdrive.car.card.REPLAY', False), patch.object(card.sm, 'update'):
+          cs, radar = card.state_update()
+        published = {}
+        factory = messaging.new_message
+
+        def stamped(service, size=None, factory=factory, now=now, **kwargs):
+          return factory(service, size, **({'logMonoTime': now} | kwargs))
+
+        with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped), \
+             patch.object(card.pm, 'send', side_effect=lambda service, msg, published=published: published.update({service: msg.as_reader()})):
+          card.state_publish(cs, radar)
+        published['pandaStates'] = panda.as_reader()
+        assert published['starpilotCarState'].logMonoTime == published['carState'].logMonoTime
+        status = published['starpilotCarState'].starpilotCarState
+        assert status.sourceCarStateMonoTime == published['carState'].logMonoTime
+        assert status.pedalLongActive == ci.CS.preap.enableLongControl
+        assert status.pedalMaxRegen == (ci.CS.preap.pccEvent == 'pedalMaxRegen')
+        mask = safety.aol_get_permission_mask()
+        native = SafetyState(1, True, now, now + 30_000_000, 39, word, bool(mask & 1), bool(mask & 2),
+                             bool(safety.aol_get_request_mask() & 1), bool(safety.aol_get_request_mask() & 2),
+                             'tesla-pedal-native', sd.aol_session_id, now,
+                             (PandaSlot(0, 'tesla-pedal-native', 39, word, 32, bool(safety.get_controls_allowed()),
+                                        not bool(safety.safety_config_valid())),))
+        receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
+        receipt.aolSafetyWire = encode_safety(native)
+
+        def with_actual_companion(service, size=None, published=published, factory=factory, **kwargs):
+          if service == 'starpilotCarState' and service in published:
+            return published[service].as_builder()
+          return factory(service, size, **kwargs)
+
+        with patch('openpilot.selfdrive.selfdrived.selfdrived.messaging.new_message', side_effect=with_actual_companion):
+          selfdriven.update(selfdrive_step(sd, published, receipt, previous, card.CP, safety, now))
+        alerts_seen.update(alert.alert_type for alert in sd.tesla_stock_alerts)
+        wanted = sd.aol_axis_decision
+        safety.set_aol_test_heartbeat(bool(card.aol_card_intent.allowed_latch or sd.enabled))
+        safety.aol_set_host_request(int(wanted.desired_lateral) | (int(wanted.desired_longitudinal) << 1))
+        mask = safety.aol_get_permission_mask()
+        native = SafetyState(1, True, now, now + 30_000_000, 39, word, bool(mask & 1), bool(mask & 2),
+                             wanted.desired_lateral, wanted.desired_longitudinal, 'tesla-pedal-native', sd.aol_session_id, now,
+                             (PandaSlot(0, 'tesla-pedal-native', 39, word, 32, bool(safety.get_controls_allowed()),
+                                        not bool(safety.safety_config_valid())),))
+        receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
+        receipt.aolSafetyWire = encode_safety(native)
+        feed(controls, now, tick, active=sd.active, enabled=sd.enabled)
+        controls.sm.update_msgs((now + 1000) / 1e9, [published['carState'],
+          selfdriven['aolAxisState'], selfdriven['selfdriveState'], selfdriven['onroadEvents'], receipt.as_reader()])
+        command, lateral = controls.state_control()
+        controls.publish(command, lateral)
+        current = messaging.new_message('carControl', valid=True, logMonoTime=now)
+        current.carControl = command
+        card.sm.update_msgs((now + 1000) / 1e9, [current.as_reader(), selfdriven['aolAxisState'], selfdriven['selfdriveState'], receipt.as_reader()])
+        sent = {}
+        with patch('openpilot.selfdrive.car.card.time.monotonic', return_value=(now + 1000) / 1e9), \
+             patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now + 1000), \
+             patch.object(card.pm, 'send', side_effect=lambda service, msg, sent=sent: sent.update({service:
+               messaging.log_from_bytes(msg) if isinstance(msg, bytes) else msg.as_reader()})):
+          card.controls_update(cs, command.as_reader())
+        outgoing = [(item.address, bytes(item.dat), item.src) for item in sent['sendcan'].sendcan] if 'sendcan' in sent else []
+        enabled_pedal = False
+        for address, data, source in outgoing:
+          assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, source, data)), (fault, tick, hex(address), data.hex())
+          if address == 0x551:
+            assert source == bus and len(data) == 6
+            enabled_pedal |= bool(data[4] & 128)
+        assert not command.longActive or bool(mask & 2)
+        assert not enabled_pedal or command.longActive
+        if bad and fault in ('gas', 'brake', 'reverse', 'feedback_state', 'counter', 'channels', 'eps', 'door', 'belt') and tick >= 122:
+          assert not enabled_pedal
+        if bad and fault == 'eps_temporary' and tick >= 122:
+          assert not command.latActive, 'temporary EPS inhibition must suppress lateral commands'
+        observed[tick] = (bool(command.longActive), enabled_pedal, bool(cs.canValid), bool(mask & 2))
+        if tick in (64, 65, 66, 74, 75, 76, 95, 100, 110, 160, 250):
+          diagnostics[tick] = (cs.cruiseState.enabled, ci.CS.preap.enableLongControl, card.aol_card_intent.allowed_latch, sd.enabled,
+                               str(wanted), sd.events.names, mask, observed[tick], command.actuators.accel)
+        previous = command
+      assert any(observed[t][0] and observed[t][1] for t in range(95, 115)), ('actual initial pedal engagement missing', diagnostics)
+      assert 'teslaPreapPedal/engaged/warning' in alerts_seen
+      assert 'teslaPreapPedal/disengaged/warning' in alerts_seen
+      assert not any(name.startswith('teslaStockCruise/') for name in alerts_seen)
+      if fault not in ('gas', 'eps_temporary'):
+        assert not any(observed[t][0] or observed[t][1] for t in range(200, 219)), 'fault/gear/brake recovery must not auto-resume long'
+      else:
+        assert any(observed[t][0] and observed[t][1] for t in range(160, 175)), (
+          'healthy override/temporary-pause recovery did not preserve existing active long')
+      assert any(observed[t][0] and observed[t][1] for t in range(250, 270)), 'fresh physical reengagement missing'
+      assert not any(observed[t][0] or observed[t][1] for t in range(278, 310)), 'physical cancel must withdraw pedal long'
+    finally:
+      safety.set_aol_test_heartbeat(False)
+      safety.set_safety_hooks(0, 0)
+      del sd
+      gc.collect()

openpilot/starpilot/car/tesla/tests/test_pedal_planner.py

@@ -0,0 +1,55 @@
+"""Actual selected pedal factory reaches current LongControl and adaptive MPC."""
+import pytest
+from opendbc.car import structs
+from opendbc.car.interfaces import ACCEL_MAX
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.starpilot.car.tesla.tests.pedal_fixture import actual_pedal_startup
+from openpilot.starpilot.car.tesla.tests.test_preap_adaptive import frame
+from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import DRIVE_NS, SPEED
+from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import message_bytes
+
+
+@pytest.mark.parametrize('bus', [0, 2])
+@pytest.mark.parametrize('radar_mode', ['off', 'front', 'nosecone'])
+def test_actual_selected_startup_uses_original_integral_tune_in_current_long_control(bus, radar_mode, monkeypatch):
+  with actual_pedal_startup(bus, radar_mode, False, monkeypatch) as (card, controls, calibration, cp_bytes):
+    loc = controls.LoC
+    assert loc.CP.to_dict() == card.CP.to_dict()
+    assert loc.extension is None
+    assert loc.stopping_decel_rate == 1.0
+    for speed, gain in ((0., .05), (3., .08), (6., .1), (35., .15)):
+      loc.pid.speed = speed
+      assert loc.pid.k_p == 0.0
+      assert loc.pid.k_i == pytest.approx(gain)
+    state = structs.CarState(vEgo=35., aEgo=0., gearShifter='drive', canValid=True)
+    output = loc.update(True, state, .2, False, (-1.5, 1.))
+    assert loc.long_control_state == structs.CarControl.Actuators.LongControlState.pid
+    assert loc.pid.i == pytest.approx(.0003)
+    assert output == pytest.approx(.2003)
+    assert loc.update(False, state, .2, False, (-1.5, 1.)) == 0.0
+    assert loc.pid.i == 0.0
+
+
+@pytest.mark.parametrize('bus', [0, 2])
+@pytest.mark.parametrize('radar_mode', ['off', 'front', 'nosecone'])
+def test_actual_selected_startup_reaches_adaptive_planner_and_current_mpc(bus, radar_mode, monkeypatch):
+  with actual_pedal_startup(bus, radar_mode, False, monkeypatch) as (card, controls, calibration, cp_bytes):
+    params = Params()
+    params.put_bool('NAPAdaptiveAccel', False, block=True)
+    original = LongitudinalPlanner(card.CP, init_v=SPEED)
+    params.put_bool('NAPAdaptiveAccel', True, block=True)
+    selected = LongitudinalPlanner(card.CP, init_v=SPEED)
+    assert selected.preap_adaptive.enabled and not original.preap_adaptive.enabled
+    params.put_bool('NAPAdaptiveAccel', False, block=True)
+    for index in range(20):
+      sm = frame(index)
+      before = message_bytes(sm.envelopes)
+      original.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      selected.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      assert before == message_bytes(sm.envelopes)
+      assert selected.mpc.solution_status == original.mpc.solution_status == 0
+      assert all(float(value) < .8 for value in selected.mpc.params[:, 1])
+      assert all(float(value) == pytest.approx(ACCEL_MAX) for value in original.mpc.params[:, 1])
+      assert selected.output_a_target <= max(float(value) for value in selected.mpc.params[:, 1])
+      assert selected.output_a_target <= original.output_a_target + 1e-5

openpilot/starpilot/car/tesla/tests/test_pedal_schema_evolution.py

@@ -0,0 +1,70 @@
+"""Compiled incumbent IDs/layout and wire regressions for the nested startup type."""
+from importlib.resources import files
+from pathlib import Path
+import re
+
+import capnp
+import pytest
+from openpilot.cereal import custom, CEREAL_PATH
+from openpilot.starpilot.car.tesla.tests.schema_fixture import layout
+from openpilot.starpilot.car.tesla.tests.test_radar_schema_evolution import populated, assert_occupied_equal
+
+FIXTURE = Path(__file__).with_name('fixtures') / 'custom_before_preap_pedal.capnp'
+
+
+@pytest.fixture
+def historical():
+  parser = capnp.SchemaParser()
+  yield parser.load(str(FIXTURE), imports=[CEREAL_PATH, str(files('opendbc') / 'car')])
+
+
+def compare_node(old, new):
+  assert old.schema.node.id == new.schema.node.id
+  kind = old.schema.node.which()
+  assert kind == new.schema.node.which()
+  if kind == 'struct':
+    assert layout(old.schema) == layout(new.schema)
+  elif kind == 'enum':
+    assert old.schema.node.enum.to_dict() == new.schema.node.enum.to_dict()
+  for child in old.schema.node.nestedNodes:
+    compare_node(getattr(old, child.name), getattr(new, child.name))
+
+
+def test_all_incumbent_custom_type_ids_fields_and_layouts_are_preserved(historical):
+  for name in re.findall(r'^(?:struct|enum) (\w+)', FIXTURE.read_text(), re.MULTILINE):
+    compare_node(getattr(historical, name), getattr(custom, name))
+  # Startup capsules have no parent slots; published phone GPS@3 stays occupied.
+  assert set(custom.StarPilotCarControl.schema.fields) == {
+    'hudControl', 'steeringLimitInfo', 'navigation', 'phoneGps'}
+  assert custom.StarPilotCarControl.PreapPedalStartup.schema.node.id not in {
+    getattr(historical, name).schema.node.id for name in
+    re.findall(r'^struct (\w+)', FIXTURE.read_text(), re.MULTILINE)}
+
+
+def test_standalone_startup_does_not_modify_parent_and_old_relay_preserves_navigation(historical):
+  parent = populated(custom.StarPilotCarControl)
+  original = parent.to_bytes()
+  capsule = custom.StarPilotCarControl.PreapPedalStartup.new_message(
+    version=1, kind=5, carParamsSha256=bytes(range(32)), safetyModel=39,
+    safetyParam=7, factor=1.25, zero=-0.5, pedalBus=2, sourceMonoTime=1234)
+  with custom.StarPilotCarControl.PreapPedalStartup.from_bytes(capsule.to_bytes()) as recovered:
+    assert recovered.to_dict() == capsule.to_dict()
+  parent.clear_write_flag()
+  assert parent.to_bytes() == original
+  with historical.StarPilotCarControl.from_bytes(original) as old:
+    assert_occupied_equal(parent, old)
+    relayed = old.as_builder().to_bytes()
+  with custom.StarPilotCarControl.from_bytes(relayed) as recovered:
+    assert_occupied_equal(parent, recovered)
+
+
+def test_old_gps_and_pedal_status_survive_current_reader_and_old_relay(historical):
+  old = historical.StarPilotCarState.new_message(pedalMaxRegen=True, pedalLongActive=True,
+    sourceCarStateMonoTime=123456, gps={'sourceMonoTime': 123456, 'latitude': 41.25,
+      'longitude': -87.5, 'altitude': 180., 'speed': 20., 'bearingDeg': 90., 'hasFix': True,
+      'unixTimestampMillis': 1700000000000, 'vNED': [1., 2., 3.]})
+  with custom.StarPilotCarState.from_bytes(old.to_bytes()) as current:
+    assert {name: current.to_dict()[name] for name in old.to_dict()} == old.to_dict()
+    relayed = current.as_builder().to_bytes()
+  with historical.StarPilotCarState.from_bytes(relayed) as recovered:
+    assert recovered.to_dict() == old.to_dict()

openpilot/starpilot/car/tesla/tests/test_preap_adaptive.py

@@ -0,0 +1,94 @@
+"""Real planner/MPC coverage of typed pedal-profile protocol; not pedal startup proof."""
+import pytest
+from opendbc.car import gen_empty_fingerprint
+from opendbc.car.interfaces import ACCEL_MAX
+from opendbc.car.tesla.interface import CarInterface
+from opendbc.car.tesla.values import CAR
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPlanner
+from openpilot.starpilot.car.tesla.preap_adaptive import PreapAdaptiveAccel, profile_key
+from openpilot.starpilot.longitudinal.tests.test_lead_approach_planner import Frame, DRIVE_NS, SPEED
+from openpilot.starpilot.longitudinal.tests.test_cruise_ceiling import snapshot, message_bytes
+
+
+def protocol_params():
+  # Existing factory remains stock; native pedal admission is a separate unfinished packet.
+  cp = CarInterface.get_params(CAR.TESLA_MODEL_S_PREAP, gen_empty_fingerprint(), [], False, False, False)
+  assert not cp.openpilotLongitudinalControl and cp.pcmCruise and cp.safetyConfigs[0].safetyParam == 0
+  cp.openpilotLongitudinalControl = True
+  cp.pcmCruise = False
+  cp.safetyConfigs[0].safetyParam = 1
+  return cp
+
+
+def frame(index):
+  sm = Frame(index).with_drive()
+  sm['carState'].gearShifter = 'drive'
+  return sm
+
+
+def test_real_planner_mpc_and_cruise_receive_cap_without_mutating_inputs():
+  with OpenpilotPrefix():
+    saved = Params()
+    cp = protocol_params()
+    saved.put_bool('NAPAdaptiveAccel', False, block=True)
+    original = LongitudinalPlanner(cp, init_v=SPEED)
+    saved.put_bool('NAPAdaptiveAccel', True, block=True)
+    selected = LongitudinalPlanner(cp, init_v=SPEED)
+    assert original.preap_adaptive is not None and not original.preap_adaptive.enabled
+    assert selected.preap_adaptive is not None and selected.preap_adaptive.enabled
+    # The original complete saved choice is frozen for each planner instance.
+    saved.put_bool('NAPAdaptiveAccel', False, block=True)
+    for index in range(20):
+      sm = frame(index)
+      before = message_bytes(sm.envelopes)
+      original.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      selected.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      assert message_bytes(sm.envelopes) == before
+      assert original.mpc.solution_status == selected.mpc.solution_status == 0
+      assert all(float(v) < 0.8 for v in selected.mpc.params[:, 1])
+      assert all(float(v) == pytest.approx(ACCEL_MAX) for v in original.mpc.params[:, 1])
+      assert selected.output_a_target <= max(float(v) for v in selected.mpc.params[:, 1])
+      assert selected.output_a_target <= original.output_a_target + 1e-5
+
+
+@pytest.mark.parametrize('defect', ['stale', 'future', 'old_drive', 'can', 'gas', 'brake', 'off', 'absent', 'changed_config'])
+def test_actual_planner_rejected_optional_input_matches_unmodified_path(defect):
+  with OpenpilotPrefix():
+    saved = Params()
+    cp = protocol_params()
+    saved.put_bool('NAPAdaptiveAccel', False, block=True)
+    original = LongitudinalPlanner(cp, init_v=SPEED)
+    saved.put_bool('NAPAdaptiveAccel', True, block=True)
+    selected = LongitudinalPlanner(cp, init_v=SPEED)
+    for index in range(4):
+      sm = frame(index)
+      if defect == 'stale':
+        sm.logMonoTime['radarState'] = sm.now_ns - 250_000_001
+      elif defect == 'future':
+        sm.logMonoTime['radarState'] = sm.now_ns + 1
+      elif defect == 'old_drive':
+        sm.logMonoTime['carState'] = DRIVE_NS - 1
+      elif defect == 'can':
+        sm['carState'].canValid = False
+      elif defect in ('gas', 'brake'):
+        setattr(sm['carState'], defect + 'Pressed', True)
+      elif defect == 'off':
+        sm['carControl'].longActive = False
+      elif defect == 'absent':
+        sm['radarState'].leadOne.present = False
+      else:
+        # Retire only the optional owner key; solver CP stays unchanged.
+        selected.preap_adaptive.key = None
+      original.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      selected.update(sm, now_ns=sm.now_ns, drive_id=DRIVE_NS)
+      assert snapshot(original) == snapshot(selected)
+
+
+@pytest.mark.parametrize('identity', [CAR.TESLA_MODEL_S_PREAP, CAR.TESLA_MODEL_S_HW1, CAR.TESLA_MODEL_3,
+                                      CAR.TESLA_MODEL_Y, CAR.TESLA_MODEL_X])
+def test_real_current_factory_profiles_remain_outside_unfinished_pedal_path(identity):
+  cp = CarInterface.get_params(identity, gen_empty_fingerprint(), [], False, False, False)
+  assert profile_key(cp) is None
+  assert not PreapAdaptiveAccel(cp, True).enabled

openpilot/starpilot/car/tesla/tests/test_preap_intent.py

@@ -89,3 +89,38 @@ def test_unbuckled_fresh_pull_arms_without_stock_engagement():
   assert intent.allowed_latch
   intent.update(state(), now_ns=4)
   assert intent.allowed_latch and not source.cruiseState.enabled
+
+
+
+def test_known_reverse_neutral_pause_and_explicit_cancel_keep_the_physical_contract():
+  intent = owner()
+  intent.update(state(), now_ns=1)
+  pull(intent, 2)
+  assert intent.allowed_latch
+  for tick, gear in enumerate((structs.CarState.GearShifter.neutral, structs.CarState.GearShifter.reverse,
+                                structs.CarState.GearShifter.neutral), 3):
+    paused = state(gearShifter=gear)
+    intent.update(paused, now_ns=tick)
+    assert intent.allowed_latch and not intent.output(paused)[0]
+  intent.update(state(), now_ns=6)
+  assert intent.allowed_latch and intent.output(state())[0]
+  paused = state(button=structs.CarState.ButtonEvent.Type.cancel, pressed=True,
+                 gearShifter=structs.CarState.GearShifter.reverse)
+  intent.update(paused, now_ns=7)
+  intent.update(state(), now_ns=8)
+  assert not intent.allowed_latch
+
+
+def test_reverse_pull_does_not_acquire_a_new_arm_and_park_still_disarms():
+  intent = owner()
+  intent.update(state(), now_ns=1)
+  paused = state(button=structs.CarState.ButtonEvent.Type.setCruise, pressed=True,
+                 gearShifter=structs.CarState.GearShifter.reverse)
+  intent.update(paused, now_ns=2)
+  assert not intent.allowed_latch
+  intent.update(state(button=structs.CarState.ButtonEvent.Type.setCruise), now_ns=3)
+  pull(intent, 4)
+  assert intent.allowed_latch
+  intent.update(state(gearShifter=structs.CarState.GearShifter.park), now_ns=5)
+  intent.update(state(), now_ns=6)
+  assert not intent.allowed_latch

openpilot/starpilot/car/tesla/tests/test_preap_pause_callers.py

@@ -0,0 +1,135 @@
+"""Physical CAN through actual preAP startup, Card, SD, Controls, CI and native TX."""
+import gc
+from unittest.mock import patch
+
+import pytest
+
+from opendbc.car import structs
+from opendbc.car.tesla.tests.preap_stream import PreAPStream
+from opendbc.car.tesla.tests.test_preap_startup import exercise_startup
+from opendbc.car.tesla.values import CAR
+from opendbc.safety.tests.libsafety import libsafety_py
+from openpilot.cereal import messaging
+from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD
+from openpilot.starpilot.aol.wire import SafetyState, PandaSlot, encode_safety
+from openpilot.starpilot.lateral.tests.test_lane_runtime import feed
+from openpilot.starpilot.tests.test_tesla_screen_integration import selfdrive_step
+
+
+@pytest.mark.parametrize('pause', ('reverse', 'neutral', 'neutral_reverse_neutral', 'temporary'))
+def test_preap_actual_physical_arm_pause_return_and_cancel(pause, monkeypatch):
+  def probe(card, ci, controls, saved):
+    safety = libsafety_py.libsafety
+    safety.init_tests()
+    safety.set_alternative_experience(32)
+    assert safety.set_safety_hooks(39, 0) == 0
+    with patch('openpilot.selfdrive.selfdrived.selfdrived.REPLAY', False):
+      sd = SelfdriveD(CP=card.CP)
+    stream = PreAPStream()
+    previous_command = structs.CarControl()
+    previous_receipt = None
+    selfdriven = {}
+    observed = {}
+    try:
+      for tick in range(210):
+        now = 2_000_000_000 + tick * 10_000_000
+        paused = 110 <= tick < 140
+        lever = 2 if tick == 65 else 1 if tick == 175 else 0
+        gear = 4
+        if paused:
+          if pause == 'reverse':
+            gear = 2
+          elif pause == 'neutral':
+            gear = 3
+          elif pause == 'neutral_reverse_neutral':
+            gear = 3 if tick < 115 or tick >= 135 else 2
+        incoming = stream.frames(tick, lever=lever, cruise=1,
+                                 gear=gear,
+                                 eps_status=0 if paused and pause == 'temporary' else 1)
+        safety.set_timer(now // 1000)
+        safety.set_aol_test_heartbeat(bool(card.aol_card_intent.allowed_latch or sd.enabled))
+        for address, payload, bus in incoming:
+          assert safety.safety_rx_hook(libsafety_py.make_CANPacket(address, bus, payload))
+        safety.safety_tick()
+        peers = []
+        cc = messaging.new_message('carControl', valid=True, logMonoTime=now)
+        cc.carControl = previous_command
+        peers.append(cc.as_reader())
+        peers.append(selfdriven['onroadEvents'] if selfdriven else
+                     messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=now).as_reader())
+        panda = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=now)
+        panda.pandaStates[0].safetyModel = card.CP.safetyConfigs[0].safetyModel
+        panda.pandaStates[0].safetyParam = 0
+        panda.pandaStates[0].alternativeExperience = 32
+        panda.pandaStates[0].controlsAllowed = bool(safety.get_controls_allowed())
+        panda.pandaStates[0].safetyRxChecksInvalid = not bool(safety.safety_config_valid())
+        peers.append(panda.as_reader())
+        if previous_receipt is not None:
+          peers.append(previous_receipt.as_reader())
+        if selfdriven:
+          peers.extend(selfdriven[key] for key in ('aolAxisState', 'selfdriveState'))
+        card.sm.update_msgs(now / 1e9, peers)
+        card.CC_prev = previous_command
+        with patch('openpilot.selfdrive.car.card.messaging.drain_sock_raw', return_value=[b'can']), \
+             patch('openpilot.selfdrive.car.card.can_capnp_to_list', return_value=[(now, incoming)]), \
+             patch('openpilot.selfdrive.car.card.time.monotonic_ns', return_value=now), \
+             patch('openpilot.selfdrive.car.card.time.clock_gettime_ns', return_value=now), \
+             patch('openpilot.selfdrive.car.card.REPLAY', False), patch.object(card.sm, 'update'):
+          cs, radar = card.state_update()
+        published = {}
+        factory = messaging.new_message
+
+        def stamped(service, size=None, factory=factory, now=now, **kwargs):
+          return factory(service, size, **({'logMonoTime': now} | kwargs))
+
+        with patch('openpilot.selfdrive.car.card.messaging.new_message', side_effect=stamped), \
+             patch.object(card.pm, 'send', side_effect=lambda service, msg, published=published: published.update({service: msg.as_reader()})):
+          card.state_publish(cs, radar)
+        published['pandaStates'] = panda.as_reader()
+        mask = safety.aol_get_permission_mask()
+        native = SafetyState(1, True, now, now + 30_000_000, 39, 0, bool(mask & 1), bool(mask & 2),
+                             bool(safety.aol_get_request_mask() & 1), False, 'tesla-preap-panda', sd.aol_session_id, now,
+                             (PandaSlot(0, 'tesla-preap-panda', 39, 0, 32, bool(safety.get_controls_allowed()),
+                                        not bool(safety.safety_config_valid())),))
+        receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
+        receipt.aolSafetyWire = encode_safety(native)
+        selfdriven.update(selfdrive_step(sd, published, receipt, previous_command, card.CP, safety, now))
+        wanted = sd.aol_axis_decision
+        safety.set_aol_test_heartbeat(bool(card.aol_card_intent.allowed_latch or sd.enabled))
+        safety.aol_set_host_request(int(wanted.desired_lateral) | (int(wanted.desired_longitudinal) << 1))
+        mask = safety.aol_get_permission_mask()
+        native = SafetyState(1, True, now, now + 30_000_000, 39, 0, bool(mask & 1), bool(mask & 2),
+                             wanted.desired_lateral, wanted.desired_longitudinal, 'tesla-preap-panda', sd.aol_session_id, now,
+                             (PandaSlot(0, 'tesla-preap-panda', 39, 0, 32, bool(safety.get_controls_allowed()),
+                                        not bool(safety.safety_config_valid())),))
+        receipt = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=now)
+        receipt.aolSafetyWire = encode_safety(native)
+        feed(controls, now, tick, active=bool(cs.cruiseState.enabled), enabled=bool(cs.cruiseState.enabled))
+        controls.sm.update_msgs((now + 1000) / 1e9, [published['carState'], selfdriven['aolAxisState'],
+          selfdriven['selfdriveState'], selfdriven['onroadEvents'], receipt.as_reader()])
+        command, lateral_log = controls.state_control()
+        controls.publish(command, lateral_log)
+        assert not command.longActive
+        assert command.latActive == wanted.lateral_active
+        _, outgoing = ci.apply(command.as_reader(), now)
+        for address, payload, bus in outgoing:
+          assert safety.safety_tx_hook(libsafety_py.make_CANPacket(address, bus, payload)), (tick, hex(address))
+        observed[tick] = (bool(command.latActive), bool(card.aol_card_intent.allowed_latch), mask,
+                          bool(cs.canValid), bool(safety.get_controls_allowed()))
+        previous_command, previous_receipt = command, receipt
+      assert all(row[3] for tick, row in observed.items() if tick >= 60)
+      assert not observed[60][0]
+      assert observed[90][0] and observed[90][1]
+      for tick in range(112, 140):
+        assert not observed[tick][0] and observed[tick][1] and observed[tick][2] == 0
+      assert observed[155][0] and observed[155][1]
+      if pause in ('reverse', 'neutral', 'neutral_reverse_neutral'):
+        assert not observed[155][4], 'returning Drive must not restore ordinary controlsAllowed'
+      assert not any(observed[tick][0] or observed[tick][1] for tick in range(177, 210))
+    finally:
+      safety.init_tests()
+      safety.set_alternative_experience(0)
+      safety.set_safety_hooks(structs.CarParams.SafetyModel.noOutput, 0)
+      del sd
+      gc.collect()
+  exercise_startup(CAR.TESLA_MODEL_S_PREAP, False, False, False, monkeypatch, probe=probe)

openpilot/starpilot/car/tesla/tests/test_radar_context.py

@@ -0,0 +1,82 @@
+from dataclasses import replace
+import hashlib
+import math
+
+import pytest
+
+from opendbc.car import structs
+from opendbc.car.tesla.preap.radar_profile import RadarStartupContext
+from opendbc.car.tesla.tests.test_preap_radar import radar_params
+from opendbc.car.tesla.radar_interface import RadarInterface
+from openpilot.starpilot.car.tesla.radar_context import SCHEMA, encode_context, decode_context, install_startup_context
+from openpilot.starpilot.car.tesla.radar_settings import RadarSettings
+
+
+def context(cp, offset=0.25):
+  raw = cp.to_bytes()
+  return raw, RadarStartupContext(1, int(cp.safetyConfigs[0].safetyParam), hashlib.sha256(raw).digest(), offset)
+
+
+@pytest.mark.parametrize('mount', (False, True))
+def test_final_serialized_cp_context_and_reader_lifetime(mount):
+  cp = radar_params(mount=mount)
+  raw, frozen = context(cp)
+  wire = encode_context(cp, raw, frozen)
+  with structs.CarParams.from_bytes(raw) as published:
+    decoded = decode_context(published, raw, wire)
+    ri = RadarInterface(published)
+    ri.configure_preap_radar(decoded, raw)
+  assert decoded == frozen and ri.preap.radar_offset == 0.25
+  assert not ri.preap.radar_off_can
+
+
+@pytest.mark.parametrize('field,value', (('version', 2), ('kind', 3), ('safetyModel', 10), ('safetyParam', 6),
+                                        ('carParamsSha256', bytes(32)), ('lateralOffset', math.nan),
+                                        ('lateralOffset', math.inf)))
+def test_unknown_or_mismatched_wire_cannot_configure_decoder(field, value):
+  cp = radar_params()
+  raw, frozen = context(cp)
+  message = SCHEMA.new_message(version=1, kind=4, safetyModel=39, safetyParam=2,
+                               carParamsSha256=frozen.car_params_sha256, lateralOffset=frozen.offset)
+  setattr(message, field, value)
+  assert decode_context(cp, raw, message.to_bytes()) is None
+
+
+def test_missing_truncated_multisegment_or_mutated_cp_context_denies():
+  cp = radar_params()
+  raw, frozen = context(cp)
+  wire = encode_context(cp, raw, frozen)
+  for damaged in (None, b'', wire[:-8], wire + bytes(8), bytes([1]) + wire[1:], memoryview(wire)):
+    assert decode_context(cp, raw, damaged) is None
+  cp.carVin = '1' * 17
+  assert decode_context(cp, raw, wire) is None
+  with pytest.raises(ValueError):
+    RadarInterface(cp).configure_preap_radar(frozen, raw)
+
+
+def test_missing_context_reports_errors_and_never_decodes_tracks():
+  ri = RadarInterface(radar_params())
+  results = [ri.update([]) for _ in range(5)]
+  assert results[-1] is not None and not results[-1].points
+  assert results[-1].errors.canError and results[-1].errors.radarUnavailableTemporary
+
+
+def test_immutable_startup_settings_install_and_mismatched_mount_denial():
+  cp = radar_params()
+  raw, _ = context(cp)
+  ri = RadarInterface(cp)
+  settings = RadarSettings(True, False, 0.25, True)
+  frozen = install_startup_context(ri, cp, raw, settings)
+  assert frozen.offset == ri.preap.radar_offset == 0.25
+  with pytest.raises(ValueError):
+    install_startup_context(ri, cp, raw, replace(settings, behind_nosecone=True))
+  assert ri.preap.radar_offset == 0.25
+
+
+def test_radar_native_receipt_requires_same_cp_namespace_and_word():
+  from openpilot.starpilot.car.tesla.aol import native_accepts_cp
+  cp = radar_params()
+  cp.alternativeExperience = 32
+  assert native_accepts_cp(cp, 39, 2)
+  for model, word in ((39, 0), (39, 6), (10, 512), (10, 1536), (35, 2)):
+    assert not native_accepts_cp(cp, model, word)

openpilot/starpilot/car/tesla/tests/test_radar_pipeline.py

@@ -0,0 +1,84 @@
+from opendbc.can import CANPacker
+from opendbc.car import structs
+from opendbc.car.tesla.tests.test_preap_radar import frames, radar_params, radar_interface
+from openpilot.cereal import messaging
+from openpilot.selfdrive.controls.radard import RadarD
+
+
+def source_tick(sm, tick, rr=None, speed=0.0, probability=0.0, distance=10.0, standstill=False, stamp=None, radar_event=None):
+  stamp = 1_000_000_000 + tick * 50_000_000 if stamp is None else stamp
+  model_event = messaging.new_message('modelV2', valid=True)
+  model_event.logMonoTime = stamp
+  model = model_event.modelV2
+  model.velocity.x = [speed]
+  leads = model.init('leadsV3', 2)
+  for lead in leads:
+    lead.x = [distance + 1.52]
+    lead.y = [0.0]
+    lead.v = [speed]
+    lead.a = [0.0]
+    lead.xStd = lead.yStd = lead.vStd = [1.0]
+    lead.prob = probability
+  car_event = messaging.new_message('carState', valid=True)
+  car_event.logMonoTime = stamp
+  car_event.carState.vEgo = speed
+  car_event.carState.standstill = standstill
+  events = [model_event.as_reader(), car_event.as_reader()]
+  if radar_event is not None:
+    events.append(radar_event.as_reader())
+  elif rr is not None:
+    radar_event = messaging.new_message('radarTracks', valid=True)
+    radar_event.logMonoTime = stamp
+    radar_event.radarTracks = rr
+    events.append(radar_event.as_reader())
+  sm.update_msgs(stamp / 1e9, events)
+
+
+def test_actual_preap_decoder_to_radard_tracks_errors_retirement_and_recovery():
+  cp = radar_params(0.25)
+  decoder, packer = radar_interface(cp, 0.25), CANPacker("tesla_radar_bosch_generated")
+  radar = RadarD(cp.radarDelay)
+  sm = messaging.SubMaster(["modelV2", "carState", "radarTracks"])
+  decoder.update([])
+  last_cycle = 0
+  identifier = None
+  for tick in range(1, 31):
+    now = 1_000_000_000 + tick * 50_000_000
+    cycle = (now - 1_000_000_000) // 125_000_000
+    rr = None
+    if cycle > last_cycle:
+      radar_time = 1_000_000_000 + cycle * 125_000_000
+      rr = decoder.update([(radar_time, frames(packer, measured=False))])
+      last_cycle = cycle
+    if rr is not None:
+      with structs.RadarData.from_bytes(rr.to_bytes()) as decoded:
+        source_tick(sm, tick, decoded, speed=10.0, probability=1.0, distance=30.0)
+    else:
+      source_tick(sm, tick, speed=10.0, probability=1.0, distance=30.0)
+    radar.update(sm, sm["radarTracks"])
+    if radar.tracks:
+      identifier = next(iter(radar.tracks))
+  assert identifier is not None
+  count = radar.tracks[identifier].cnt
+  assert radar.kalman_params.A[0][1] == 0.05
+  source_tick(sm, 31, speed=10.0, probability=1.0, distance=30.0)
+  radar.update(sm, sm["radarTracks"])
+  assert radar.tracks[identifier].cnt == count + 1
+  assert radar.radar_state.leadOne.present and radar.radar_state.leadOne.radar
+  trigger = packer.make_can_msg("RadarPoint31_B", 1, {"Index2": 0})
+  for step in range(5):
+    rr = decoder.update([(4_000_000_000 + step * 125_000_000, [trigger])])
+  with structs.RadarData.from_bytes(rr.to_bytes()) as decoded:
+    source_tick(sm, 70, decoded)
+  radar.update(sm, sm["radarTracks"])
+  assert radar.radar_state.radarErrors.canError
+  rr = decoder.update([(4_625_000_000, frames(packer, fault=True, index=1))])
+  with structs.RadarData.from_bytes(rr.to_bytes()) as decoded:
+    source_tick(sm, 73, decoded)
+  radar.update(sm, sm["radarTracks"])
+  assert radar.radar_state.radarErrors.radarFault and not radar.tracks
+  rr = decoder.update([(4_750_000_000, frames(packer))])
+  with structs.RadarData.from_bytes(rr.to_bytes()) as decoded:
+    source_tick(sm, 75, decoded)
+  radar.update(sm, sm["radarTracks"])
+  assert len(radar.tracks) == 1 and identifier not in radar.tracks

openpilot/starpilot/car/tesla/tests/test_radar_schema_evolution.py

@@ -0,0 +1,152 @@
+"""Compiled old/new occupied-field wire checks for optional preAP radar context."""
+import capnp
+import pytest
+
+from openpilot.cereal import custom
+from openpilot.starpilot.car.tesla.tests.schema_fixture import layout
+
+HISTORICAL_SCHEMA = '''@0xb526ba661d550a59;
+struct StarPilotCarControl @0x81c2f05a394cf4af {
+  hudControl @0 :HUDControl;
+  steeringLimitInfo @1 :SteeringLimitInfo;
+
+  struct HUDControl {
+    audibleAlert @0 :AudibleAlert;
+
+    enum AudibleAlert {
+      none @0;
+
+      engage @1;
+      disengage @2;
+      refuse @3;
+
+      warningSoft @4;
+      warningImmediate @5;
+
+      prompt @6;
+      promptRepeat @7;
+      promptDistracted @8;
+
+      # Random Events
+      angry @9;
+      continued @10;
+      dejaVu @11;
+      doc @12;
+      fart @13;
+      firefox @14;
+      goat @15;
+      hal9000 @16;
+      mail @17;
+      nessie @18;
+      noice @19;
+      startup @20;
+      thisIsFine @21;
+      uwu @22;
+    }
+  }
+
+  struct SteeringLimitInfo {
+    valid @0 :Bool;
+    modelLimitErrorDeg @1 :Float32;
+    resumeLimitErrorDeg @2 :Float32;
+    cooperativeLimitErrorDeg @3 :Float32;
+    cooperativeOffsetDeg @4 :Float32;
+    monoTime @5 :UInt64;
+    combinedLimitErrorDeg @6 :Float32;
+  }
+
+  navigation @2 :Navigation;
+
+  struct Navigation {
+  sessionId @0 :Text;
+    frameMonoTime @1 :UInt64;
+    startedMonoTime @2 :UInt64;
+    revision @3 :Text;
+    enabled @4 :Bool;
+    status @5 :Text;
+    destinationName @6 :Text;
+    instruction @7 :Instruction;
+    route @8 :List(Coordinate);
+    nextManeuver @9 :Instruction;
+    locationMonoTime @10 :UInt64;
+    controlValid @11 :Bool;
+
+    struct Coordinate {
+      latitude @0 :Float64;
+      longitude @1 :Float64;
+    }
+    struct Instruction {
+      text @0 :Text;
+      maneuverType @1 :Text;
+      maneuverModifier @2 :Text;
+      distanceMeters @3 :Float32;
+      remainingDistanceMeters @4 :Float32;
+      remainingDurationSeconds @5 :Float32;
+    }
+    version @12 :UInt16;
+  }
+}
+'''
+
+
+@pytest.fixture
+def historical(tmp_path):
+  path = tmp_path / 'tesla-occupied-fields.capnp'
+  path.write_text(HISTORICAL_SCHEMA)
+  parser = capnp.SchemaParser()
+  yield parser.load(str(path)).StarPilotCarControl
+
+
+def populated(schema):
+  return schema.new_message(
+    hudControl={'audibleAlert': 'warningImmediate'},
+    steeringLimitInfo={'valid': True, 'modelLimitErrorDeg': 1.25, 'resumeLimitErrorDeg': 2.5,
+                       'cooperativeLimitErrorDeg': 3.75, 'cooperativeOffsetDeg': -0.5,
+                       'monoTime': 123456789, 'combinedLimitErrorDeg': 4.0},
+    navigation={'sessionId': 'tesla-old-context', 'frameMonoTime': 1234, 'startedMonoTime': 1000,
+                'revision': 'v1', 'enabled': True, 'status': 'active', 'destinationName': 'Destination',
+                'instruction': {'text': 'Continue', 'maneuverType': 'turn', 'maneuverModifier': 'left',
+                                'distanceMeters': 12.5, 'remainingDistanceMeters': 30.0,
+                                'remainingDurationSeconds': 45.0},
+                'route': [{'latitude': 41.25, 'longitude': -87.5}],
+                'nextManeuver': {'text': 'Then right', 'distanceMeters': 25.0},
+                'locationMonoTime': 1100, 'controlValid': True, 'version': 1})
+
+
+def assert_occupied_equal(old, new):
+  for field in ('hudControl', 'steeringLimitInfo', 'navigation'):
+    assert getattr(old, field).to_dict() == getattr(new, field).to_dict()
+
+
+def test_historical_occupied_fields_decode_under_current_schema(historical):
+  old = populated(historical)
+  with custom.StarPilotCarControl.from_bytes(old.to_bytes()) as current:
+    assert_occupied_equal(old, current)
+    assert current.phoneGps.sourceMonoTime == 0
+
+
+def test_current_extension_preserves_old_values_through_old_reader_and_relay(historical):
+  current = populated(custom.StarPilotCarControl)
+  current.phoneGps = {'sourceMonoTime': 123456, 'latitude': 41.25, 'longitude': -87.5, 'hasFix': True}
+  original = current.to_bytes()
+  startup = custom.StarPilotCarControl.PreapRadarStartup.new_message(
+    version=1, kind=4, carParamsSha256=bytes(range(32)), safetyModel=39, safetyParam=6, lateralOffset=0.25)
+  with custom.StarPilotCarControl.PreapRadarStartup.from_bytes(startup.to_bytes()) as decoded:
+    assert decoded.to_dict() == startup.to_dict()
+  current.clear_write_flag()
+  assert current.to_bytes() == original
+  with historical.from_bytes(original) as old:
+    assert_occupied_equal(old, current)
+    relayed = old.as_builder().to_bytes()
+  with custom.StarPilotCarControl.from_bytes(relayed) as recovered:
+    assert_occupied_equal(current, recovered)
+    assert recovered.phoneGps.to_dict() == current.phoneGps.to_dict()
+
+
+def test_occupied_field_slots_and_types_remain_identical(historical):
+  current = custom.StarPilotCarControl
+  for name in ('hudControl', 'steeringLimitInfo', 'navigation'):
+    assert historical.schema.fields[name].proto.as_builder().to_bytes() == current.schema.fields[name].proto.as_builder().to_bytes()
+  for nested in ('HUDControl', 'SteeringLimitInfo', 'Navigation'):
+    old, new = getattr(historical, nested), getattr(current, nested)
+    assert layout(old.schema) == layout(new.schema)

openpilot/starpilot/car/tesla/tests/test_radar_settings.py

@@ -0,0 +1,98 @@
+import pytest
+
+from openpilot.starpilot.car.tesla.radar_settings import read_radar_settings
+
+
+@pytest.mark.parametrize("raw,valid", ((b"0", True), (b"-0.25", True), (b"nan", False), (b"inf", False),
+                                    (b"1e300", False), (b"malformed", False)))
+def test_offset_is_finite_and_representable_in_startup_context(tmp_path, raw, valid):
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  (tmp_path / "NAPRadarEnabled").write_bytes(b"1")
+  (tmp_path / "NAPRadarOffset").write_bytes(raw)
+  settings = read_radar_settings(Saved())
+  assert settings.valid == valid
+
+
+def test_nosecone_cannot_select_radar_without_radar_enabled(tmp_path):
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  (tmp_path / "NAPRadarBehindNosecone").write_bytes(b"1")
+  assert not read_radar_settings(Saved()).valid
+
+
+@pytest.mark.parametrize("raw", (b"nan", b"1e300", b"malformed"))
+def test_disabled_stock_ignores_irrelevant_offset(tmp_path, raw):
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  (tmp_path / "NAPRadarOffset").write_bytes(raw)
+  settings = read_radar_settings(Saved())
+  assert settings.valid and not settings.enabled and settings.offset == 0.0
+
+
+def test_invalid_snapshot_cannot_fall_back_to_stock_even_if_admitted_flag_is_true():
+  from opendbc.car import structs
+  from opendbc.car.tesla.tests.test_preap_stock import params
+  from openpilot.starpilot.car.tesla.preap_preferences import prepare_stock
+  from openpilot.starpilot.car.tesla.radar_settings import RadarSettings
+
+  cp = params()
+  prepare_stock(cp, True, RadarSettings(False, False, 0.0, False))
+  assert cp.dashcamOnly and cp.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput
+
+
+def test_startup_reads_one_radar_snapshot(monkeypatch, tmp_path):
+  from openpilot.starpilot.vehicle_preferences import VehicleStartupPreferences
+  from openpilot.starpilot.car.tesla import radar_settings
+  from openpilot.starpilot.car.tesla.radar_settings import RadarSettings
+
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  calls = []
+
+  def read_once(params):
+    calls.append(params)
+    assert len(calls) == 1
+    return RadarSettings(False, False, 0.0, True)
+
+  monkeypatch.setattr(radar_settings, "read_radar_settings", read_once)
+  preferences = VehicleStartupPreferences.read(Saved(), enabled=False)
+  assert preferences.tesla_preap_stock and len(calls) == 1
+
+
+def test_canonical_document_is_authoritative_over_legacy_hardware_choices(tmp_path):
+  from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY, encode
+
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  (tmp_path / "NAPRadarEnabled").write_bytes(b"1")
+  (tmp_path / "NAPRadarBehindNosecone").write_bytes(b"1")
+  (tmp_path / DOCUMENT_KEY).write_bytes(encode("off", 0.25))
+  settings = read_radar_settings(Saved())
+  assert settings.valid and not settings.enabled and settings.offset == 0.25
+  (tmp_path / DOCUMENT_KEY).write_bytes(b"{invalid")
+  assert not read_radar_settings(Saved()).valid
+
+
+@pytest.mark.parametrize('raw', (b'{"version":1,"mode":"front","mode":"off","offset":0}',
+                                b'{"version":1,"mode":"front","offset":0,"extra":1}',
+                                b'{"version":true,"mode":"front","offset":0}'))
+def test_unknown_or_ambiguous_document_does_not_admit_radar(tmp_path, raw):
+  from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY
+
+  class Saved:
+    def get_param_path(self, name):
+      return str(tmp_path / name)
+
+  (tmp_path / DOCUMENT_KEY).write_bytes(raw)
+  assert not read_radar_settings(Saved()).valid

openpilot/starpilot/car/tesla/tests/test_radar_startup.py

@@ -0,0 +1,55 @@
+import gc
+import json
+
+import pytest
+
+from opendbc.car import car_helpers, structs
+from opendbc.car.tesla.tests.test_preap_startup import exercise_startup
+from opendbc.car.tesla.values import CAR
+from openpilot.cereal import messaging
+from openpilot.common.params import Params
+from openpilot.common.prefix import OpenpilotPrefix
+from openpilot.selfdrive.car import card as card_module
+from openpilot.selfdrive.car.card import Car
+from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY, encode
+from openpilot.starpilot.vehicle_selection import KEY as SELECTION_KEY, encode as encode_selection
+
+
+@pytest.mark.parametrize('mode', ('front', 'nosecone'))
+@pytest.mark.parametrize('release', (False, True))
+def test_auto_actual_factory_card_final_cp_and_frozen_context(mode, release, monkeypatch):
+  exercise_startup(CAR.TESLA_MODEL_S_PREAP, True, release, False, monkeypatch,
+                   radar_mode=mode, radar_offset=0.25)
+
+
+@pytest.mark.parametrize('mode', ('front', 'nosecone'))
+def test_manual_selection_remains_denied_at_actual_card_startup(mode, monkeypatch):
+  monkeypatch.setenv('SIMULATION', '1')
+  monkeypatch.setenv('REPLAY', '1')
+  monkeypatch.setenv('AOL_REPLAY_RUNTIME', '1')
+  monkeypatch.delenv('FINGERPRINT', raising=False)
+  with OpenpilotPrefix():
+    saved = Params()
+    saved.put_bool('OpenpilotEnabledToggle', True, block=True)
+    saved.put(SELECTION_KEY, json.loads(encode_selection(str(CAR.TESLA_MODEL_S_PREAP))), block=True)
+    saved.put(DOCUMENT_KEY, json.loads(encode(mode, 0.25)), block=True)
+    # The actual get_car function must apply the selection and mark it fixed.
+    monkeypatch.setattr(car_helpers, 'fingerprint', lambda *args: (None, {0: {}, 1: {}, 2: {}},
+                         '0' * 17, [], structs.CarParams.FingerprintSource.can, False))
+    monkeypatch.setattr(card_module, 'can_comm_callbacks', lambda *args: (lambda *args: [], lambda frames: None))
+    monkeypatch.setattr(card_module.messaging, 'recv_one_retry', lambda socket: messaging.new_message('can', 1))
+    card = None
+    try:
+      card = Car()
+      assert card.CP.carFingerprint == CAR.TESLA_MODEL_S_PREAP
+      assert card.CP.fingerprintSource == structs.CarParams.FingerprintSource.fixed
+      assert card.CP.dashcamOnly and card.CP.passive
+      assert len(card.CP.safetyConfigs) == 1
+      assert card.CP.safetyConfigs[0].safetyModel == structs.CarParams.SafetyModel.noOutput
+      assert not card.aol_qualified and card.tesla_preap_radar_context is None
+      assert card.RI.preap.radar_off_can
+      with structs.CarParams.from_bytes(saved.get('CarParams')) as published:
+        assert published.to_dict() == card.CP.to_dict()
+    finally:
+      del card
+      gc.collect()

openpilot/starpilot/tests/test_tesla_screen_integration.py

@@ -48,6 +48,9 @@ def selfdrive_step(sd, published, native, previous_command, cp, safety, now):
   for service in sd.sm.data:
     if service in ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire'):
       continue
+    if service == 'pandaStates' and service in published:
+      peers.append(published[service])
+      continue
     if service == 'aolIntentWire':
       peers.append(published[service])
       continue

openpilot/starpilot/ui/feature_settings_owner.py

@@ -53,6 +53,7 @@ from openpilot.starpilot.ui.wheel_feature import WheelFeature, PREFIX as WHEEL_P
 from openpilot.starpilot.longitudinal.output_max import KEY as OUTPUT_MAX_KEY
 from openpilot.starpilot.ui.tesla_screen_feature import TeslaScreenFeature, KEYS as TESLA_SCREEN_KEYS
 from openpilot.starpilot.ui.ford_lateral_feature import FordLateralFeature
+from openpilot.starpilot.ui.preap_radar_feature import PreAPRadarFeature, KEYS as PREAP_RADAR_KEYS, capability as preap_radar_capability
 from openpilot.starpilot.ui.gm_tune_feature import GmTuneFeature, GmTruckFeature, GmEvPresetFeature
 from openpilot.starpilot.car.gm.tune_preferences import KEY as GM_TUNE_KEY
 from openpilot.starpilot.car.gm.radar_recovery import KEY as RADAR_RECOVERY_KEY
@@ -152,6 +153,8 @@ class FeatureSettingsOwner:
     self.tesla_screen = TeslaScreenFeature(self)
     from openpilot.starpilot.ui.gm_cancel_remap_feature import GmCancelRemapFeature
     self.gm_cancel_remap = GmCancelRemapFeature(self)
+
+    self.preap_radar = PreAPRadarFeature(self)
     self.gm_tune = GmTuneFeature(self)
     self.gm_truck = GmTruckFeature(self)
     self.gm_ev_preset = GmEvPresetFeature(self)
@@ -715,7 +718,7 @@ class FeatureSettingsOwner:
           self._gm_stop_capability("GMAutoHold") is not None or self.tesla_screen.capability() is not None or
           self.gm_tune.capability() is not None or self.gm_truck.capability() is not None or
           self.gm_ev_preset.capability() is not None or self.volt_radar.capability() is not None or
-          self.panda_wake.capability() is not None):
+          self.panda_wake.capability() is not None or preap_radar_capability(self.vehicle_params()) is not None):
         rows.append(FeatureRow("", "Vehicle Settings", "Configure features supported by your vehicle", page=FeaturePage.VEHICLE, available=True))
       rows.extend(FeatureRow("", name.title() + " Personality", "", page=name, available=True)
                   for name in (*PROFILE_NAMES, "traffic"))
@@ -739,6 +742,8 @@ class FeatureSettingsOwner:
           capability=radar_capability, default_value="Off"))
       rows.extend(self.tesla_screen.rows(parked))
       rows.extend(self.gm_cancel_remap.rows(parked))
+
+      rows.extend(self.preap_radar.rows())
       rows.extend(self.gm_tune.rows(parked))
       rows.extend(self.gm_truck.rows(parked))
       rows.extend(self.gm_ev_preset.rows(parked))
@@ -1175,6 +1180,8 @@ class FeatureSettingsOwner:
       return self.gm_cancel_remap.apply(request)
     if key == "GMPedalLongitudinal":
       return self._apply_pedal_setup(request)
+    if key in PREAP_RADAR_KEYS:
+      return self.preap_radar.apply(request)
     if key in TESLA_SCREEN_KEYS:
       return self.tesla_screen.apply(request)
     if key == GM_TUNE_KEY:

openpilot/starpilot/ui/preap_radar_feature.py

@@ -0,0 +1,71 @@
+from opendbc.car import structs
+from opendbc.car.tesla.values import CAR
+from openpilot.starpilot.saved_document import commit_exact
+from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY, MAX_BYTES, encode, read_radar_settings
+from openpilot.starpilot.ui.feature_settings_state import FeatureRow
+
+KEYS = frozenset(("preap_radar:mode", "preap_radar:offset"))
+FLOAT32_MAX = 3.4028234663852886e38
+
+
+def capability(cp):
+  if cp is None or cp.brand != "tesla" or cp.carFingerprint != CAR.TESLA_MODEL_S_PREAP:
+    return None
+  if (cp.flags or cp.notCar or cp.transmissionType == structs.CarParams.TransmissionType.manual or
+      cp.fingerprintSource == structs.CarParams.FingerprintSource.fixed):
+    return None
+  configurations = tuple((str(s.safetyModel), int(s.safetyParam)) for s in cp.safetyConfigs)
+  if len(configurations) != 1 or configurations[0] not in (("teslaPreap", 0), ("teslaPreap", 2), ("teslaPreap", 6), ("noOutput", 0)):
+    return None
+  return (cp.carFingerprint, configurations, bool(cp.dashcamOnly), bool(cp.passive))
+
+
+class PreAPRadarFeature:
+  def __init__(self, owner):
+    self.owner = owner
+
+  def rows(self):
+    owner = self.owner
+    capable = capability(owner.vehicle_params())
+    if capable is None:
+      return []
+    saved = read_radar_settings(owner.params)
+    source = dict(saved.sources).get(DOCUMENT_KEY)
+    allowed = owner.authority("parked_preferences")
+    mode = "Behind nosecone" if saved.behind_nosecone else "Front" if saved.enabled else "Off"
+    return [FeatureRow("preap_radar:mode", "Hardware radar", mode if saved.valid else "Invalid saved configuration", source,
+                       choices=("Off", "Front", "Behind nosecone") if saved.valid else (),
+                       repair_value="" if saved.valid else "Off", available=allowed, capability=capable, vehicle_fingerprint=owner.vehicle_fingerprint(),
+                       dependencies=saved.sources, reason="Parked hardware setup. Applies after restart."),
+            FeatureRow("preap_radar:offset", "Radar lateral offset", str(saved.offset) if saved.valid else "Invalid saved configuration", source,
+                       step=0.05 if saved.valid else 0.0, minimum=-FLOAT32_MAX, maximum=FLOAT32_MAX, unit="m",
+                       repair_value="" if saved.valid else "0.0", available=allowed, capability=capable, vehicle_fingerprint=owner.vehicle_fingerprint(),
+                       dependencies=saved.sources, reason="Parked hardware setup. Applies after restart.")]
+
+  def apply(self, request):
+    if request.key not in KEYS or request.related_source is not None or request.direction or request.display_unit:
+      return False
+    row = next((row for row in self.rows() if row.key == request.key), None)
+    if row is None or not row.available or request.capability != row.capability or request.dependencies != row.dependencies or request.expected != row.source:
+      return False
+    saved = read_radar_settings(self.owner.params)
+    try:
+      if request.key == "preap_radar:mode":
+        if request.value not in ("Off", "Front", "Behind nosecone") or (not saved.valid and request.value != "Off"):
+          return False
+        mode = {"Off": "off", "Front": "front", "Behind nosecone": "nosecone"}[request.value]
+        raw = encode(mode, saved.offset if saved.valid else 0.0)
+      else:
+        if not saved.valid and request.value != "0.0":
+          return False
+        mode = "nosecone" if saved.behind_nosecone else "front" if saved.enabled else "off"
+        raw = encode(mode, request.value)
+    except (ValueError, TypeError, OverflowError):
+      return False
+    def authorized():
+      fresh = next((row for row in self.rows() if row.key == request.key), None)
+      return bool(fresh is not None and fresh.available and fresh.capability == request.capability and
+                  fresh.dependencies == request.dependencies and self.owner.vehicle_fingerprint() == request.vehicle_fingerprint)
+    result = commit_exact(self.owner.params, key=DOCUMENT_KEY, max_bytes=MAX_BYTES, raw=raw, expected=request.expected,
+                          authorized=authorized, temp_prefix=".preap-radar-")
+    return result.committed and result.verified

openpilot/starpilot/ui/tests/test_preap_radar_feature.py

@@ -0,0 +1,57 @@
+from opendbc.car.tesla.tests.test_preap_stock import params
+from openpilot.common.params import Params
+from openpilot.starpilot.car.tesla.radar_settings import DOCUMENT_KEY, read_radar_settings
+from openpilot.starpilot.galaxy.settings import AuthorityContext, SettingsGateway
+from openpilot.starpilot.ui.feature_settings_owner import FeatureSettingsOwner
+from openpilot.starpilot.ui.feature_settings_state import FeatureSettingsRequest
+
+
+def owner(saved, cp, parked=True):
+  return FeatureSettingsOwner(saved, lambda group: parked and group == "parked_preferences",
+    vehicle_fingerprint=lambda: cp.carFingerprint, vehicle_params=lambda: cp)
+
+
+def request(row, value):
+  return FeatureSettingsRequest(row.key, row.source, value, confirmation=True,
+    vehicle_fingerprint=row.vehicle_fingerprint, capability=row.capability, dependencies=row.dependencies)
+
+
+def test_shared_owner_changes_nosecone_to_off_atomically_and_preserves_legacy(tmp_path):
+  saved, cp = Params(str(tmp_path)), params()
+  saved.put("NAPRadarEnabled", True)
+  saved.put("NAPRadarBehindNosecone", True)
+  feature = owner(saved, cp)
+  row = next(r for r in feature.snapshot("vehicle", parked=True, system_long=False, lateral_context=False, metric=True).rows
+             if r.key == "preap_radar:mode")
+  assert row.value == "Behind nosecone" and row.available
+  assert feature.apply(request(row, "Off"))
+  assert read_radar_settings(saved).valid and not read_radar_settings(saved).enabled
+  assert saved.get_bool("NAPRadarEnabled") and saved.get_bool("NAPRadarBehindNosecone")
+  assert saved.get(DOCUMENT_KEY) is not None
+
+
+def test_onroad_and_changed_source_cannot_commit_hardware_choice(tmp_path):
+  saved, cp = Params(str(tmp_path)), params()
+  feature = owner(saved, cp)
+  row = feature.preap_radar.rows()[0]
+  change = request(row, "Front")
+  saved.put("NAPRadarEnabled", True)
+  assert not feature.apply(change)
+  row = owner(saved, cp, parked=False).preap_radar.rows()[0]
+  assert not row.available
+
+
+def test_galaxy_vehicle_page_uses_same_radar_setup_owner(tmp_path):
+  saved, cp = Params(str(tmp_path)), params()
+
+  class Context:
+    def sample(self):
+      return AuthorityContext(True, cp, b"preap-cp")
+
+  gateway = SettingsGateway(saved, Context(), clock=lambda: 10.0)
+  page = gateway.page("vehicle", "session", b"generation")
+  index = next(i for i, row in enumerate(page["rows"]) if row["label"] == "Hardware radar")
+  intent = gateway.preview(page["view"], index, 1, "session", b"generation")
+  assert gateway.confirm(intent["intent"], "session", b"generation")
+  settings = read_radar_settings(saved)
+  assert settings.valid and settings.enabled and not settings.behind_nosecone

openpilot/starpilot/vehicle_preferences.py

@@ -4,6 +4,8 @@ from opendbc.car.gm.startup_preferences import prepare_disable_longitudinal
 from opendbc.car.toyota.interface import apply_toyota_auto_hold
 from opendbc.car.toyota.values import ToyotaFlags
 from openpilot.starpilot.saved_source import read_saved
+from openpilot.starpilot.car.tesla.radar_settings import RadarSettings
+from openpilot.starpilot.car.tesla.pedal_preferences import PedalSettings
 
 
 def bolt_disable_supported(cp) -> bool:
@@ -22,6 +24,8 @@ class VehicleStartupPreferences:
   disable_bolt_long: bool = False
   honda_bosch_a_radar: bool = True
   tesla_preap_stock: bool = True
+  tesla_preap_radar: RadarSettings | None = None
+  tesla_preap_pedal: PedalSettings | None = None
   volt_sng: bool = False
   gm_auto_hold: bool = False
   volt_one_pedal: bool = False
@@ -42,7 +46,11 @@ class VehicleStartupPreferences:
     from openpilot.starpilot.car.toyota.radar_preferences import selected as toyota_radar_mode
     from openpilot.starpilot.car.gm.tune_preferences import selected_tune
     from openpilot.starpilot.car.tesla.preap_preferences import stock_configuration
-    preap_stock = stock_configuration(params)
+    from openpilot.starpilot.car.tesla.radar_settings import read_radar_settings
+    from openpilot.starpilot.car.tesla.pedal_preferences import read_pedal_settings
+    radar = read_radar_settings(params)
+    pedal = read_pedal_settings(params)
+    preap_stock = stock_configuration(params, radar, pedal)
     try:
       raw_radar, radar_readable = read_saved(params, "HondaBoschARadar", 8)
       honda_radar = radar_readable and raw_radar in (None, b"1")
@@ -65,6 +73,7 @@ class VehicleStartupPreferences:
                             readable and safe in (None, b"0"))
     except (OSError, TypeError, ValueError):
       return cls(disable_bolt_long=disable_bolt, honda_bosch_a_radar=honda_radar, tesla_preap_stock=preap_stock,
+                 tesla_preap_radar=radar, tesla_preap_pedal=pedal,
                  gm_camera_pedal=False, toyota_filter=False)
     try:
       pitch, pitch_readable = read_saved(params, "LongPitch", 8)
@@ -94,7 +103,8 @@ class VehicleStartupPreferences:
                gm_camera_pedal=bool(enabled and not disable_bolt and readable and safe in (None, b"0")),
                toyota_filter=bool(enabled and not disable_bolt and readable and safe in (None, b"0")),
                turn_assist=bool(enabled and assist and readable and safe in (None, b"0")),
-               gm_long_pitch=pitch_enabled, disable_bolt_long=disable_bolt, honda_bosch_a_radar=honda_radar, tesla_preap_stock=preap_stock)
+               gm_long_pitch=pitch_enabled, disable_bolt_long=disable_bolt, honda_bosch_a_radar=honda_radar,
+               tesla_preap_stock=preap_stock, tesla_preap_radar=radar, tesla_preap_pedal=pedal)
 
   def _prepare_honda_radar(self, cp) -> None:
     if cp.brand != "honda":
@@ -145,7 +155,7 @@ class VehicleStartupPreferences:
   def prepare(self, cp, *, fingerprints=None):
     from opendbc.car.gm.values import apply_gm_auto_hold, apply_volt_one_pedal
     from openpilot.starpilot.car.tesla.preap_preferences import prepare_stock
-    prepare_stock(cp, self.tesla_preap_stock)
+    prepare_stock(cp, self.tesla_preap_stock, self.tesla_preap_radar, self.tesla_preap_pedal)
     from opendbc.car.tesla.screen_button import apply_screen_button
     apply_screen_button(cp, self.tesla_screen, self.tesla_screen_brake)
     self._prepare_honda_radar(cp)
@@ -172,7 +182,7 @@ class VehicleStartupPreferences:
     admitted_one_pedal = is_volt_one_pedal(cp)
     admitted_hold = is_gm_auto_hold(cp)
     from openpilot.starpilot.car.tesla.preap_preferences import prepare_stock
-    prepare_stock(cp, self.tesla_preap_stock)
+    prepare_stock(cp, self.tesla_preap_stock, self.tesla_preap_radar, self.tesla_preap_pedal)
     from opendbc.car.tesla.screen_button import apply_screen_button
     apply_screen_button(cp, self.tesla_screen, self.tesla_screen_brake)
     self._prepare_honda_radar(cp)

panda/board/main_comms.h

@@ -78,6 +78,14 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) {
   uint32_t time;
 
   switch (req->request) {
+    // Fork-local frozen Dom Pre-AP calibration. Configuration is never CAN TX.
+    case 0xb7:
+      (void)tesla_preap_pedal_config_word(req->param1, req->param2,
+        !is_car_safety_mode(current_safety_mode) && !controls_allowed && !heartbeat_engaged);
+      break;
+    case 0xb8:
+      resp_len = tesla_preap_pedal_config_read(resp);
+      break;
     // Fork-local read-only AOL capability and actual native permissions.
     // Older firmware returns no matching packet, so the host fails closed.
     case 0xd5:

third_party/linux/include/media/cam_isp_vfe.h

@@ -0,0 +1,44 @@
+#ifndef __UAPI_CAM_ISP_VFE_H__
+#define __UAPI_CAM_ISP_VFE_H__
+
+/* VFE output port resource type  (global unique)  */
+#define CAM_ISP_VFE_OUT_RES_BASE               0x1000
+
+#define CAM_ISP_VFE_OUT_RES_ENC                (CAM_ISP_VFE_OUT_RES_BASE + 0)
+#define CAM_ISP_VFE_OUT_RES_VIEW               (CAM_ISP_VFE_OUT_RES_BASE + 1)
+#define CAM_ISP_VFE_OUT_RES_VID                (CAM_ISP_VFE_OUT_RES_BASE + 2)
+#define CAM_ISP_VFE_OUT_RES_RDI_0              (CAM_ISP_VFE_OUT_RES_BASE + 3)
+#define CAM_ISP_VFE_OUT_RES_RDI_1              (CAM_ISP_VFE_OUT_RES_BASE + 4)
+#define CAM_ISP_VFE_OUT_RES_RDI_2              (CAM_ISP_VFE_OUT_RES_BASE + 5)
+#define CAM_ISP_VFE_OUT_RES_RDI_3              (CAM_ISP_VFE_OUT_RES_BASE + 6)
+#define CAM_ISP_VFE_OUT_RES_STATS_AEC          (CAM_ISP_VFE_OUT_RES_BASE + 7)
+#define CAM_ISP_VFE_OUT_RES_STATS_AF           (CAM_ISP_VFE_OUT_RES_BASE + 8)
+#define CAM_ISP_VFE_OUT_RES_STATS_AWB          (CAM_ISP_VFE_OUT_RES_BASE + 9)
+#define CAM_ISP_VFE_OUT_RES_STATS_RS           (CAM_ISP_VFE_OUT_RES_BASE + 10)
+#define CAM_ISP_VFE_OUT_RES_STATS_CS           (CAM_ISP_VFE_OUT_RES_BASE + 11)
+#define CAM_ISP_VFE_OUT_RES_STATS_IHIST        (CAM_ISP_VFE_OUT_RES_BASE + 12)
+#define CAM_ISP_VFE_OUT_RES_STATS_SKIN         (CAM_ISP_VFE_OUT_RES_BASE + 13)
+#define CAM_ISP_VFE_OUT_RES_STATS_BG           (CAM_ISP_VFE_OUT_RES_BASE + 14)
+#define CAM_ISP_VFE_OUT_RES_STATS_BF           (CAM_ISP_VFE_OUT_RES_BASE + 15)
+#define CAM_ISP_VFE_OUT_RES_STATS_BE           (CAM_ISP_VFE_OUT_RES_BASE + 16)
+#define CAM_ISP_VFE_OUT_RES_STATS_BHIST        (CAM_ISP_VFE_OUT_RES_BASE + 17)
+#define CAM_ISP_VFE_OUT_RES_STATS_BF_SCALE     (CAM_ISP_VFE_OUT_RES_BASE + 18)
+#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BE       (CAM_ISP_VFE_OUT_RES_BASE + 19)
+#define CAM_ISP_VFE_OUT_RES_STATS_HDR_BHIST    (CAM_ISP_VFE_OUT_RES_BASE + 20)
+#define CAM_ISP_VFE_OUT_RES_STATS_AEC_BG       (CAM_ISP_VFE_OUT_RES_BASE + 21)
+#define CAM_ISP_VFE_OUT_RES_CAMIF_RAW          (CAM_ISP_VFE_OUT_RES_BASE + 22)
+#define CAM_ISP_VFE_OUT_RES_IDEAL_RAW          (CAM_ISP_VFE_OUT_RES_BASE + 23)
+#define CAM_ISP_VFE_OUT_RES_MAX                (CAM_ISP_VFE_OUT_RES_BASE + 24)
+
+/* VFE input port_ resource type (global unique) */
+#define CAM_ISP_VFE_IN_RES_BASE                0x2000
+
+#define CAM_ISP_VFE_IN_RES_TPG                 (CAM_ISP_VFE_IN_RES_BASE + 0)
+#define CAM_ISP_VFE_IN_RES_PHY_0               (CAM_ISP_VFE_IN_RES_BASE + 1)
+#define CAM_ISP_VFE_IN_RES_PHY_1               (CAM_ISP_VFE_IN_RES_BASE + 2)
+#define CAM_ISP_VFE_IN_RES_PHY_2               (CAM_ISP_VFE_IN_RES_BASE + 3)
+#define CAM_ISP_VFE_IN_RES_PHY_3               (CAM_ISP_VFE_IN_RES_BASE + 4)
+#define CAM_ISP_VFE_IN_RES_FE                  (CAM_ISP_VFE_IN_RES_BASE + 5)
+#define CAM_ISP_VFE_IN_RES_MAX                 (CAM_ISP_VFE_IN_RES_BASE + 6)
+
+#endif /* __UAPI_CAM_ISP_VFE_H__ */

tools/ci/run_vehicle_tests.py

@@ -53,7 +53,21 @@ RELEASE_TARGETS = (
   "opendbc_repo/opendbc/car/hyundai/tests/test_classic_cancel_construction.py",
   "openpilot/starpilot/aol/tests/test_classic_cancel_guards.py",
   "opendbc_repo/opendbc/safety/tests/test_ioniq5pe_paired_native.py::TestPELongReleaseDenied",
+  "opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py",
+  "openpilot/starpilot/car/tesla/tests/test_pedal_callers.py",
+  "openpilot/starpilot/car/tesla/tests/test_pedal_planner.py",
+  "openpilot/starpilot/car/tesla/tests/test_preap_adaptive.py",
+  "openpilot/starpilot/car/tesla/tests/test_preap_pause_callers.py",
+  "openpilot/starpilot/car/tesla/tests/test_pedal_schema_evolution.py",
+  "openpilot/starpilot/car/tesla/tests/test_radar_schema_evolution.py",
   "opendbc_repo/opendbc/safety/tests/test_tesla_preap_stock.py",
+  "opendbc_repo/opendbc/safety/tests/test_tesla_preap_radar.py",
+  "opendbc_repo/opendbc/car/tesla/tests/test_preap_radar.py",
+  "openpilot/starpilot/car/tesla/tests/test_radar_settings.py",
+  "openpilot/starpilot/car/tesla/tests/test_radar_context.py",
+  "openpilot/starpilot/car/tesla/tests/test_radar_startup.py",
+  "openpilot/starpilot/car/tesla/tests/test_radar_pipeline.py",
+  "openpilot/starpilot/ui/tests/test_preap_radar_feature.py",
   "opendbc_repo/opendbc/car/tesla/tests/test_preap_startup.py",
   "opendbc_repo/opendbc/car/tesla/tests/test_preap_stock.py",
   "openpilot/starpilot/car/tesla/tests/test_preap_intent.py",