diff --git a/CREDITS.md b/CREDITS.md index ca32b66a7b..dbbc738e58 100644 --- a/CREDITS.md +++ b/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. diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 785732b0ef..383d091fb4 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/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: diff --git a/opendbc_repo/opendbc/car/tesla/preap/aol.py b/opendbc_repo/opendbc/car/tesla/preap/aol.py index 218a9f3bc9..cfdd4483c4 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/aol.py +++ b/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): diff --git a/opendbc_repo/opendbc/car/tesla/preap/constants.py b/opendbc_repo/opendbc/car/tesla/preap/constants.py new file mode 100644 index 0000000000..cbb1f73f16 --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/preap/ff_table_default.py b/opendbc_repo/opendbc/car/tesla/preap/ff_table_default.py new file mode 100644 index 0000000000..c84a793f6c --- /dev/null +++ b/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], +] diff --git a/opendbc_repo/opendbc/car/tesla/preap/pedal_contract.py b/opendbc_repo/opendbc/car/tesla/preap/pedal_contract.py new file mode 100644 index 0000000000..eeda525f58 --- /dev/null +++ b/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('> 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. diff --git a/opendbc_repo/opendbc/car/tesla/preap/pedal_control.py b/opendbc_repo/opendbc/car/tesla/preap/pedal_control.py new file mode 100644 index 0000000000..0a11314017 --- /dev/null +++ b/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)] diff --git a/opendbc_repo/opendbc/car/tesla/preap/pedal_dynamics.py b/opendbc_repo/opendbc/car/tesla/preap/pedal_dynamics.py new file mode 100644 index 0000000000..12a69af30c --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/preap/pedal_parser.py b/opendbc_repo/opendbc/car/tesla/preap/pedal_parser.py new file mode 100644 index 0000000000..a02435567c --- /dev/null +++ b/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) diff --git a/opendbc_repo/opendbc/car/tesla/preap/pedal_profile.py b/opendbc_repo/opendbc/car/tesla/preap/pedal_profile.py new file mode 100644 index 0000000000..f14d0fea52 --- /dev/null +++ b/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)) diff --git a/opendbc_repo/opendbc/car/tesla/preap/radar.py b/opendbc_repo/opendbc/car/tesla/preap/radar.py new file mode 100644 index 0000000000..35896cabe5 --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/preap/radar_profile.py b/opendbc_repo/opendbc/car/tesla/preap/radar_profile.py new file mode 100644 index 0000000000..15e9e7d5d8 --- /dev/null +++ b/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(' 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)) diff --git a/opendbc_repo/opendbc/car/tesla/radar_interface.py b/opendbc_repo/opendbc/car/tesla/radar_interface.py index caadb5a881..cc77d47298 100644 --- a/opendbc_repo/opendbc/car/tesla/radar_interface.py +++ b/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: diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_preap_radar.py b/opendbc_repo/opendbc/car/tesla/tests/test_preap_radar.py new file mode 100644 index 0000000000..d3360fbffb --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_preap_radar_receiver.py b/opendbc_repo/opendbc/car/tesla/tests/test_preap_radar_receiver.py new file mode 100644 index 0000000000..d1f2b218b0 --- /dev/null +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_preap_startup.py b/opendbc_repo/opendbc/car/tesla/tests/test_preap_startup.py index 029bb6f67b..66e21910ee 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_preap_startup.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_preap_stock.py b/opendbc_repo/opendbc/car/tesla/tests/test_preap_stock.py index 11f3e55f1d..5440006d01 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_preap_stock.py +++ b/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 diff --git a/opendbc_repo/opendbc/car/tesla/values.py b/opendbc_repo/opendbc/car/tesla/values.py index 7448b244eb..cc623c6e12 100644 --- a/opendbc_repo/opendbc/car/tesla/values.py +++ b/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', }, ) diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap.h b/opendbc_repo/opendbc/safety/modes/tesla_preap.h index 05efcf1e8d..9b6129ebe8 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla_preap.h +++ b/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; } diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap_pedal.h b/opendbc_repo/opendbc/safety/modes/tesla_preap_pedal.h new file mode 100644 index 0000000000..22b8d6f2f2 --- /dev/null +++ b/opendbc_repo/opendbc/safety/modes/tesla_preap_pedal.h @@ -0,0 +1,223 @@ +#pragma once +#include +#include + +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; +} diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap_radar.h b/opendbc_repo/opendbc/safety/modes/tesla_preap_radar.h new file mode 100644 index 0000000000..e303fa110e --- /dev/null +++ b/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 { + } + } +} diff --git a/opendbc_repo/opendbc/safety/safety.h b/opendbc_repo/opendbc/safety/safety.h index 6cf11438ed..08e9abc50a 100644 --- a/opendbc_repo/opendbc/safety/safety.h +++ b/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; diff --git a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c b/opendbc_repo/opendbc/safety/tests/libsafety/safety.c index e434362a78..69f63c5380 100644 --- a/opendbc_repo/opendbc/safety/tests/libsafety/safety.c +++ b/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); +} diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap_pedal.py new file mode 100644 index 0000000000..20bcb85730 --- /dev/null +++ b/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)) diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap_radar.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap_radar.py new file mode 100644 index 0000000000..95705922cc --- /dev/null +++ b/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])) diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap_stock.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap_stock.py index 2ab6330fb0..36169c1615 100644 --- a/opendbc_repo/opendbc/safety/tests/test_tesla_preap_stock.py +++ b/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): diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 1480bb6a7e..1f57593d1a 100644 --- a/openpilot/cereal/custom.capnp +++ b/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; diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index f8aa8c45b6..4d55276dbf 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -312,7 +312,15 @@ inline static std::unordered_map 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"}}, diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 2721474c14..8d97d27e19 100755 --- a/openpilot/selfdrive/car/card.py +++ b/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)) diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index 79fc72104d..0c650a44fa 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/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) diff --git a/openpilot/selfdrive/controls/lib/longitudinal_planner.py b/openpilot/selfdrive/controls/lib/longitudinal_planner.py index 817b2f7bab..fca50b956c 100755 --- a/openpilot/selfdrive/controls/lib/longitudinal_planner.py +++ b/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 diff --git a/openpilot/selfdrive/pandad/SConscript b/openpilot/selfdrive/pandad/SConscript index d31b5cdb41..7065c076b9 100644 --- a/openpilot/selfdrive/pandad/SConscript +++ b/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']) diff --git a/openpilot/selfdrive/pandad/panda.cc b/openpilot/selfdrive/pandad/panda.cc index ab85df251e..6c7b563fc3 100644 --- a/openpilot/selfdrive/pandad/panda.cc +++ b/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 &words) { + std::array 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(actual.size()) && actual == expected; +} + +bool Panda::stage_preap_pedal(const std::array &words) { + for (size_t i = 0; i < words.size(); ++i) { + if (handle->control_write(0xb7, static_cast(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; } diff --git a/openpilot/selfdrive/pandad/panda.h b/openpilot/selfdrive/pandad/panda.h index 8a987dfb04..11ed52674e 100644 --- a/openpilot/selfdrive/pandad/panda.h +++ b/openpilot/selfdrive/pandad/panda.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -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 &words); + bool matches_preap_pedal(const std::array &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); diff --git a/openpilot/selfdrive/pandad/panda_safety.cc b/openpilot/selfdrive/pandad/panda_safety.cc index 5f837035d1..7c2eaf3013 100644 --- a/openpilot/selfdrive/pandad/panda_safety.cc +++ b/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 ¶ms_string) { +bool PandaSafety::setSafetyMode(const std::string ¶ms_string) { AlignedBuffer aligned_buf; capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); cereal::CarParams::Reader car_params = cmsg.getRoot(); 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 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 ¶ms_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() { diff --git a/openpilot/selfdrive/pandad/pandad.h b/openpilot/selfdrive/pandad/pandad.h index 052496e566..a85dc64869 100644 --- a/openpilot/selfdrive/pandad/pandad.h +++ b/openpilot/selfdrive/pandad/pandad.h @@ -17,7 +17,7 @@ public: private: void updateMultiplexingMode(); std::string fetchCarParams(); - void setSafetyMode(const std::string ¶ms_string); + bool setSafetyMode(const std::string ¶ms_string); bool initialized_ = false; bool log_once_ = false; @@ -27,5 +27,8 @@ private: std::vector> safety_slots_; uint16_t alternative_experience_ = 0; size_t safety_config_count_ = 0; + std::optional> preap_pedal_words_; + std::string preap_pedal_capsule_; + std::string preap_pedal_cp_bytes_; Params params_; }; diff --git a/openpilot/selfdrive/pandad/preap_pedal_wire.h b/openpilot/selfdrive/pandad/preap_pedal_wire.h new file mode 100644 index 0000000000..751c89b459 --- /dev/null +++ b/openpilot/selfdrive/pandad/preap_pedal_wire.h @@ -0,0 +1,78 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#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; + +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 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(before.st_size)) : -1; + const bool stable = fstat(fd, &after) == 0 && lstat(path.c_str(), ¤t) == 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(count)); +} + +inline std::optional 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(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(); + 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(hash[2 * i + 1]) << 8); + return result; + } catch (const kj::Exception &) { + return std::nullopt; + } +} diff --git a/openpilot/selfdrive/pandad/preap_sha256.h b/openpilot/selfdrive/pandad/preap_sha256.h new file mode 100644 index 0000000000..be5df72a50 --- /dev/null +++ b/openpilot/selfdrive/pandad/preap_sha256.h @@ -0,0 +1,53 @@ +#pragma once +#include +#include +#include + +// SHA-256 over the bounded serialized startup CP; no external runtime library. +inline std::array preap_sha256(std::string_view input) { + constexpr std::array 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 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(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 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(input[position]); + else if (position == input.size()) value = 0x80U; + else if (block == blocks - 1U && byte >= 56U) value = static_cast(bits >> ((63U - byte) * 8U)); + words[byte / 4U] |= static_cast(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 result{}; + for (size_t i = 0; i < result.size(); ++i) result[i] = static_cast(hash[i / 4U] >> ((3U - i % 4U) * 8U)); + return result; +} diff --git a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc b/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc index b8da4b09d1..b4f6900f56 100644 --- a/openpilot/selfdrive/pandad/tests/test_aol_protocol.cc +++ b/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(word)) == (word == 0U)); - assert(tesla_preap_aol_param(static_cast(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(word)) == preap_expected); auto preap = status(); preap.safety_mode = 39U; preap.safety_param = static_cast(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}; diff --git a/openpilot/selfdrive/pandad/tests/test_preap_pedal_startup.cc b/openpilot/selfdrive/pandad/tests/test_preap_pedal_startup.cc new file mode 100644 index 0000000000..1f0fa4e459 --- /dev/null +++ b/openpilot/selfdrive/pandad/tests/test_preap_pedal_startup.cc @@ -0,0 +1,191 @@ +#include +#include +#include +#include +#include +#include +#include +#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 T symbol(const char *name) { + auto result = reinterpret_cast(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("set_safety_hooks"); + mode = symbol("get_current_safety_mode"); + param = symbol("get_current_safety_param"); + alternative = symbol("set_alternative_experience"); + heartbeat = symbol("set_aol_test_heartbeat"); + word = symbol("tesla_preap_test_config_word"); + read = symbol("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 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(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 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(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(); + 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> 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(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 ¶ms; 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(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(); + 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(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(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(cereal::CarParams::SafetyModel::NO_OUTPUT)); + handle->old_firmware = false; + } +} +int main() { return run_native_test(actual_panda_safety_to_native_startup); } diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 5c51290b3c..3f1775802e 100644 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/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) diff --git a/openpilot/starpilot/car/tesla/aol.py b/openpilot/starpilot/car/tesla/aol.py index e0e2a6d233..dc7d067299 100644 --- a/openpilot/starpilot/car/tesla/aol.py +++ b/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): diff --git a/openpilot/starpilot/car/tesla/aol_policy.h b/openpilot/starpilot/car/tesla/aol_policy.h index aa074f6102..84c691a0d3 100644 --- a/openpilot/starpilot/car/tesla/aol_policy.h +++ b/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}; diff --git a/openpilot/starpilot/car/tesla/pedal_events.py b/openpilot/starpilot/car/tesla/pedal_events.py new file mode 100644 index 0000000000..9c5eec670a --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/pedal_preferences.py b/openpilot/starpilot/car/tesla/pedal_preferences.py new file mode 100644 index 0000000000..a8e32214af --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/pedal_startup.py b/openpilot/starpilot/car/tesla/pedal_startup.py new file mode 100644 index 0000000000..84a74eda22 --- /dev/null +++ b/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(' 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 diff --git a/openpilot/starpilot/car/tesla/preap_adaptive.py b/openpilot/starpilot/car/tesla/preap_adaptive.py new file mode 100644 index 0000000000..66cf6f6c50 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/preap_preferences.py b/openpilot/starpilot/car/tesla/preap_preferences.py index 363485711f..185f3ba02e 100644 --- a/openpilot/starpilot/car/tesla/preap_preferences.py +++ b/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)] diff --git a/openpilot/starpilot/car/tesla/radar_context.py b/openpilot/starpilot/car/tesla/radar_context.py new file mode 100644 index 0000000000..4e3f6331ae --- /dev/null +++ b/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(' 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 diff --git a/openpilot/starpilot/car/tesla/radar_settings.py b/openpilot/starpilot/car/tesla/radar_settings.py new file mode 100644 index 0000000000..930da37bf8 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/stock_events.py b/openpilot/starpilot/car/tesla/stock_events.py index 38a1404832..bea7346b42 100644 --- a/openpilot/starpilot/car/tesla/stock_events.py +++ b/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 diff --git a/openpilot/starpilot/car/tesla/stock_intent.py b/openpilot/starpilot/car/tesla/stock_intent.py index b103f321f0..3227953652 100644 --- a/openpilot/starpilot/car/tesla/stock_intent.py +++ b/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) diff --git a/openpilot/starpilot/car/tesla/tests/fixtures/custom_before_preap_pedal.capnp b/openpilot/starpilot/car/tesla/tests/fixtures/custom_before_preap_pedal.capnp new file mode 100644 index 0000000000..1480bb6a7e --- /dev/null +++ b/openpilot/starpilot/car/tesla/tests/fixtures/custom_before_preap_pedal.capnp @@ -0,0 +1,1397 @@ +using Cxx = import "/include/c++.capnp"; +$Cxx.namespace("cereal"); +using Car = import "/car.capnp"; + +@0xb526ba661d550a59; + +# custom.capnp: a home for empty structs reserved for custom forks +# These structs are guaranteed to remain reserved and empty in mainline +# cereal, so use these if you want custom events in your fork. + +# DO rename the structs +# DON'T change the identifier (e.g. @0x81c2f05a394cf4af) + +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; + } + + phoneGps @3 :StarPilotCarState.Gps; # sourceMonoTime is comma receive time minus the phone's fix age +} +struct StarPilotCarParams @0xaedffd8f31e7b55d { + alternativeExperience @0 :Int16; + canUsePedal @1 :Bool; + canUseSDSU @2 :Bool; + flags @3 :UInt32; + isHDA2 @4 :Bool; + openpilotLongitudinalControlDisabled @5 :Bool; + safetyConfigs @6 :List(SafetyConfig); + pcmCruiseSpeed @7 :Bool = true; + redneckCruiseAvailable @8 :Bool; + + struct SafetyConfig { + safetyParam @0 :UInt16; + } + + learning @9 :AutoSteeringLearning; + + struct AutoSteeringLearning { + version @0 :UInt16; + status @1 :Status; + reason @2 :Text; + identity @3 :Identity; + session @4 :Text; + sequence @5 :UInt64; + observedMonoTime @6 :UInt64; # MONOTONIC, separate from Event BOOTTIME. + validUntilMonoTime @7 :UInt64; + sourceWindowStartMonoTime @8 :UInt64; + sourceWindowEndMonoTime @9 :UInt64; + family @10 :Family; + latAccelFactor @11 :Float64; + acceptancePolicyId @12 :Text; + trainWindows @13 :UInt32; + heldoutWindows @14 :UInt32; + trainBaselineLoss @15 :Float64; + trainCandidateLoss @16 :Float64; + heldoutBaselineLoss @17 :Float64; + heldoutCandidateLoss @18 :Float64; + + enum Status { unavailable @0; collecting @1; candidate @2; rejected @3; qualifiedFit @4; } + enum Family { none @0; linearFactor @1; runtimeProfile @2; } + runtimeProfile @19 :AutomaticRuntimeProfile; + sourceReceipt @20 :AutomaticLearningSourceReceipt; + centreApplicationWitness @21 :CentreApplicationWitness; + struct Identity { + vehicle @0 :Text; + brand @1 :Text; + controller @2 :Text; + policy @3 :Text; + profile @4 :Text; + conversionVersion @5 :Text; + firmwareSha256 @6 :Text; + basis @7 :List(Float64); + baselineSha256 @8 :Text; + preferenceSha256 @9 :Text; + resetGeneration @10 :Text; + controllerRevision @11 :Text; + } + } +} + +struct StarPilotCarState @0xf35cc4560bbf6ec2 { + accelPressed @0 :Bool; + alwaysOnLateralEnabled @1 :Bool; + brakeLights @2 :Bool; + dashboardSpeedLimit @3 :Float32; + decelPressed @4 :Bool; + distancePressed @5 :Bool; + distanceLongPressed @6 :Bool; + distanceVeryLongPressed @7 :Bool; + ecoGear @8 :Bool; + forceCoast @9 :Bool; + isParked @10 :Bool; + pauseLateral @11 :Bool; + pauseLongitudinal @12 :Bool; + sportGear @13 :Bool; + trafficModeEnabled @14 :Bool; + gasStack @15 :Bool; # Compatibility with older StarPilot payloads + modePressed @16 :Bool; + customPressed @17 :Bool; + alwaysOnLateralAllowed @18 :Bool; + dashboardStopSign @19 :UInt8; # 0 = no signal / platform doesn't publish + cancelPressed @20 :Bool; + cancelLongPressed @21 :Bool; + cancelVeryLongPressed @22 :Bool; + pedalMaxRegen @23 :Bool; # pedal at max regen, driver should use brake for more decel + pedalLongActive @24 :Bool; # Pre-AP pedal longitudinal mode is active (enableLongControl) + teslaCCEngaged @25 :Bool; # rising edge of stock Tesla CC engaging (no-pedal mode) + teslaCCDisengaged @26 :Bool; # falling edge of stock Tesla CC + teslaCCNotArmed @27 :Bool; # lateral engaged but DI_cruiseState != STANDBY/ENABLED + accelHardCruise @28 :Bool; # current/releasing accel cruise button came from GM hard-press signal + decelHardCruise @29 :Bool; # current/releasing decel cruise button came from GM hard-press signal + pulseAndGlide @30 :Bool; # developer-only wheel-button pulse-and-glide mode is enabled + gps @31 :Gps; + lateralAuthorityUnavailable @32 :Bool; + sourceCarStateMonoTime @33 :UInt64; + appliedEvidence @34 :LearningAppliedEvidence; + fordLearningEvidence @35 :FordLearningEvidence; + learningApplication @36 :LearningApplication; + + struct Gps { + sourceMonoTime @0 :UInt64; # CAN observation converted to CLOCK_MONOTONIC + latitude @1 :Float64; + longitude @2 :Float64; + altitude @3 :Float64; + speed @4 :Float32; + bearingDeg @5 :Float32; + horizontalAccuracy @6 :Float32; + verticalAccuracy @7 :Float32; + bearingAccuracyDeg @8 :Float32; + speedAccuracy @9 :Float32; + unixTimestampMillis @10 :Int64; + hasFix @11 :Bool; + vNED @12 :List(Float32); + } +} + +struct StarPilotLongitudinalPlan @0xda96579883444c35 { + version @0 :UInt16; + sourcePlanMonoTime @1 :UInt64; + modelMonoTime @2 :UInt64; + driveStartMonoTime @3 :UInt64; + forceStopHolding @4 :Bool; +} + +struct SlcState @0xa1680744031fdb2d { + # Retain the historical model-status fields for log compatibility. + slotId @0 :Text; + slotName @1 :Text; + variant @2 :Text; + variantLabel @3 :Text; + reason @4 :Text; + wallTimeNanos @5 :UInt64; + + sessionId @6 :Text; + frameMonoTime @7 :UInt64; + enabled @8 :Bool; + displayOnly @9 :Bool; + observationKind @10 :Text; + source @11 :Text; + speedLimit @12 :Float32; # m/s, meaningful only for a valid observation + acceptedSpeedLimit @13 :Float32; # m/s, meaningful only when hasAccepted is true + hasAccepted @14 :Bool; + offset @15 :Float32; # m/s + effectiveCap @16 :Float32; # m/s, meaningful only when hasCeiling is true + hasCeiling @17 :Bool; + pendingSpeedLimit @18 :Float32; # m/s, meaningful only when hasPending is true + hasPending @19 :Bool; + decisionId @20 :UInt64; + presentationId @21 :UInt64; + status @22 :Text; + actionSequenceId @23 :UInt64; + actionStatus @24 :Text; + sourceProducerSessionId @25 :Text; + sourceEpisode @26 :UInt32; + sourceObservedMonoTime @27 :UInt64; + sourceValidUntilMonoTime @28 :UInt64; + commandId @29 :UInt64; + commandStatus @30 :Text; + # Independently qualified diagnostics from the same plannerd publisher. + # Existing SLC fields keep their meaning when the curve runtime is present. + curve @31 :CurveState; + # Saved conditional-mode proposal from this plannerd cycle. Consumers must + # independently join it to live authority and the current saved revision. + conditionalMode @32 :ConditionalModeState; + # Exact Ioniq physical media intent, accepted by this planner only while + # its original source and current longitudinal authority remain qualified. + trafficMode @33 :TrafficModeState; + switchbackMode @34 :SwitchbackModeState; + # Presentation metadata only; planner/control consumers retain existing fields. + acceptedSource @35 :Text; + pendingSource @36 :Text; + effectiveClusterTarget @37 :Float32; # m/s in the selected-cruise cluster coordinate + hasEffectiveClusterTarget @38 :Bool; + isLimitingMaxSet @39 :Bool; # qualified raw ceiling below selected raw cruise + driverOverrideActive @40 :Bool; + # Presentation only. Qualified source observations from this publisher cycle; + # consumers use the outer session and frame freshness, never these rows for control. + sourceReadings @41 :List(SourceReading); + acceptedAdjustedLimit @42 :Float32; + hasAcceptedAdjustedLimit @43 :Bool; + pendingAdjustedLimit @44 :Float32; + hasPendingAdjustedLimit @45 :Bool; + presentationAdjustedLimit @46 :Float32; + hasPresentationAdjustedLimit @47 :Bool; + overrideBasis @48 :Text; # none/pedal/persistent; qualified contribution only + retainedOverride @49 :Bool; + commandActionSequenceId @50 :UInt64; # UI sequence that issued this command + struct SourceReading { + source @0 :Text; + enabled @1 :Bool; + observationKind @2 :Text; # valid/absent/stale/unknown; numeric defaults are not readings + speedLimit @3 :Float32; # raw m/s only when observationKind is valid + } + + struct SwitchbackModeState { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + driveStartMonoTime @5 :UInt64; + requested @6 :Bool; + effective @7 :Bool; + sourceCarControlMonoTime @8 :UInt64; + } + + struct TrafficModeState { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + driveStartMonoTime @5 :UInt64; + sourceEpoch @6 :UInt64; + sourceBootTime @7 :UInt64; + accepted @8 :Bool; + effective @9 :Bool; + reason @10 :Text; + settingsFingerprint @11 :Text; + buttonMapFingerprint @12 :Text; + profileTargetReady @13 :Bool; + profileReason @14 :Text; + controllerSource @15 :Bool; + } + + struct CurveState { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + modelMonoTime @5 :UInt64; + configured @6 :Bool; + documentValid @7 :Bool; + hasCandidate @8 :Bool; + candidateMps @9 :Float32; + hasCeiling @10 :Bool; + ceilingMps @11 :Float32; + applied @12 :Bool; + controlling @13 :Bool; + training @14 :Bool; + calibrationProgress @15 :Float32; # percent, 0..100 + comfortAccel @16 :Float32; + bindingDistance @17 :Float32; + reason @18 :Text; + plannerStatus @19 :Text; + persistenceStatus @20 :Text; + glow @21 :Bool; + curveOnly @22 :Bool; + hasRoadCurvature @23 :Bool; + roadCurvature @24 :Float32; # 1/m, diagnostic prediction for display + } + + struct ConditionalModeState { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + driveStartMonoTime @5 :UInt64; + modelMonoTime @6 :UInt64; + carStateMonoTime @7 :UInt64; + settingsRevision @8 :UInt64; + settingsFingerprint @9 :Text; + choice @10 :Choice; + hasOverride @11 :Bool; + experimental @12 :Bool; + status @13 :Text; + reason @14 :Text; + statusCode @15 :UInt16; + + enum Choice { + stock @0; + conditionalExperimental @1; + conditionalChill @2; + } + } +} + +struct SlcAction @0x9ccdc8676701b412 { + sessionId @0 :Text; + sequenceId @1 :UInt64; + decisionId @2 :UInt64; + presentationId @3 :UInt64; + kind @4 :Kind; + conditionalManual @5 :ConditionalManualAction; + controllerCruise @6 :ControllerCruiseAction; + controllerMode @7 :ControllerCruiseAction; + + struct ControllerCruiseAction { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + driveStartMonoTime @5 :UInt64; + carFingerprint @6 :Text; + sourceCarControlMonoTime @7 :UInt64; + actionCode @8 :UInt8; + buttonKey @9 :Text; + sourceCarStateMonoTime @10 :UInt64; + carParamsFingerprint @11 :Text; + targetSpeedMps @12 :Float32; + } + + struct ConditionalManualAction { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + driveStartMonoTime @5 :UInt64; + settingsFingerprint @6 :Text; + choice @7 :SlcState.ConditionalModeState.Choice; + plannerSessionId @8 :Text; + sourceCarStateMonoTime @9 :UInt64; + sourceSelfdriveStateMonoTime @10 :UInt64; + expectedManualCode @11 :UInt8; + expectedExperimental @12 :Bool; + } + + enum Kind { + unknown @0; + accept @1; + reject @2; + adopt @3; + conditionalModeCycle @4; + cruiseIncrease @5; + cruiseDecrease @6; + trafficModeToggle @7; + switchbackModeToggle @8; + cruiseSet @9; + forceCoastToggle @10; + pulseGlideToggle @11; + disengageRequest @12; + } +} + +struct SlcCruiseEvent @0xcd96dafb67a082d0 { + eventId @0 :UInt64; + observedMonoTime @1 :UInt64; + previousMps @2 :Float32; + selectedMps @3 :Float32; + button @4 :Button; + longPress @5 :Bool; + producerSessionId @6 :Text; + kind @7 :Kind; + sessionId @8 :Text; + decisionId @9 :UInt64; + presentationId @10 :UInt64; + commandId @11 :UInt64; + manualMode @12 :ManualModeGesture; + trafficMode @13 :TrafficModeGesture; + wheelAction @14 :SlcAction.ControllerCruiseAction; + teslaStockCruise @15 :TeslaStockCruise; + + struct TeslaStockCruise { + version @0 :UInt16; + carStateMonoTime @1 :UInt64; + cpFingerprint @2 :Text; + engaged @3 :Bool; + notArmed @4 :Bool; + } + + enum Kind { + unknown @0; + driverChange @1; + confirmationAccept @2; + confirmationReject @3; + commandApplied @4; + commandRejected @5; + curveAccelPress @6; + conditionalMode @7; + trafficMode @8; + switchbackMode @9; + wheelAction @10; + teslaStockCruise @11; + } + + struct TrafficModeGesture { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + driveStartMonoTime @4 :UInt64; + settingsFingerprint @5 :Text; + buttonMapFingerprint @6 :Text; + sourceEpoch @7 :UInt64; + sourceBootTime @8 :UInt64; + sourceCarStateMonoTime @9 :UInt64; + validUntilMonoTime @10 :UInt64; + toggle @11 :Bool; + button @12 :Button; + press @13 :Press; + + enum Button { unknown @0; mode @1; custom @2; distance @3; } + enum Press { unknown @0; short @1; long @2; veryLong @3; } + } + + enum Button { + unknown @0; + accel @1; + decel @2; + } + + struct ManualModeGesture { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + driveStartMonoTime @4 :UInt64; + settingsFingerprint @5 :Text; + choice @6 :Choice; + button @7 :Button; + press @8 :Press; + sourceCarStateMonoTime @9 :UInt64; + validUntilMonoTime @10 :UInt64; + + enum Choice { + stock @0; + conditionalExperimental @1; + conditionalChill @2; + } + + enum Button { + unknown @0; + lkas @1; + distance @2; + mode @3; + custom @4; + } + + enum Press { + unknown @0; + short @1; + long @2; + veryLong @3; + } + } +} + +struct SlcDashboardObservation @0xb057204d7deadf3f { + producerSessionId @0 :Text; + carStateLogMonoTime @1 :UInt64; + status @2 :Status; + speedMps @3 :Float32; # SI, only meaningful when valid + observedMonoTime @4 :UInt64; + validUntilMonoTime @5 :UInt64; + episode @6 :UInt32; + + enum Status { + unknown @0; + valid @1; + absent @2; + stale @3; + } +} + +struct StarPilotModelDataV2 @0x80ae746ee2596b11 { + turnDirection @0 :TurnDirection; + vision @1 :VisionObservation; + + enum TurnDirection @0xab5928774e6e64fc { + none @0; + turnLeft @1; + turnRight @2; + } + + struct VisionObservation @0x9e04a179bcb4ab72 { + producerSessionId @0 :Text; + status @1 :Status; + speedMps @2 :Float32; # meaningful only for valid + confidence @3 :Float32; # 0..1, meaningful only for valid + supportCount @4 :UInt16; + observedMonoTime @5 :UInt64; # Python CLOCK_MONOTONIC at inference completion + validUntilMonoTime @6 :UInt64; # short producer support TTL in CLOCK_MONOTONIC + cameraFrameEofBootTime @7 :UInt64; # VisionIPC/camerad CLOCK_BOOTTIME + episode @8 :UInt32; + modelId @9 :Text; # exact bundled detector/classifier identity + frameId @10 :UInt32; + stream @11 :Stream; + + enum Status @0xc97a1c56dc1427ae { + unknown @0; + valid @1; + stale @2; + unavailable @3; + } + + enum Stream @0xb45fd9e3634d228f { + unknown @0; + road @1; + wideRoad @2; + } + } +} + +struct CustomReserved5 @0xa5cd762cd951a455 { +} + +struct CustomReserved6 @0xf98d843bfd7004a3 { + # Temporary old StarPilot route replay compatibility: historical starpilotPlan at Event @113. + # Adapter and retirement instructions: openpilot/starpilot/ui/old_starpilot_route_shim.py. + # Keep field ordinals/types frozen. When old-route replay support is removed, + # empty this struct but retain its type ID. + # Historical prefix fields preserve the wire layout needed to decode the SLC fields below. + accelerationJerk @0 :Float32; cscControllingSpeed @1 :Bool; cscSpeed @2 :Float32; + cscTraining @3 :Bool; dangerFactor @4 :Float32; dangerJerk @5 :Float32; + desiredFollowDistance @6 :Int64; experimentalMode @7 :Bool; forcingStop @8 :Bool; + forcingStopLength @9 :Float32; starpilotEvents @10 :AnyPointer; starpilotToggles @11 :Text; + increasedStoppedDistance @12 :Float32; lateralCheck @13 :Bool; laneWidthLeft @14 :Float32; + laneWidthRight @15 :Float32; maxAcceleration @16 :Float32; minAcceleration @17 :Float32; + redLight @18 :Bool; roadCurvature @19 :Float32; + # Historical SLC values (speeds/offsets in m/s); replay display only. + slcMapSpeedLimit @20 :Float32; + slcMapboxSpeedLimit @21 :Float32; slcNextSpeedLimit @22 :Float32; slcOverriddenSpeed @23 :Float32; + slcSpeedLimit @24 :Float32; slcSpeedLimitOffset @25 :Float32; slcSpeedLimitSource @26 :Text; +} + +struct StarPilotRadarState @0xb86e6369214c01c8 { + # Frozen 678af783 fields and nested layouts are retained for old log readers. + # These historical fields are not populated by the new qualified producer. + leadLeft @0 :LeadData; + leadRight @1 :LeadData; + adjacentStopped @2 :AdjacentStopped; + + struct AdjacentStopped { + status @0 :Bool; + dRel @1 :Float32; + yRel @2 :Float32; + radarTrackId @3 :Int32 = -1; + } + + struct LeadData { + dRel @0 :Float32; + yRel @1 :Float32; + vRel @2 :Float32; + aRel @3 :Float32; + vLead @4 :Float32; + dPath @6 :Float32; + vLat @7 :Float32; + vLeadK @8 :Float32; + aLeadK @9 :Float32; + fcw @10 :Bool; + status @11 :Bool; + aLeadTau @12 :Float32; + modelProb @13 :Float32; + radar @14 :Bool; + radarTrackId @15 :Int32 = -1; + + aLeadDEPRECATED @5 :Float32; + } + + qualifiedAdjacent @3 :QualifiedAdjacent; + + struct QualifiedAdjacent { + version @0 :UInt16; + status @1 :Status; + producerSessionId @2 :Text; + sequence @3 :UInt64; + radarTracksMonoTime @4 :UInt64; + modelMonoTime @5 :UInt64; + carStateMonoTime @6 :UInt64; + cameraEofBootTime @7 :UInt64; + observedMonoTime @8 :UInt64; + validUntilMonoTime @9 :UInt64; + left @10 :Candidate; + right @11 :Candidate; + + enum Status { + unknown @0; + clear @1; + ambiguous @2; + } + + struct Candidate { + present @0 :Bool; + trackId @1 :UInt32; + distanceM @2 :Float32; + lateralM @3 :Float32; + speedMps @4 :Float32; + } + } +} + +struct StarPilotSelfdriveState @0xf416ec09499d9d19 { + # Exact historical fields @0..6. Old route readers retain their meanings. + alertText1 @0 :Text; + alertText2 @1 :Text; + alertStatus @2 :AlertStatus; + alertSize @3 :AlertSize; + alertType @4 :Text; + alertSound @5 :Car.CarControl.HUDControl.AudibleAlert; + vEgo @6 :Float32; + conditionalModeAck @7 :ConditionalModeAck; + + enum AlertStatus @0xc0f486ad93ed68c9 { + normal @0; + userPrompt @1; + critical @2; + starpilot @3; + } + + enum AlertSize @0xe22723d973fc2afb { + none @0; + small @1; + mid @2; + full @3; + } + + struct ConditionalModeAck @0xb42d77eaa5b93711 { + version @0 :UInt16; + sessionId @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + sourceSelfdriveStateMonoTime @5 :UInt64; + driveStartMonoTime @6 :UInt64; + choice @7 :SlcState.ConditionalModeState.Choice; + accepted @8 :Bool; + effectiveExperimental @9 :Bool; + plannerSessionId @10 :Text; + plannerSequence @11 :UInt64; + settingsFingerprint @12 :Text; + settingsRevision @13 :UInt64; + modelMonoTime @14 :UInt64; + reason @15 :Text; + statusCode @16 :UInt16; + } +} + +struct SpotMonitorState @0xcb9fd56c7057593a { + # Frozen StarPilotLateralManeuverPlanDEPRECATED @136 carried this field. + # It retains the original 1/m meaning; V-ASM never populates it. + desiredCurvature @0 :Float32; + observation @1 :Observation; + + struct Observation { + version @0 :UInt16; + producerSessionId @1 :Text; + sequence @2 :UInt64; + modelSha256 @3 :Text; + settingsFingerprint @4 :Text; + observedMonoTime @5 :UInt64; + observedBootTime @6 :UInt64; + sourceFrameId @7 :UInt64; + sourceFrameEofBootTime @8 :UInt64; + validUntilBootTime @9 :UInt64; + # Display sides: left derives from camera-right, right from camera-left. + left @10 :Side; + right @11 :Side; + + struct Side { + status @0 :Status; + confidence @1 :Float32; + warning @2 :Bool; + sourceFrameId @3 :UInt64; + sourceFrameEofBootTime @4 :UInt64; + sourceObservedMonoTime @5 :UInt64; + validUntilBootTime @6 :UInt64; + + enum Status { + unknown @0; + clear @1; + warning @2; + } + } + } +} + +struct StarPilotLateralState @0xc2243c65e0340384 { + active @0 :Bool; + frictionThreshold @1 :Float32; + frictionScale @2 :Float32; + feedforward @3 :Float32; + frictionJerk @4 :Float32; + frictionJerkDeadzone @5 :Float32; + lowSpeedFactor @6 :Float32; + unwindDetected @7 :Bool; + laneCentering @8 :LaneCentering; + learningEvidence @9 :LearningControlEvidence; + learningApplication @10 :LearningApplication; + + struct LaneCentering { + version @0 :UInt16; + modelMonoTime @1 :UInt64; + carControlMonoTime @2 :UInt64; + lateralActive @3 :Bool; + requestedCorrection @4 :Float32; + appliedCorrection @5 :Float32; + reason @6 :Text; + } +} + +struct SlcCruiseCommand @0xbd443b539493bc68 { + kind @0 :Kind; + sessionId @1 :Text; + commandId @2 :UInt64; + actionId @3 :UInt64; + decisionId @4 :UInt64; + presentationId @5 :UInt64; + sourceProducerSessionId @6 :Text; + sourceEpisode @7 :UInt32; + sourceObservedMonoTime @8 :UInt64; + sourceValidUntilMonoTime @9 :UInt64; + issuedMonoTime @10 :UInt64; + expiresMonoTime @11 :UInt64; + expectedSelectedMps @12 :Float32; + targetMps @13 :Float32; + + enum Kind { + unknown @0; + adoptAcceptedHigherLimit @1; + } +} + +struct AolAxisState @0xfc6241ed8877b611 { + sessionId @0 :Text; + sequence @1 :UInt64; + sourceCarStateMonoTime @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + mode @5 :Mode; + lateralActive @6 :Bool; + longitudinalActive @7 :Bool; + qualified @8 :Bool; + desiredLateral @9 :Bool; + desiredLongitudinal @10 :Bool; + # Fresh negotiated native status exists; each active axis separately checks + # its requested bit and permission against AolSafetyState. + nativeAcknowledged @11 :Bool; + optionalSetRelease @12 :Bool; + # Intent retention provenance; default/old senders cannot classify a pause. + faultReason @13 :FaultReason; + faultEventMonoTime @14 :UInt64; + faultSessionId @15 :Text; + intentProducerSessionId @16 :Text; + intentIntegrityEpoch @17 :UInt64; + + enum FaultReason { + unknown @0; + none @1; + transportPause @2; + critical @3; + availabilityPause @4; + } + + enum Mode { + off @0; + lateralOnly @1; + longitudinalOnly @2; + combined @3; + } + + # Flat Cap'n Proto payloads carried by reserved Event Data slots 124/125. + # Keeping these nested under the owned axis type gives them fresh generated + # type IDs without repurposing historic Mapd reserved types 17/18. + struct SafetyWire { + kind @0 :UInt8; + version @1 :UInt16; + protocolVersion @2 :UInt16; + compatible @3 :Bool; + observedMonoTime @4 :UInt64; + validUntilMonoTime @5 :UInt64; + safetyModel @6 :UInt16; + safetyParam @7 :UInt16; + lateralAllowed @8 :Bool; + longitudinalAllowed @9 :Bool; + requestedLateral @10 :Bool; + requestedLongitudinal @11 :Bool; + pandaSerial @12 :Text; + axisSessionId @13 :Text; + # Exact source Event stamp uses pandad BOOTTIME, for provenance only. + # Inventory shares the wire's CLOCK_MONOTONIC observed/expiry lease. + sourcePandaStatesMonoTime @14 :UInt64; + pandaInventory @15 :List(PandaSlot); + + struct PandaSlot { + slotIndex @0 :UInt16; + hardwareSerial @1 :Text; + safetyModel @2 :UInt16; + safetyParam @3 :UInt16; + alternativeExperience @4 :UInt16; + controlsAllowed @5 :Bool; + safetyRxChecksInvalid @6 :Bool; + heartbeatLost @7 :Bool; + faults @8 :UInt32; + } + } + + struct IntentWire { + kind @0 :UInt8; + version @1 :UInt16; + producerSessionId @2 :Text; + sequence @3 :UInt64; + carStateLogMonoTime @4 :UInt64; + observedMonoTime @5 :UInt64; + validUntilMonoTime @6 :UInt64; + allowedLatch @7 :Bool; + pauseLateral @8 :Bool; + pauseLongitudinal @9 :Bool; + settingsQualified @10 :Bool; + # Selection feedback only; allowedLatch retains its driving gates. + lateralArmed @11 :Bool; + optionalSetRelease @12 :Bool; + } + + # A separate modeld-owned payload carried by reserved Event Data @126. + # Nesting keeps its new type inside an existing fork-owned schema root. + struct LaneChangeStatusWire { + kind @0 :UInt8; + version @1 :UInt16; + producerSessionId @2 :Text; + sequence @3 :UInt64; + modelFrameId @4 :UInt32; + modelTimestampEofNs @5 :UInt64; + observedMonoTimeNs @6 :UInt64; + validUntilMonoTimeNs @7 :UInt64; + phase @8 :Phase; + direction @9 :Direction; + autoConfigured @10 :Bool; + engaged @11 :Bool; + + enum Phase { + manualRequired @0; + waitingForDelay @1; + laneUnavailable @2; + blindspotBlocked @3; + } + + enum Direction { + none @0; + left @1; + right @2; + } + } +} + +struct MapdExtendedOut @0xa30662f84033036c { + downloadProgress @0 :MapdDownloadProgress; + settings @1 :Text; + path @2 :List(MapdPathPoint); + position @3 :MapdPosition; + loopRateAverage @4 :Float32; + loopRateMin @5 :Float32; + + struct MapdDownloadLocationDetails @0xff889853e7b0987f { + location @0 :Text; + totalFiles @1 :UInt32; + downloadedFiles @2 :UInt32; + } + + struct MapdDownloadProgress @0xfaa35dcac85073a2 { + active @0 :Bool; + cancelled @1 :Bool; + totalFiles @2 :UInt32; + downloadedFiles @3 :UInt32; + locations @4 :List(Text); + locationDetails @5 :List(MapdDownloadLocationDetails); + } + + struct MapdPathPoint @0xd6f78acca1bc3939 { + latitude @0 :Float64; + longitude @1 :Float64; + curvature @2 :Float32; + targetVelocity @3 :Float32; + } + + struct MapdPosition @0xde9705979aca8339 { + latitude @0 :Float64; + longitude @1 :Float64; + } +} + +struct MapdIn @0xc86a3d38d13eb3ef { + type @0 :MapdInputType; + float @1 :Float32; + str @2 :Text; + bool @3 :Bool; + jsonPath @4 :Text; + + enum MapdInputType @0x859f26628fc24358 { + download @0; + reloadSettings @9; + saveSettings @10; + loadDefaultSettings @21; + loadRecommendedSettings @22; + loadPersistentSettings @26; + cancelDownload @27; + setJsonPathFloat @43; + setJsonPathText @44; + setJsonPathBool @45; + acceptSpeedLimit @34; + + # DEPRECATED settings inputs + setLogLevel @6; + setLogSource @29; + setLogJson @28; + setTargetLateralAccel @1; + setSpeedLimitOffset @2; + setSpeedLimitControl @3; + setMapCurveSpeedControl @4; + setVisionCurveSpeedControl @5; + setVisionCurveTargetLatA @7; + setVisionCurveMinTargetV @8; + setEnableSpeed @11; + setVisionCurveUseEnableSpeed @12; + setMapCurveUseEnableSpeed @13; + setSpeedLimitUseEnableSpeed @14; + setHoldLastSeenSpeedLimit @15; + setTargetSpeedJerk @16; + setTargetSpeedAccel @17; + setTargetSpeedTimeOffset @18; + setDefaultLaneWidth @19; + setMapCurveTargetLatA @20; + setSlowDownForNextSpeedLimit @23; + setSpeedUpForNextSpeedLimit @24; + setHoldSpeedLimitWhileChangingSetSpeed @25; + setExternalSpeedLimitControl @30; + setExternalSpeedLimit @31; + setSpeedLimitPriority @32; + setSpeedLimitChangeRequiresAccept @33; + setPressGasToAcceptSpeedLimit @35; + setAdjustSetSpeedToAcceptSpeedLimit @36; + setAcceptSpeedLimitTimeout @37; + setPressGasToOverrideSpeedLimit @38; + setConditionalSpeedLimitControl @39; + setShadowCarState @40; + setShadowModelV2 @41; + setShadowGpsLocation @42; + setShadowGpsLocationExternal @46; + } +} + +struct MapdOut @0xa4f1eb3323f5f582 { + # Historical map output, append-compatible with the pinned provider. + # These nested enums retain their original type IDs and ordinal meanings. + wayName @0 :Text; + wayRef @1 :Text; + roadName @2 :Text; + speedLimit @3 :Float32; + nextSpeedLimit @4 :Float32; + nextSpeedLimitDistance @5 :Float32; + hazard @6 :Text; + nextHazard @7 :Text; + nextHazardDistance @8 :Float32; + advisorySpeed @9 :Float32; + nextAdvisorySpeed @10 :Float32; + nextAdvisorySpeedDistance @11 :Float32; + oneWay @12 :Bool; + lanes @13 :UInt8; + tileLoaded @14 :Bool; + speedLimitSuggestedSpeed @15 :Float32; + suggestedSpeed @16 :Float32; + estimatedRoadWidth @17 :Float32; + roadContext @18 :RoadContext; + distanceFromWayCenter @19 :Float32; + visionCurveSpeed @20 :Float32; + mapCurveSpeed @21 :Float32; + waySelectionType @22 :WaySelectionType; + speedLimitAccepted @23 :Bool; + highwayClass @24 :HighwayClass; + wayId @25 :Int64; + conditionalSpeedLimit @26 :Text; + + # Producer v2 sample evidence; older versions are unqualified. + sampleVersion @27 :UInt16; + roadStatus @28 :SampleStatus; + gpsSource @29 :GpsSource; + # V2: Python GPS MONOTONIC observation converted to BOOTTIME after a startup/resume fence. + # V1 raw-time evidence is unqualified. + gpsMonoTime @30 :UInt64; + computedMonoTime @31 :UInt64; # Computation boot-time nanoseconds, not send time. + sourceGeneration @32 :UInt64; + producerSession @33 :UInt64; # Random nonzero process session. + + enum SampleStatus { + unknown @0; + noGps @1; + noCoverage @2; + noMatch @3; + matchedNoLimit @4; + matchedLimit @5; + } + + enum GpsSource { + none @0; + internal @1; + external @2; + car @3; + } + + enum RoadContext @0xabefa88b9563dbae { + freeway @0; + city @1; + unknown @2; + } + + enum WaySelectionType @0xfa3e5ce74e25e82a { + current @0; + predicted @1; + possible @2; + extended @3; + fail @4; + } + + enum HighwayClass @0xd18274dcdc63c41d { + unknown @0; + motorway @1; + motorwayLink @2; + trunk @3; + trunkLink @4; + primary @5; + primaryLink @6; + secondary @7; + secondaryLink @8; + tertiary @9; + tertiaryLink @10; + unclassified @11; + residential @12; + livingStreet @13; + } +} + +# Automatic FLM adds children only to existing reserved fork carriers. +# All evidence clocks are explicitly MONOTONIC; default/version0 is invalid. +struct LearningControlEvidence @0xf2447170bb68f358 { + version @0 :UInt16; + identity @1 :Text; + generation @2 :UInt64; + controlMonoTime @3 :UInt64; + sourceStateMonoTime @4 :UInt64; + domain @5 :Text; + stageRevision @6 :Text; + responseRevision @7 :Text; + requested @8 :Float64; + features @9 :List(Float64); + baselinePrediction @10 :Float64; + enabled @11 :Bool; + manual @12 :Bool; + active @13 :Bool; + valid @14 :Bool; + override @15 :Bool; + fault @16 :Bool; + saturated @17 :Bool; + limited @18 :Bool; + modelWobble @19 :Bool; + controllerChatter @20 :Bool; + stageSchemaRevision @21 :Text; + stageValues @22 :List(StageValue); + appliedCoordinates @23 :List(Float64); + sourceEpoch @24 :Text; + sourceReceiptMonoTime @25 :UInt64; + controllerMode @26 :Text; + policy @27 :Text; + baselineRevision @28 :Text; + baselineValues @29 :List(Float64); + controllerRevision @30 :Text; + replaySnapshot @31 :List(NamedValue); + replayInputs @32 :List(NamedValue); + snapshotSequence @33 :UInt64; + turnAssist @34 :Bool; + appliedParameterNames @35 :List(Text); + nativeRequired @36 :Bool; + nativeObservedMonoTime @37 :UInt64; + nativeValidUntilMonoTime @38 :UInt64; + nativeAllowed @39 :Bool; + nativeIdentity @40 :Text; + appliedProfileSequence @41 :UInt64; + appliedProfileObservedMonoTime @42 :UInt64; + appliedProfileValidUntilMonoTime @43 :UInt64; + effectTrace @44 :FullEffectTrace; + effectPrepareMonoTime @45 :UInt64; # Exact MONOTONIC owner tick, not carControl publication. + struct FullEffectTrace { + version @0 :UInt16; + envelope @1 :AutomaticRuntimeProfile.FullSurfaceEnvelope; + controlMonoTime @2 :UInt64; + previousMonoTime @3 :UInt64; + qualified @4 :Bool; + before @5 :List(StageValue); + target @6 :List(StageValue); + applied @7 :List(StageValue); + appliedFullProfileSha256 @8 :Text; + } + struct NamedValue { + name @0 :Text; + union { number @1 :Float64; text @2 :Text; boolean @3 :Bool; } + } + struct StageValue { name @0 :Text; value @1 :Float64; } +} +struct LearningAppliedEvidence @0xbab0f51a74ffee6e { + version @0 :UInt16; + controlMonoTime @1 :UInt64; + sourceStateMonoTime @2 :UInt64; + appliedMonoTime @3 :UInt64; + domain @4 :Text; + applied @5 :Float64; + valid @6 :Bool; + active @7 :Bool; + override @8 :Bool; + fault @9 :Bool; + limited @10 :Bool; + nativeAllowed @11 :Bool; + projection @12 :LearningTorqueProjection; + # Carrier continuity only; never a command/physical/native lease timestamp. + carrierFrame @13 :UInt64; + carrierObservedMonoTime @14 :UInt64; +} +struct AutomaticRuntimeProfile @0xef38d302825641e3 { + version @0 :UInt16; + identity @1 :Text; + sequence @2 :UInt64; + observedMonoTime @3 :UInt64; + validUntilMonoTime @4 :UInt64; + policy @5 :Text; + parameters @6 :List(Parameter); + coefficients @7 :List(Float64); + trainIds @8 :List(Text); + heldoutIds @9 :List(Text); + baselineLoss @10 :Float64; + candidateLoss @11 :Float64; + cohortImprovements @12 :List(CohortImprovement); + replay @13 :ReplayValidation; + fullSurface @14 :FullSurfaceEnvelope; + struct FullSurfaceEnvelope { + version @0 :UInt16; + manifestSha256 @1 :Text; + fullOriginalAnchor @2 :Text; + trialAnchor @3 :Text; + contextKey @4 :Text; + cpSha256 @5 :Text; + controllerRevision @6 :Text; + sourceEpoch @7 :Text; + resetGeneration @8 :Text; + selectedBaselineRevision @9 :Text; + priorFullProfileSha256 @10 :Text; + fullProfileSha256 @11 :Text; + priorCoordinates @12 :List(Coordinate); + coordinates @13 :List(Coordinate); + trialNames @14 :List(Text); + persistentSourceEpoch @15 :Text; + operation @16 :Text; + historyFullProfileSha256 @17 :Text; + restoreTransactionSha256 @18 :Text; + struct Coordinate { + name @0 :Text; + value @1 :Float64; + } + } + struct ReplayValidation { + baselineRevision @0 :Text; + physicalModelRevision @1 :Text; + noSaturationRegression @2 :Bool; + noSlewRegression @3 :Bool; + worstCohortImprovement @4 :Float64; + nativeBaselineValues @5 :List(Float64); + } + struct Parameter { + name @0 :Text; + stageRevision @1 :Text; + low @2 :Float64; + high @3 :Float64; + maxStep @4 :Float64; + slewPerS @5 :Float64; + excitationFloor @6 :Float64 = 0.01; + } + struct CohortImprovement { + name @0 :Text; + lowerBound @1 :Float64; + } +} + +struct AutomaticLearningSourceReceipt @0x9fe2d23e75805149 { + version @0 :UInt16; + enabled @1 :Bool; + manual @2 :Bool; + resetGeneration @3 :Text; + sourceEpoch @4 :Text; + observedMonoTime @5 :UInt64; + validUntilMonoTime @6 :UInt64; + cpSha256 @7 :Text; + calibrationBasis @8 :List(Float64); + calibrationGeneration @9 :Text; + persistentSourceEpoch @10 :Text; +} + +struct LearningApplication @0x82b6c8ea6a6efabd { + version @0 :UInt16; + identity @1 :Text; + profileSequence @2 :UInt64; + observedMonoTime @3 :UInt64; + status @4 :Text; + actualCoordinates @5 :List(Float64); + reason @6 :Text; + baselineRevision @7 :Text; + physicalModelRevision @8 :Text; + effectManifestSha256 @9 :Text; + effectOriginalAnchor @10 :Text; + effectAppliedProfileSha256 @11 :Text; +} + + +struct FordLearningEvidence @0xd1dfbadb6a811fee { + version @0 :UInt16; + identity @1 :Text; + generation @2 :UInt64; + sourceEpoch @3 :Text; + baselineRevision @4 :Text; + baselineValues @5 :List(Float64); + controllerRevision @6 :Text; + cpSha256 @7 :Text; + controllerMode @8 :Text; + policy @9 :Text; + conversionRevision @10 :Text; + stageRevision @11 :Text; + sourceReceiptMonoTime @12 :UInt64; + sourceValidUntilMonoTime @13 :UInt64; + enabled @14 :Bool; + manual @15 :Bool; + rows @16 :List(FordLearningEvent); + dropped @17 :UInt64; + appliedProfileSequence @18 :UInt64; + appliedProfileObservedMonoTime @19 :UInt64; + appliedProfileValidUntilMonoTime @20 :UInt64; +} + +struct FordLearningEvent @0xb276dede9e6b2cc4 { + eventMonoTime @0 :UInt64; + sequence @1 :UInt64; + kind @2 :Text; + inputs @3 :List(LearningControlEvidence.NamedValue); + appliedCoefficient @4 :Float64; + actualPending @5 :List(Float64); + actualCommand @6 :List(Float64); + controlMonoTime @7 :UInt64; + pendingControlMonoTime @8 :UInt64; + announcedControlMonoTime @9 :UInt64; + carStateMonoTime @10 :UInt64; + nativeAllowed @11 :Bool; + fault @12 :Bool; + override @13 :Bool; + limited @14 :Bool; + hasWirePath @15 :Bool; + actualWirePath @16 :Float64; + checkpoint @17 :List(LearningControlEvidence.NamedValue); + checkpointSequence @18 :UInt64; + sourceReceiptMonoTime @19 :UInt64; + sourceValidUntilMonoTime @20 :UInt64; + appliedProfileSequence @21 :UInt64; + appliedProfileObservedMonoTime @22 :UInt64; + appliedProfileValidUntilMonoTime @23 :UInt64; + nativeIdentity @24 :Text; + nativeObservedMonoTime @25 :UInt64; + nativeValidUntilMonoTime @26 :UInt64; + nativeValidatedMonoTime @27 :UInt64; + nativeCommandMonoTime @28 :UInt64; + announcedCoefficient @29 :Float64; +} + + +struct CentreApplicationWitness @0xab0b33cac682523e { + version @0 :UInt16; + identity @1 :Text; + sourceEpoch @2 :Text; + controlMonoTime @3 :UInt64; + profileSequence @4 :UInt64; + actualCoordinates @5 :List(Float64); + baselineRevision @6 :Text; + physicalModelRevision @7 :Text; + originalObservedMonoTime @8 :UInt64; + originalValidUntilMonoTime @9 :UInt64; + baselineFriction @10 :Float64; + actualFriction @11 :Float64; +} + +struct LearningTorqueProjection @0xf078f562cbce49ad { + version @0 :UInt16; + family @1 :Text; + fingerprint @2 :Text; + flags @3 :UInt64; + codecId @4 :Text; + mappingKind @5 :Text; + frame @6 :UInt64; + requestedTorque @7 :Float64; + codecInputTorque @8 :Float64; + unlimitedRaw @9 :Int32; + finalRaw @10 :Int32; + effectiveMax @11 :Float64; + lookupX @12 :List(Float64); + lookupY @13 :List(Float64); + limiterKind @14 :Text; + previousTorque @15 :Float64; + measuredTorque @16 :Float64; + limits @17 :List(LearningControlEvidence.StageValue); + shape @18 :List(LearningControlEvidence.NamedValue); + steerRequested @19 :Bool; + canAddress @20 :UInt32; + canBus @21 :UInt16; + canData @22 :Data; + reportedTorque @23 :Float64; + unlimitedRawPresent @24 :Bool; +} diff --git a/openpilot/starpilot/car/tesla/tests/pedal_fixture.py b/openpilot/starpilot/car/tesla/tests/pedal_fixture.py new file mode 100644 index 0000000000..e6a90dd6f2 --- /dev/null +++ b/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('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 diff --git a/openpilot/starpilot/car/tesla/tests/schema_fixture.py b/openpilot/starpilot/car/tesla/tests/schema_fixture.py new file mode 100644 index 0000000000..865a48d452 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_pedal_callers.py b/openpilot/starpilot/car/tesla/tests/test_pedal_callers.py new file mode 100644 index 0000000000..2f1bf19c1b --- /dev/null +++ b/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() diff --git a/openpilot/starpilot/car/tesla/tests/test_pedal_planner.py b/openpilot/starpilot/car/tesla/tests/test_pedal_planner.py new file mode 100644 index 0000000000..3ad4e63faa --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_pedal_schema_evolution.py b/openpilot/starpilot/car/tesla/tests/test_pedal_schema_evolution.py new file mode 100644 index 0000000000..34603362d4 --- /dev/null +++ b/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() diff --git a/openpilot/starpilot/car/tesla/tests/test_preap_adaptive.py b/openpilot/starpilot/car/tesla/tests/test_preap_adaptive.py new file mode 100644 index 0000000000..292acbcf18 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_preap_intent.py b/openpilot/starpilot/car/tesla/tests/test_preap_intent.py index ea75da032d..3d24cd10f5 100644 --- a/openpilot/starpilot/car/tesla/tests/test_preap_intent.py +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_preap_pause_callers.py b/openpilot/starpilot/car/tesla/tests/test_preap_pause_callers.py new file mode 100644 index 0000000000..0fede47d67 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/tests/test_radar_context.py b/openpilot/starpilot/car/tesla/tests/test_radar_context.py new file mode 100644 index 0000000000..334d29af31 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/tests/test_radar_pipeline.py b/openpilot/starpilot/car/tesla/tests/test_radar_pipeline.py new file mode 100644 index 0000000000..4c6ab303e1 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_radar_schema_evolution.py b/openpilot/starpilot/car/tesla/tests/test_radar_schema_evolution.py new file mode 100644 index 0000000000..a075cb7386 --- /dev/null +++ b/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) diff --git a/openpilot/starpilot/car/tesla/tests/test_radar_settings.py b/openpilot/starpilot/car/tesla/tests/test_radar_settings.py new file mode 100644 index 0000000000..ee55534a18 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/car/tesla/tests/test_radar_startup.py b/openpilot/starpilot/car/tesla/tests/test_radar_startup.py new file mode 100644 index 0000000000..062b734447 --- /dev/null +++ b/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() diff --git a/openpilot/starpilot/tests/test_tesla_screen_integration.py b/openpilot/starpilot/tests/test_tesla_screen_integration.py index 5ec6735414..5122bf764b 100644 --- a/openpilot/starpilot/tests/test_tesla_screen_integration.py +++ b/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 diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index fd239ebd46..de0c0cc6bc 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/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: diff --git a/openpilot/starpilot/ui/preap_radar_feature.py b/openpilot/starpilot/ui/preap_radar_feature.py new file mode 100644 index 0000000000..fe5ec5525b --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/ui/tests/test_preap_radar_feature.py b/openpilot/starpilot/ui/tests/test_preap_radar_feature.py new file mode 100644 index 0000000000..2cb4993341 --- /dev/null +++ b/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 diff --git a/openpilot/starpilot/vehicle_preferences.py b/openpilot/starpilot/vehicle_preferences.py index 54fc2ba396..03f946e5b8 100644 --- a/openpilot/starpilot/vehicle_preferences.py +++ b/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) diff --git a/panda/board/main_comms.h b/panda/board/main_comms.h index 08d0963207..661cb8bc5d 100644 --- a/panda/board/main_comms.h +++ b/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: diff --git a/third_party/linux/include/media/cam_isp_vfe.h b/third_party/linux/include/media/cam_isp_vfe.h new file mode 100644 index 0000000000..e48db2f98d --- /dev/null +++ b/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__ */ diff --git a/tools/ci/run_vehicle_tests.py b/tools/ci/run_vehicle_tests.py index 01abb79f76..0e95e6b971 100755 --- a/tools/ci/run_vehicle_tests.py +++ b/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",