"""Default-off parked motion observation; hardwared and sensord retain ownership.""" from collections.abc import Callable import math import signal import threading import time from typing import Any from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.starpilot.parked_evidence import ParkedEvidence, RESUME_SKEW_NS, fresh_parked from openpilot.starpilot.power.offroad_preferences import read_saved as read_power, effective as effective_power from openpilot.starpilot.saved_source import read_saved from openpilot.starpilot.sentry_mode.policy import Inputs, MotionSample, SentryPolicy from openpilot.starpilot.sentry_mode.preferences import read_preferences from openpilot.starpilot.sentry_mode.status import RuntimeStatus PERIPHERAL_TTL_NS = 1_500_000_000 # C++ 2 Hz publisher; one missed frame. SAMPLE_TTL_NS = 500_000_000 TICK_SECONDS = 0.1 def boot_time_ns() -> int: return time.clock_gettime_ns(getattr(time, "CLOCK_BOOTTIME", time.CLOCK_MONOTONIC)) class SentryRuntime: def __init__(self, params: Params, messages: Any, *, mono_clock: Callable[[], int] = time.monotonic_ns, boot_clock: Callable[[], int] = boot_time_ns, record: Callable[[str, int, Callable[[], bool]], Any] | None = None, status: RuntimeStatus | None = None): self.params, self.messages = params, messages self.mono_clock, self.boot_clock = mono_clock, boot_clock self.record = record self.status = status self.policy = SentryPolicy() self.saved_raw: bytes | None = None self.device_after_mono_ns = mono_clock() self.sensor_after_mono_ns = self.device_after_mono_ns self.device_offset_ns: int | None = None self.last_pair_offset_ns: int | None = None self.last_sample_ns = 0 self.recording_state = "idle" def _recording_state(self, state: str) -> None: if state != self.recording_state: self.recording_state = state if state not in ("idle", "stored"): cloudlog.warning(f"sentry motion recording: {state}") def _clocks(self) -> tuple[int, int] | None: before = self.mono_clock() boot = self.boot_clock() after = self.mono_clock() if after < before or after - before > RESUME_SKEW_NS: self.device_after_mono_ns = max(self.device_after_mono_ns, after) self.sensor_after_mono_ns = max(self.sensor_after_mono_ns, after) self.device_offset_ns = None self.policy = SentryPolicy(self.policy.settings) return None offset = boot - (before + after) // 2 if self.last_pair_offset_ns is not None and abs(offset - self.last_pair_offset_ns) > RESUME_SKEW_NS: self.device_after_mono_ns = max(self.device_after_mono_ns, after) self.sensor_after_mono_ns = max(self.sensor_after_mono_ns, after) self.device_offset_ns = None self.policy = SentryPolicy(self.policy.settings) self.last_pair_offset_ns = offset return after, boot def _fresh(self, name: str, *, now_mono_ns: int, now_boot_ns: int, ttl_ns: int) -> bool: sm = self.messages stamp = int(sm.logMonoTime[name]) receipt = int(sm.recv_time[name] * 1e9) if not (sm.seen[name] and sm.alive[name] and sm.valid[name] and stamp > 0 and receipt > 0): return False # C++ pandad stamps BOOTTIME; receipt is SubMaster MONOTONIC. age = now_boot_ns - stamp return 0 <= age <= ttl_ns and 0 <= now_mono_ns - receipt <= ttl_ns def _authority(self) -> tuple[bool, bool, bool, bool, int]: """Return offroad, ignition-off, voltage-safe, device-fresh, current MONO ns.""" clocks = self._clocks() if clocks is None: return False, False, False, False, self.mono_clock() now_ns, boot_ns = clocks sm = self.messages offroad, offroad_readable = read_saved(self.params, "IsOffroad", 8) shutdown, shutdown_readable = read_saved(self.params, "DoShutdown", 8) manager_offroad = offroad_readable and offroad == b"1" and shutdown_readable and shutdown in (None, b"0") try: pandas = sm["pandaStates"] ignition = tuple(bool(p.ignitionLine or p.ignitionCan) for p in pandas) known_pandas = bool(pandas) and all(str(p.pandaType) != "unknown" for p in pandas) ignition_off = known_pandas and not any(ignition) if sm.updated["deviceState"] and int(sm.logMonoTime["deviceState"]) > self.device_after_mono_ns: self.device_offset_ns = self.last_pair_offset_ns evidence = ParkedEvidence( manager_offroad, bool(sm.seen["deviceState"]), bool(sm.alive["deviceState"]), bool(sm.valid["deviceState"]), bool(sm["deviceState"].started), int(sm.logMonoTime["deviceState"]), int(sm.recv_time["deviceState"] * 1e9), self.device_offset_ns, self.device_after_mono_ns, bool(sm.seen["pandaStates"]), bool(sm.alive["pandaStates"]), bool(sm.valid["pandaStates"]), int(sm.logMonoTime["pandaStates"]), int(sm.recv_time["pandaStates"] * 1e9), ignition, ) device_fresh = known_pandas and fresh_parked(evidence, now_mono_ns=now_ns, now_boot_ns=boot_ns) peripheral = sm["peripheralState"] power_saved = read_power(self.params) cutoff_mv = effective_power(power_saved).cutoff_tenths * 100 voltage_ok = (power_saved.readable and power_saved.valid and self._fresh("peripheralState", now_mono_ns=now_ns, now_boot_ns=boot_ns, ttl_ns=PERIPHERAL_TTL_NS) and str(peripheral.pandaType) != "unknown" and int(peripheral.voltage) > cutoff_mv and int(sm["deviceState"].carBatteryCapacityUwh) > 0) return bool(manager_offroad and not sm["deviceState"].started), ignition_off, bool(voltage_ok), device_fresh, now_ns except (AttributeError, KeyError, TypeError, ValueError, OverflowError, OSError, RuntimeError): return bool(manager_offroad), False, False, False, now_ns def _sample(self, now_ns: int) -> MotionSample | None: sm = self.messages try: if not (sm.updated["accelerometer"] and sm.seen["accelerometer"] and sm.alive["accelerometer"] and sm.valid["accelerometer"]): return None event = sm["accelerometer"] stamp = int(event.timestamp) envelope = int(sm.logMonoTime["accelerometer"]) receipt = int(sm.recv_time["accelerometer"] * 1e9) if (stamp <= max(self.last_sample_ns, self.sensor_after_mono_ns) or not 0 < stamp <= envelope <= now_ns or now_ns - stamp > SAMPLE_TTL_NS or now_ns - envelope > SAMPLE_TTL_NS or not 0 <= now_ns - receipt <= SAMPLE_TTL_NS): return None values = tuple(float(v) for v in event.acceleration.v) if len(values) != 3 or any(not math.isfinite(v) for v in values): return None sample = MotionSample(stamp / 1e9, values) self.last_sample_ns = stamp return sample except (AttributeError, KeyError, TypeError, ValueError, OverflowError): return None def _permitted(self, expected_raw: bytes) -> bool: self.messages.update(0) offroad, ignition_off, voltage_ok, fresh, now_ns = self._authority() self._sample(now_ns) saved = read_preferences(self.params) return (saved.readable and saved.valid and saved.preferences.enabled and saved.raw == expected_raw and offroad and ignition_off and voltage_ok and fresh and self.policy.arm_started is not None and self.policy.last_sample_time is not None and 0 <= now_ns - self.last_sample_ns <= SAMPLE_TTL_NS) def tick(self) -> Any: self.messages.update(0) saved = read_preferences(self.params) active = (saved.readable and saved.valid and saved.preferences.enabled) if saved.raw != self.saved_raw: self.policy = SentryPolicy(saved.preferences.settings) self.saved_raw = saved.raw offroad, ignition_off, voltage_ok, fresh, now_ns = self._authority() sample = self._sample(now_ns) if active else None decision = self.policy.update(now_ns / 1e9, Inputs(active, offroad, voltage_ok, fresh, ignition_off, sample)) expected_raw = saved.raw if decision.event is not None and self.record is not None and expected_raw is not None and self._permitted(expected_raw): try: receipt = self.record(decision.event, now_ns, lambda: self._permitted(expected_raw)) self._recording_state("stored" if getattr(receipt, "durable", False) else "durability_unknown") except Exception: # No retry: storage may have published before its durability result failed. self._recording_state("unavailable") if self.status is not None: self.status.publish(decision, saved.raw, self.recording_state) return decision def close(self) -> None: for socket in getattr(self.messages, "sock", {}).values(): close = getattr(socket, "close", None) if close is not None: close() def main() -> None: from openpilot.cereal import messaging from openpilot.starpilot.sentry_mode.storage import EventStore stop = threading.Event() previous = {number: signal.getsignal(number) for number in (signal.SIGINT, signal.SIGTERM)} for number in previous: signal.signal(number, lambda _signal, _frame: stop.set()) messages = messaging.SubMaster(["accelerometer", "deviceState", "pandaStates", "peripheralState"]) store = EventStore() from openpilot.starpilot.galaxy.camera_snapshot import CameraSnapshot, SnapshotUnavailable camera = CameraSnapshot() def record(kind, stamp, permitted): images = {} for name in ("wide", "cabin"): try: images[name] = camera.capture(name, permitted=permitted) except SnapshotUnavailable: if not permitted(): raise return store.record(kind, stamp, permitted=permitted, images=images) runtime = SentryRuntime(Params(), messages, record=record, status=RuntimeStatus()) try: while not stop.is_set(): started = time.monotonic() runtime.tick() stop.wait(max(0.0, TICK_SECONDS - (time.monotonic() - started))) finally: runtime.close() for number, handler in previous.items(): signal.signal(number, handler) if __name__ == "__main__": main()