From fda22037d7ea1d3469b12acd3bdce563809bfced Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 25 Sep 2026 14:53:18 -0500 Subject: [PATCH] Wheel Controls Route controller input and cruise actions through shared transport and mode-action owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> --- openpilot/starpilot/controllers/__init__.py | 0 openpilot/starpilot/controllers/coast.py | 147 ++++++++ .../starpilot/controllers/cruise_action.py | 104 ++++++ openpilot/starpilot/controllers/input.py | 269 ++++++++++++++ .../starpilot/controllers/mode_actions.py | 252 ++++++++++++++ openpilot/starpilot/controllers/owner.py | 328 ++++++++++++++++++ openpilot/starpilot/controllers/selfie.py | 56 +++ .../starpilot/controllers/tests/__init__.py | 0 .../starpilot/controllers/tests/test_input.py | 262 ++++++++++++++ .../controllers/tests/test_mode_actions.py | 232 +++++++++++++ .../starpilot/controllers/tests/test_owner.py | 235 +++++++++++++ .../controllers/tests/test_selfie.py | 126 +++++++ .../controllers/tests/test_transport.py | 122 +++++++ .../starpilot/controllers/toyota_cruise.py | 32 ++ openpilot/starpilot/controllers/transport.py | 256 ++++++++++++++ .../starpilot/controllers/wheel_actions.py | 247 +++++++++++++ .../starpilot/longitudinal/niro_follow.py | 75 ++++ .../tests/test_bolt_acc_pedal_stop_hold.py | 149 ++++++++ 18 files changed, 2892 insertions(+) create mode 100644 openpilot/starpilot/controllers/__init__.py create mode 100644 openpilot/starpilot/controllers/coast.py create mode 100644 openpilot/starpilot/controllers/cruise_action.py create mode 100644 openpilot/starpilot/controllers/input.py create mode 100644 openpilot/starpilot/controllers/mode_actions.py create mode 100644 openpilot/starpilot/controllers/owner.py create mode 100644 openpilot/starpilot/controllers/selfie.py create mode 100644 openpilot/starpilot/controllers/tests/__init__.py create mode 100644 openpilot/starpilot/controllers/tests/test_input.py create mode 100644 openpilot/starpilot/controllers/tests/test_mode_actions.py create mode 100644 openpilot/starpilot/controllers/tests/test_owner.py create mode 100644 openpilot/starpilot/controllers/tests/test_selfie.py create mode 100644 openpilot/starpilot/controllers/tests/test_transport.py create mode 100644 openpilot/starpilot/controllers/toyota_cruise.py create mode 100644 openpilot/starpilot/controllers/transport.py create mode 100644 openpilot/starpilot/controllers/wheel_actions.py create mode 100644 openpilot/starpilot/longitudinal/niro_follow.py create mode 100644 openpilot/starpilot/longitudinal/tests/test_bolt_acc_pedal_stop_hold.py diff --git a/openpilot/starpilot/controllers/__init__.py b/openpilot/starpilot/controllers/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/controllers/coast.py b/openpilot/starpilot/controllers/coast.py new file mode 100644 index 0000000000..e904e6f618 --- /dev/null +++ b/openpilot/starpilot/controllers/coast.py @@ -0,0 +1,147 @@ +import math +from dataclasses import dataclass + + +@dataclass(frozen=True) +class CoastPlan: + force_decel: bool = False + brake_floor: float | None = None + speed_ceiling_mps: float | None = None + pulse_coasting: bool = False + + +class CoastOwner: + def __init__(self): + self.force_coast = False + self.pulse_glide = False + self.coasting = False + self.hill_paused = False + + def reset(self): + self.__init__() + + def toggle(self, action, *, long_active): + if action == 2: + self.force_coast = not self.force_coast + return True + if action == 14 and (long_active or self.pulse_glide): + self.pulse_glide = not self.pulse_glide + return True + return False + + def sample(self, *, admitted, active, gas, brake, speed_mps, target_mps, delta_mps, pitch, + lead_relevant, stop_context): + values = (speed_mps, target_mps, delta_mps) + if (not admitted or not all(type(value) in (int, float) and math.isfinite(value) for value in values) or + speed_mps < 0 or delta_mps < 0 or type(active) is not bool or type(gas) is not bool or + type(brake) is not bool or type(lead_relevant) is not bool or type(stop_context) is not bool): + self.reset() + return CoastPlan() + self.force_coast &= not (gas or brake) + if not active: + return CoastPlan() + ceiling = None + if not self.pulse_glide: + self.coasting = False + self.hill_paused = False + else: + if type(pitch) in (int, float) and math.isfinite(pitch): + if self.hill_paused: + if abs(pitch) <= math.radians(2.5): + self.hill_paused = False + elif abs(pitch) >= math.radians(3.0): + self.hill_paused = True + lower = target_mps - delta_mps + if (self.hill_paused or target_mps <= 5.0 or lower < 3.0 or delta_mps <= 0 or + lead_relevant or stop_context): + self.coasting = False + elif self.coasting: + if speed_mps <= lower + 0.25: + self.coasting = False + elif speed_mps >= target_mps - 0.25: + self.coasting = True + if self.coasting: + ceiling = lower + return CoastPlan(self.force_coast, -0.6 if self.force_coast else -0.03 if self.coasting else None, + ceiling, self.coasting) + + +class CoastRuntime: + def __init__(self): + from openpilot.starpilot.controllers.wheel_actions import WheelConsumer + self.consumer = WheelConsumer() + self.owner = CoastOwner() + self.session = None + self.drive = 0 + self.traffic_toggles = 0 + self.cp_signature = None + self.conditional_stop = False + + def receive(self, event, params, cp, sm, *, now_ns): + command = self.consumer.accept(event, params, cp, sm, now_ns=now_ns) + if command is None: + return False + if command.session != self.session or command.drive_id != self.drive: + self.owner.reset() + self.session, self.drive = command.session, command.drive_id + from openpilot.starpilot.controllers.wheel_actions import cp_fingerprint + self.cp_signature = cp_fingerprint(cp) + if command.action in (2, 6, 14): + from openpilot.starpilot.conditional_mode.ui_action import current_authority + from openpilot.starpilot.saved_source import read_saved + active_required = command.action == 6 or command.action == 14 and not self.owner.pulse_glide + if (not cp.openpilotLongitudinalControl or + read_saved(params, "SafeMode", 8) not in ((None, True), (b"0", True)) or + active_required and not current_authority(sm, cp, command.drive_id, now_ns)): + return False + if command.action == 6: + self.traffic_toggles += 1 + return True + return self.owner.toggle(command.action, long_active=bool(sm["carControl"].longActive)) + return False + + def sample(self, params, cp, sm, *, now_ns, target_mps, lead_relevant, stop_context): + from openpilot.starpilot.controllers.wheel_actions import eligible, LIFETIME_NS, cp_fingerprint, WheelConsumer + from openpilot.starpilot.conditional_mode.ui_action import fresh_service + from openpilot.starpilot.saved_source import read_saved + # A zero accepted-drive/heartbeat cannot satisfy the admission below. + if (type(self) is CoastRuntime and type(getattr(self, 'owner', None)) is CoastOwner and + type(getattr(self, 'consumer', None)) is WheelConsumer and 'sample' not in vars(self.owner) and + type(getattr(self, 'drive', None)) is int and type(getattr(self.consumer, 'last_heartbeat', None)) is int and + (self.drive == 0 or self.consumer.last_heartbeat == 0) and + type(stop_context) is bool and type(getattr(self, 'conditional_stop', None)) is bool): + from capnp.lib.capnp import _DynamicStructReader + from opendbc.car.structs import car + from openpilot.cereal import log, messaging + from openpilot.common.params import Params + if (type(params) is Params and type(sm) is messaging.SubMaster and type(getattr(sm, 'data', None)) is dict and + type(cp) is _DynamicStructReader and cp.schema == car.CarParams.schema): + device = sm.data.get('deviceState') + state = sm.data.get('carState') + control = sm.data.get('carControl') + if (type(device) is _DynamicStructReader and device.schema == log.DeviceState.schema and + type(state) is _DynamicStructReader and state.schema == car.CarState.schema and + type(control) is _DynamicStructReader and control.schema == car.CarControl.schema): + self.owner.reset() + return CoastPlan() + drive = int(sm["deviceState"].startedMonoTime) + raw, readable = read_saved(params, "PulseGlideSpeedDelta", 32) + units, unit_readable = read_saved(params, "IsMetric", 8) + try: + delta = 5.0 if raw is None else float(raw) + except (ValueError, TypeError, OverflowError): + delta = math.nan + admitted = bool(eligible(cp) and self.cp_signature == cp_fingerprint(cp) and cp.openpilotLongitudinalControl and sm["deviceState"].started and + drive == self.drive and 0 < drive < self.consumer.last_heartbeat <= now_ns and + now_ns - self.consumer.last_heartbeat <= LIFETIME_NS and + all(fresh_service(sm, name, drive, now_ns) for name in ("carState", "carControl")) and + sm["carState"].canValid and not sm["carState"].canTimeout and + read_saved(params, "SafeMode", 8) in ((None, True), (b"0", True)) and + readable and unit_readable and units in (None, b"0", b"1") and + math.isfinite(delta) and 0.5 <= delta <= 30.0) + state, control = sm["carState"], sm["carControl"] + pitch = control.orientationNED[1] if len(control.orientationNED) == 3 else None + return self.owner.sample(admitted=admitted, active=bool(control.enabled and control.longActive), + gas=bool(state.gasPressed), brake=bool(state.brakePressed), speed_mps=float(state.vEgo), + target_mps=target_mps, delta_mps=delta * (1 / 3.6 if units == b"1" else 0.44704), + pitch=pitch, lead_relevant=lead_relevant, stop_context=bool(stop_context or self.conditional_stop)) diff --git a/openpilot/starpilot/controllers/cruise_action.py b/openpilot/starpilot/controllers/cruise_action.py new file mode 100644 index 0000000000..7d4d1da114 --- /dev/null +++ b/openpilot/starpilot/controllers/cruise_action.py @@ -0,0 +1,104 @@ +"""Short-lived controller requests; Card owns the software cruise speed.""" + +import math +import re +import secrets + +from openpilot.cereal import messaging +from opendbc.car.structs import car +from openpilot.selfdrive.car.cruise import V_CRUISE_MIN, V_CRUISE_MAX +from openpilot.starpilot.conditional_mode.ui_action import fresh_service + +LIFETIME_NS = 250_000_000 +SESSION = re.compile(r"[0-9a-f]{32}\Z") + + +def eligible(cp) -> bool: + return bool(cp is not None and not cp.pcmCruise and cp.openpilotLongitudinalControl and + not cp.passive and not cp.dashcamOnly and not cp.notCar) + + +def ui_authority(sm, cp, now_ns: int) -> bool: + try: + drive = int(sm["deviceState"].startedMonoTime) + return bool(eligible(cp) and sm["deviceState"].started and + fresh_service(sm, "deviceState", drive, now_ns, 1_000_000_000) and + all(fresh_service(sm, name, drive, now_ns) for name in ("carState", "carControl", "selfdriveState")) and + sm["carState"].canValid and not sm["carState"].canTimeout and + sm["carControl"].enabled and sm["selfdriveState"].enabled) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + +class CruiseActionPublisher: + def __init__(self): + self.session = secrets.token_hex(16) + self.sequence = 0 + + def dispatch(self, increase: bool, sm, cp, publisher, *, now_ns: int, target_mps: float | None = None) -> bool: + if (target_mps is not None and (type(target_mps) not in (int, float) or not math.isfinite(target_mps) or + not V_CRUISE_MIN / 3.6 <= target_mps <= V_CRUISE_MAX / 3.6)) or not ui_authority(sm, cp, now_ns): + return False + self.sequence += 1 + msg = messaging.new_message("slcAction", valid=True) + msg.logMonoTime = now_ns + msg.slcAction.kind = "cruiseSet" if target_mps is not None else "cruiseIncrease" if increase else "cruiseDecrease" + msg.slcAction.controllerCruise = { + "version": 1, "sessionId": self.session, "sequence": self.sequence, + "observedMonoTime": now_ns, "validUntilMonoTime": now_ns + LIFETIME_NS, + "driveStartMonoTime": int(sm["deviceState"].startedMonoTime), + "carFingerprint": cp.carFingerprint, "sourceCarControlMonoTime": int(sm.logMonoTime["carControl"]), + } + if target_mps is not None: + msg.slcAction.controllerCruise.targetSpeedMps = float(target_mps) + publisher.send("slcAction", msg) + return True + + +class CruiseActionConsumer: + def __init__(self): + self.sequences = {} + self.drive = 0 + self.last_apply_ns = 0 + + def apply(self, msg, cp, cs, sm, helper, *, now_ns: int, is_metric: bool, enabled: bool) -> bool: + try: + kind = str(msg.slcAction.kind) + if kind not in ("cruiseIncrease", "cruiseDecrease", "cruiseSet") or not msg.valid: + return False + request = msg.slcAction.controllerCruise + drive = int(sm["deviceState"].startedMonoTime) + if drive != self.drive: + self.sequences.clear() + self.drive = drive + self.last_apply_ns = 0 + session, sequence = str(request.sessionId), int(request.sequence) + observed, expires = int(request.observedMonoTime), int(request.validUntilMonoTime) + source = int(request.sourceCarControlMonoTime) + if not (eligible(cp) and enabled and not cs.buttonEvents and now_ns - self.last_apply_ns >= 100_000_000 and + cs.canValid and not cs.canTimeout and cs.cruiseState.available and + sm["deviceState"].started and fresh_service(sm, "deviceState", drive, now_ns, 1_000_000_000) and + fresh_service(sm, "carControl", drive, now_ns) and sm["carControl"].enabled and + int(request.version) == 1 and int(request.driveStartMonoTime) == drive and + str(request.carFingerprint) == cp.carFingerprint and SESSION.fullmatch(session) and + sequence > self.sequences.get(session, 0) and drive < observed <= now_ns <= expires and + expires - observed <= LIFETIME_NS and int(msg.logMonoTime) == observed and + drive < source <= int(sm.logMonoTime["carControl"]) <= now_ns and now_ns - source <= LIFETIME_NS): + return False + if len(self.sequences) >= 8 and session not in self.sequences: + return False + self.sequences[session] = sequence + self.last_apply_ns = now_ns + if kind == "cruiseSet": + target = float(request.targetSpeedMps) + if not math.isfinite(target) or not V_CRUISE_MIN / 3.6 - 1e-5 <= target <= V_CRUISE_MAX / 3.6 + 1e-5: + return False + previous = helper.v_cruise_kph + helper.apply_slc_target(target, is_metric) + if helper.v_cruise_kph != previous: + helper.slc_cruise_change = (float(previous / 3.6), float(helper.v_cruise_kph / 3.6), 'accel' if helper.v_cruise_kph > previous else 'decel', False) + return True + button = car.CarState.ButtonEvent.Type.accelCruise if kind == "cruiseIncrease" else car.CarState.ButtonEvent.Type.decelCruise + return helper.adjust_v_cruise(button, cs, is_metric) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/controllers/input.py b/openpilot/starpilot/controllers/input.py new file mode 100644 index 0000000000..65f55348b9 --- /dev/null +++ b/openpilot/starpilot/controllers/input.py @@ -0,0 +1,269 @@ +"""Bounded external Linux button observations for a separate action owner.""" + +from dataclasses import dataclass, field +import fcntl +import hashlib +from itertools import islice +import os +from pathlib import Path +import re +import stat +import struct +import time + + +DEV_ROOT = Path("/dev/input") +SYS_ROOT = Path("/sys/class/input") +EVENT_NAME = re.compile(r"event[0-9]+\Z") +MODALIAS = re.compile(r"input:b([0-9a-f]{4})v([0-9a-f]{4})p([0-9a-f]{4})e([0-9a-f]{4})", re.IGNORECASE) +EVENT = struct.Struct("@llHHi") +EV_SYN, EV_KEY, EV_ABS = 0, 1, 3 +SYN_DROPPED = 3 +ABS_HAT0X, ABS_HAT3Y = 16, 23 +HAT_EVENT_BASE = 0x10000 +EXTERNAL_BUSES = frozenset((0x0003, 0x0005)) +RESCAN_NS = 2_000_000_000 +MAX_AGE_NS = 250_000_000 +MAX_DEVICES = 16 +MAX_CANDIDATES = 64 +MAX_EVENTS_DEVICE = 64 +MAX_EVENTS_POLL = 256 +EVIOCSCLOCKID = 0x400445A0 +KEY_STATE_BYTES = 64 +EVIOCGKEY = 0x80004518 | (KEY_STATE_BYTES << 16) +ABS_INFO = struct.Struct("iiiiii") + + +@dataclass(frozen=True) +class Press: + device_id: str + code: int + timestamp_ns: int + + +@dataclass(frozen=True) +class _Identity: + path: Path + device_id: str + name: str + bus: int + + +@dataclass +class _Open: + identity: _Identity + fd: int + armed_keys: set[int] = field(default_factory=set) + pressed_keys: set[int] = field(default_factory=set) + neutral_hats: set[int] = field(default_factory=set) + hat_values: dict[int, int] = field(default_factory=dict) + buffer: bytearray = field(default_factory=bytearray) + + +def _text(path: Path) -> str: + try: + return path.read_text(encoding="utf-8", errors="replace").strip() + except OSError: + return "" + + +def _bits(raw: str) -> set[int]: + try: + words = raw.split() + if not words or any(not re.fullmatch(r"[0-9a-fA-F]{1,16}", word) for word in words): + return set() + width = struct.calcsize("L") * 8 + return {index * width + bit for index, word in enumerate(reversed(words)) + for bit in range(width) if int(word, 16) & (1 << bit)} + except (TypeError, ValueError): + return set() + + +def _button_capable(sysfs: Path) -> bool: + capabilities = sysfs / "capabilities" + keys = _bits(_text(capabilities / "key")) + axes = _bits(_text(capabilities / "abs")) + relative = _bits(_text(capabilities / "rel")) + props = _bits(_text(sysfs / "properties")) + if 1 in props or 2 in props or 3 in props or 0x14A in keys: + return False + if 0 in relative or 1 in relative or any(0x2F <= axis <= 0x3D for axis in axes): + return False + if any(0x110 <= code <= 0x117 for code in keys): + return False + return any(0 < code < 0x110 or 0x120 <= code <= 0x14F for code in keys) or \ + any(ABS_HAT0X <= axis <= ABS_HAT3Y for axis in axes) + + +def _inspect(path: Path) -> _Identity | None: + if not EVENT_NAME.fullmatch(path.name): + return None + sysfs = SYS_ROOT / path.name / "device" + match = MODALIAS.match(_text(sysfs / "modalias")) + if match is None: + return None + bus, vendor, product, _version = (int(token, 16) for token in match.groups()) + if bus not in EXTERNAL_BUSES or not _button_capable(sysfs): + return None + name = _text(sysfs / "name") or "External input" + uniq = _text(sysfs / "uniq") + identity = f"{bus:04x}:{vendor:04x}:{product:04x}:{name.casefold()}:{uniq.casefold()}" + return _Identity(path, hashlib.sha256(identity.encode()).hexdigest()[:20], name[:80], bus) + + +def _fresh(seconds: int, microseconds: int, now_ns: int) -> int | None: + if seconds < 0 or not 0 <= microseconds < 1_000_000: + return None + stamp = seconds * 1_000_000_000 + microseconds * 1_000 + return stamp if 0 <= now_ns - stamp <= MAX_AGE_NS else None + + +def _hat_code(axis: int, value: int) -> int: + return HAT_EVENT_BASE + (axis - ABS_HAT0X) * 2 + int(value > 0) + + +class InputReader: + def __init__(self): + self._opened: dict[Path, _Open] = {} + self._next_scan_ns = 0 + self._closed = False + + def _remove(self, path: Path) -> None: + opened = self._opened.pop(path, None) + if opened is not None: + try: + os.close(opened.fd) + except OSError: + pass + + def _open(self, identity: _Identity) -> None: + flags = os.O_RDONLY | os.O_NONBLOCK | os.O_CLOEXEC | os.O_NOFOLLOW + try: + fd = os.open(identity.path, flags) + try: + if not stat.S_ISCHR(os.fstat(fd).st_mode): + return + fcntl.ioctl(fd, EVIOCSCLOCKID, struct.pack("i", time.CLOCK_MONOTONIC)) + opened = _Open(identity, fd) + try: + state = fcntl.ioctl(fd, EVIOCGKEY, bytes(KEY_STATE_BYTES)) + if len(state) == KEY_STATE_BYTES: + opened.armed_keys = {code for code in range(1, 0x150) if not state[code // 8] & (1 << (code % 8))} + except OSError: + pass + for axis in range(ABS_HAT0X, ABS_HAT3Y + 1): + try: + state = fcntl.ioctl(fd, 0x80184540 + axis, bytes(ABS_INFO.size)) + if len(state) == ABS_INFO.size and ABS_INFO.unpack(state)[0] == 0: + opened.neutral_hats.add(axis) + opened.hat_values[axis] = 0 + except OSError: + pass + self._opened[identity.path] = opened + fd = -1 + finally: + if fd >= 0: + os.close(fd) + except OSError: + return + + def _rescan(self, now_ns: int) -> None: + self._next_scan_ns = now_ns + RESCAN_NS + try: + candidates = list(islice((path for path in DEV_ROOT.glob("event*") if EVENT_NAME.fullmatch(path.name)), MAX_CANDIDATES + 1)) + if len(candidates) > MAX_CANDIDATES: + candidates = [] + else: + candidates.sort() + except OSError: + candidates = [] + identities = [identity for path in candidates if (identity := _inspect(path)) is not None] + counts: dict[str, int] = {} + for identity in identities: + counts[identity.device_id] = counts.get(identity.device_id, 0) + 1 + allowed = {identity.path: identity for identity in identities if counts[identity.device_id] == 1} + for path, opened in list(self._opened.items()): + if path not in allowed or opened.identity != allowed[path]: + self._remove(path) + for path, identity in allowed.items(): + if len(self._opened) >= MAX_DEVICES: + break + if path not in self._opened: + self._open(identity) + + def _decode(self, opened: _Open, raw: bytes, now_ns: int) -> Press | None: + seconds, microseconds, event_type, code, value = EVENT.unpack(raw) + if event_type == EV_SYN and code == SYN_DROPPED: + self._remove(opened.identity.path) + return None + stamp = _fresh(seconds, microseconds, now_ns) + if stamp is None: + return None + if event_type == EV_KEY: + if value == 0: + opened.pressed_keys.discard(code) + opened.armed_keys.add(code) + elif value == 1 and code in opened.armed_keys and code not in opened.pressed_keys: + opened.pressed_keys.add(code) + return Press(opened.identity.device_id, code, stamp) + elif event_type == EV_ABS and ABS_HAT0X <= code <= ABS_HAT3Y and value in (-1, 0, 1): + if value == 0: + opened.neutral_hats.add(code) + opened.hat_values[code] = 0 + elif code in opened.neutral_hats and value != opened.hat_values.get(code): + opened.hat_values[code] = value + return Press(opened.identity.device_id, _hat_code(code, value), stamp) + return None + + def poll(self, now_ns: int) -> list[Press]: + if self._closed: + return [] + if type(now_ns) is not int or now_ns < 0: + raise ValueError("Monotonic timestamp required") + if now_ns >= self._next_scan_ns or now_ns + RESCAN_NS < self._next_scan_ns: + self._rescan(now_ns) + presses: list[Press] = [] + remaining = MAX_EVENTS_POLL + for path, opened in list(self._opened.items()): + if remaining == 0: + self._remove(path) + continue + try: + chunk = os.read(opened.fd, EVENT.size * (min(MAX_EVENTS_DEVICE, remaining) + 1)) + except BlockingIOError: + continue + except OSError: + self._remove(path) + continue + if not chunk: + self._remove(path) + continue + opened.buffer.extend(chunk) + count = len(opened.buffer) // EVENT.size + if count > min(MAX_EVENTS_DEVICE, remaining) or len(opened.buffer) > EVENT.size * MAX_EVENTS_DEVICE: + self._remove(path) + continue + device_presses: list[Press] = [] + for _ in range(count): + raw = bytes(opened.buffer[:EVENT.size]) + del opened.buffer[:EVENT.size] + remaining -= 1 + press = self._decode(opened, raw, now_ns) + if press is not None: + device_presses.append(press) + if path not in self._opened: + break + if path in self._opened: + presses.extend(device_presses) + return presses + + def devices(self) -> list[dict]: + return [{"id": item.identity.device_id, "name": item.identity.name, "bus": item.identity.bus} + for _, item in sorted(self._opened.items())] + + def close(self) -> None: + if self._closed: + return + self._closed = True + for path in list(self._opened): + self._remove(path) diff --git a/openpilot/starpilot/controllers/mode_actions.py b/openpilot/starpilot/controllers/mode_actions.py new file mode 100644 index 0000000000..19e1858a01 --- /dev/null +++ b/openpilot/starpilot/controllers/mode_actions.py @@ -0,0 +1,252 @@ +"""Short-lived requests and per-drive mode intent; existing publishers carry them.""" +from dataclasses import dataclass +import re +import secrets + +from openpilot.cereal import messaging +from openpilot.starpilot.conditional_mode.manual import ioniq6_media_eligible +from openpilot.starpilot.conditional_mode.ui_action import current_authority, fresh_service + +LIFETIME_NS = 250_000_000 +KINDS = {'trafficModeToggle': 'traffic', 'switchbackModeToggle': 'switchback', 'forceCoastToggle': 'coast', + 'pulseGlideToggle': 'pulse', 'disengageRequest': 'disengage'} +SESSION = re.compile(r'[0-9a-f]{32}\Z') + + +def authority(sm, cp, now_ns: int, mode: str) -> bool: + try: + if mode not in ('traffic', 'switchback', 'coast', 'pulse', 'disengage') or cp is None: + return False + drive = int(sm['deviceState'].startedMonoTime) + if mode in ('traffic', 'pulse'): + return current_authority(sm, cp, drive, now_ns) + if mode == 'switchback' and not ioniq6_media_eligible(cp): + return False + if mode == 'disengage' and not fresh_service(sm, 'selfdriveState', drive, now_ns): + return False + if mode == 'coast' and not cp.openpilotLongitudinalControl: + return False + return bool(not cp.passive and not cp.dashcamOnly and not cp.notCar and + 0 < drive < now_ns and sm['deviceState'].started and + fresh_service(sm, 'deviceState', drive, now_ns, 1_000_000_000) and + all(fresh_service(sm, name, drive, now_ns) for name in ('carState', 'carControl')) and + sm['carState'].canValid and not sm['carState'].canTimeout and + (mode == 'coast' or mode == 'disengage' and sm['selfdriveState'].enabled or sm['carControl'].latActive)) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + +class ModeActionPublisher: + def __init__(self): + self.session = secrets.token_hex(16) + self.sequence = 0 + self.last_ns = 0 + + def dispatch(self, mode: str, sm, cp, publisher, *, now_ns: int) -> bool: + if (now_ns - self.last_ns < 100_000_000 or not authority(sm, cp, now_ns, mode) or + mode != 'disengage' and not producer_available(sm, now_ns=now_ns)): + return False + self.sequence += 1 + event = messaging.new_message('slcAction', valid=True) + event.logMonoTime = now_ns + event.slcAction.kind = next(kind for kind, value in KINDS.items() if value == mode) + event.slcAction.controllerMode = { + 'version': 1, 'sessionId': self.session, 'sequence': self.sequence, + 'observedMonoTime': now_ns, 'validUntilMonoTime': now_ns + LIFETIME_NS, + 'driveStartMonoTime': int(sm['deviceState'].startedMonoTime), + 'carFingerprint': cp.carFingerprint, 'sourceCarControlMonoTime': int(sm.logMonoTime['carControl']), + } + publisher.send('slcAction', event) + self.last_ns = now_ns + return True + + +@dataclass(frozen=True) +class ModeIntent: + drive_id: int + requested: bool + effective: bool + + +class ModeActionOwner: + def __init__(self): + self.drive = 0 + self.sequences = {} + self.requested = {'traffic': False, 'switchback': False, 'coast': False, 'pulse': False, 'disengage': False} + self.last_apply_ns = 0 + + def reset(self, drive: int = 0) -> None: + self.drive = drive + self.sequences.clear() + self.requested = {'traffic': False, 'switchback': False, 'coast': False, 'pulse': False, 'disengage': False} + self.last_apply_ns = 0 + + def update(self, event, sm, cp, *, now_ns: int) -> bool: + try: + drive = int(sm['deviceState'].startedMonoTime) if sm['deviceState'].started else 0 + if drive != self.drive: + self.reset(drive) + if event is None or not event.valid or event.which() != 'slcAction': + return False + mode = KINDS.get(str(event.slcAction.kind)) + if mode is None or not authority(sm, cp, now_ns, mode): + return False + wire = event.slcAction.controllerMode + session, sequence = str(wire.sessionId), int(wire.sequence) + observed, expires = int(wire.observedMonoTime), int(wire.validUntilMonoTime) + source = int(wire.sourceCarControlMonoTime) + if (wire.version != 1 or not SESSION.fullmatch(session) or sequence <= self.sequences.get(session, 0) or + len(self.sequences) >= 8 and session not in self.sequences or + int(wire.driveStartMonoTime) != drive or str(wire.carFingerprint) != cp.carFingerprint or + not drive < observed <= now_ns <= expires or expires - observed != LIFETIME_NS or + event.logMonoTime != observed or not drive < source <= int(sm.logMonoTime['carControl']) <= now_ns or + now_ns - source > LIFETIME_NS or now_ns - self.last_apply_ns < 100_000_000): + return False + self.sequences[session] = sequence + self.last_apply_ns = now_ns + self.requested[mode] = not self.requested[mode] + return True + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + def sample(self, mode: str, sm, cp, *, now_ns: int) -> ModeIntent: + drive = int(sm['deviceState'].startedMonoTime) if sm['deviceState'].started else 0 + if drive != self.drive: + self.reset(drive) + requested = self.requested.get(mode, False) + return ModeIntent(drive, requested, requested and authority(sm, cp, now_ns, mode)) + + +class SwitchbackCooldown: + def __init__(self): + self.drive = 0 + self.last = {} + + def allow(self, event: str, *, active: bool, drive_id: int, now_ns: int, cooldown_ns: int) -> bool: + if drive_id != self.drive or not active: + self.drive = drive_id + self.last.clear() + if event not in ('belowSteerSpeed', 'steerSaturated') or not active or cooldown_ns <= 0: + return True + previous = self.last.get(event) + if previous is not None and 0 <= now_ns - previous < cooldown_ns: + return False + self.last[event] = now_ns + return True + + +def publish_switchback(event, intent: ModeIntent, *, session: str, sequence: int, + now_ns: int, source_car_control_ns: int): + if event is None: + event = messaging.new_message('slcState', valid=True) + event.logMonoTime = now_ns + observed_ns = int(event.logMonoTime) + event.slcState.switchbackMode = { + 'version': 1, 'sessionId': session, 'sequence': sequence, + 'observedMonoTime': observed_ns, 'validUntilMonoTime': observed_ns + 100_000_000, + 'driveStartMonoTime': intent.drive_id, 'requested': intent.requested, + 'effective': intent.effective, 'sourceCarControlMonoTime': source_car_control_ns, + } + return event + + +def switchback_observation(event, *, drive_id: int, now_ns: int, event_ns: int | None = None) -> bool: + try: + state = getattr(event, 'slcState', event) + wire = state.switchbackMode + stamp = getattr(event, 'logMonoTime', event_ns) + return bool(getattr(event, 'valid', True) and wire.version == 1 and SESSION.fullmatch(str(wire.sessionId)) and + wire.sequence > 0 and wire.requested and wire.effective and + wire.driveStartMonoTime == drive_id and 0 < drive_id < wire.observedMonoTime <= now_ns <= + wire.validUntilMonoTime <= wire.observedMonoTime + 100_000_000 and + stamp == wire.observedMonoTime and + drive_id < wire.sourceCarControlMonoTime <= wire.observedMonoTime and + now_ns - wire.sourceCarControlMonoTime <= LIFETIME_NS) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + +class SwitchbackStatusOwner: + def __init__(self): + self.drive = 0 + self.last = None + self.retired = () + + def sample(self, event, *, drive_id: int, now_ns: int, event_ns: int | None = None) -> bool: + if drive_id != self.drive or drive_id <= 0: + self.drive, self.last, self.retired = drive_id, None, () + if not switchback_observation(event, drive_id=drive_id, now_ns=now_ns, event_ns=event_ns): + return False + wire = getattr(event, 'slcState', event).switchbackMode + identity = (str(wire.sessionId), int(wire.sequence), int(wire.observedMonoTime)) + if identity[0] in self.retired: + return False + if self.last is not None: + if identity[0] != self.last[0]: + self.retired = (*self.retired[-7:], self.last[0]) + self.last = identity + return False + if identity[1] < self.last[1] or identity[1] == self.last[1] and identity != self.last or identity[1] > self.last[1] and identity[2] <= self.last[2]: + return False + self.last = identity + return True + + +def apply_switchback_gesture(owner, event, *, params, settings, sm, cp, now_ns: int, now_boot_ns: int, mode: str = 'switchback') -> bool: + from openpilot.starpilot.conditional_mode.manual import Button, Gesture, Press, read_button_map + from openpilot.starpilot.conditional_mode.status import settings_fingerprint + try: + drive = int(sm['deviceState'].startedMonoTime) + if owner.drive != drive: + owner.reset(drive) + if mode not in ('traffic', 'switchback') or not authority(sm, cp, now_ns, mode) or not event.valid or str(event.slcCruiseEvent.kind) != mode + 'Mode': + return False + record = event.slcCruiseEvent + wire = record.trafficMode + if not wire.toggle: + return False + snapshot = settings.refresh(now_ns) + verdict = settings.verdict(snapshot, now_mono_ns=now_ns, drive_id=drive) + fingerprint = settings_fingerprint(snapshot) + buttons = read_button_map(params, include_ioniq_media=True) + if verdict.status != 'ready' or verdict.safe_mode is not False or fingerprint is None or buttons is None: + return False + session, sequence = str(wire.sessionId), int(wire.sequence) + observed, expiry = int(wire.observedMonoTime), int(wire.validUntilMonoTime) + source = int(wire.sourceCarStateMonoTime) + physical = int(wire.sourceBootTime) + gesture = Gesture(Button(str(wire.button)), Press(str(wire.press))) + key = 'physical:' + session + if (wire.version != 1 or not SESSION.fullmatch(session) or str(record.producerSessionId) != session or + record.eventId <= 0 or record.observedMonoTime != observed or wire.driveStartMonoTime != drive or + sequence <= owner.sequences.get(key, 0) or len(owner.sequences) >= 8 and key not in owner.sequences or + not drive < observed <= source <= int(sm.logMonoTime['carState']) <= now_ns <= expiry <= source + 100_000_000 or + event.logMonoTime != source or now_ns - observed > LIFETIME_NS or + not 0 < physical <= now_boot_ns <= physical + 300_000_000 or + wire.settingsFingerprint != fingerprint or wire.buttonMapFingerprint != buttons.fingerprint() or + gesture.button not in (Button.MODE, Button.CUSTOM) or buttons.action(gesture) != (7 if mode == 'switchback' else 6) or + now_ns - owner.last_apply_ns < 100_000_000): + return False + owner.sequences[key] = sequence + owner.last_apply_ns = now_ns + owner.requested[mode] = not owner.requested[mode] + return True + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False + + +def producer_available(sm, *, now_ns: int) -> bool: + """An existing fresh mode producer must acknowledge this drive, even while off.""" + try: + drive = int(sm['deviceState'].startedMonoTime) + if not fresh_service(sm, 'slcState', drive, now_ns, 100_000_000): + return False + wire = sm['slcState'].switchbackMode + return bool(wire.version == 1 and SESSION.fullmatch(str(wire.sessionId)) and wire.sequence > 0 and + wire.driveStartMonoTime == drive and + drive < wire.observedMonoTime == int(sm.logMonoTime['slcState']) <= now_ns <= + wire.validUntilMonoTime <= wire.observedMonoTime + 100_000_000 and + drive < wire.sourceCarControlMonoTime <= wire.observedMonoTime and + now_ns - wire.sourceCarControlMonoTime <= LIFETIME_NS) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return False diff --git a/openpilot/starpilot/controllers/owner.py b/openpilot/starpilot/controllers/owner.py new file mode 100644 index 0000000000..f9c5a99804 --- /dev/null +++ b/openpilot/starpilot/controllers/owner.py @@ -0,0 +1,328 @@ +"""Saved controller bindings and guarded physical button dispatch.""" + +import hashlib +import json +import time +from collections.abc import Callable, Mapping +from threading import RLock + +from openpilot.starpilot.favorites.state import FavoriteAction, FavoriteRequest, FavoriteResult, FavoriteSnapshot +from openpilot.starpilot.saved_document import commit_exact +from openpilot.starpilot.saved_source import read_saved + + +CONTROLLER_BINDINGS_PARAM = "StarPilotControllerBindings" +MAX_BYTES = 16384 +MAX_BINDINGS = 64 +SLOT_COUNT = 13 +OWN_SLOT_COUNT = 10 +LEARN_NS = 20_000_000_000 +TEST_NS = 20_000_000_000 +PRESS_AGE_NS = 250_000_000 +MAX_CODE = 65551 + + +def _revision(raw: bytes | None) -> str: + return hashlib.sha256(b"\x00" if raw is None else b"\x01" + raw).hexdigest() + + +def _unique(pairs): + result = {} + for key, value in pairs: + if key in result: + raise ValueError("Duplicate controller field") + result[key] = value + return result + + +def _default_document(): + return {"version": 1, "enabled": False, "slots": [None] * OWN_SLOT_COUNT, "bindings": []} + + +def _text(value, limit=256): + return type(value) is str and 0 < len(value) <= limit and all(char.isprintable() for char in value) + + +def _validate_document(value): + if type(value) is not dict or set(value) != {"version", "enabled", "slots", "bindings"} or value["version"] != 1 or type(value["version"]) is not int: + raise ValueError("Invalid controller document") + if type(value["enabled"]) is not bool or type(value["slots"]) is not list or len(value["slots"]) != OWN_SLOT_COUNT: + raise ValueError("Invalid controller slots") + if any(key is not None and not _text(key, 128) for key in value["slots"]): + raise ValueError("Invalid controller action") + bindings = value["bindings"] + if type(bindings) is not list or len(bindings) > MAX_BINDINGS: + raise ValueError("Invalid controller bindings") + seen = set() + for binding in bindings: + if type(binding) is not dict or set(binding) != {"deviceId", "name", "code", "slot"}: + raise ValueError("Invalid controller binding") + device_id, name, code, slot = (binding[field] for field in ("deviceId", "name", "code", "slot")) + if not _text(device_id) or not _text(name) or type(code) is not int or not 0 <= code <= MAX_CODE or type(slot) is not int or not 0 <= slot < SLOT_COUNT: + raise ValueError("Invalid controller binding") + identity = (device_id, code) + if identity in seen: + raise ValueError("Duplicate controller button") + seen.add(identity) + return value + + +def _read_document(params): + raw, readable = read_saved(params, CONTROLLER_BINDINGS_PARAM, MAX_BYTES) + if not readable: + return _default_document(), raw, False, False + if raw is None: + return _default_document(), raw, True, True + try: + return _validate_document(json.loads(raw, object_pairs_hook=_unique)), raw, True, True + except (ValueError, TypeError, UnicodeError, RecursionError): + return _default_document(), raw, True, False + + +def _field(value, key): + return value.get(key) if isinstance(value, Mapping) else getattr(value, key, None) + + +class ControllerOwner: + def __init__(self, params, *, parked: Callable[[], bool], actions: Callable[[], Mapping[str, FavoriteAction]], + favorites: Callable[[], FavoriteSnapshot], invoke_favorite: Callable[[FavoriteRequest], FavoriteResult], + clock: Callable[[], int] = time.monotonic_ns): + self.params = params + self.parked = parked + self.actions = actions + self.favorites = favorites + self.invoke_favorite = invoke_favorite + self.clock = clock + self._lock = RLock() + self._devices = {} + self._learning: dict | None = None + self._testing_until = 0 + self._last_press = None + self._sequences = {} + self._blocked_before = 0 + + def set_devices(self, devices): + trusted = {} + for device in devices: + device_id = _field(device, "id") + name = _field(device, "name") + bus = _field(device, "bus") + if len(trusted) < 16 and _text(device_id) and _text(name) and type(bus) is int and bus in (3, 5): + trusted[device_id] = {"id": device_id, "name": name, "bus": bus} + with self._lock: + self._devices = trusted + self._sequences = {identity: timestamp for identity, timestamp in self._sequences.items() if identity[0] in trusted} + + def tick(self, now_ns=None): + now = self.clock() if now_ns is None else now_ns + with self._lock: + session_before = self._learning is not None or bool(self._testing_until) + if (self._learning is not None or self._testing_until) and not self.parked(): + self._learning = None + self._testing_until = 0 + if self._learning is not None and now >= self._learning["until"]: + self._learning = None + if self._testing_until and now >= self._testing_until: + self._testing_until = 0 + if session_before and self._learning is None and not self._testing_until: + self._blocked_before = max(self._blocked_before, now) + + def snapshot(self, devices=None): + if devices is not None: + self.set_devices(devices) + now = self.clock() + self.tick(now) + document, raw, readable, valid = _read_document(self.params) + catalog = self.actions() + favorite = self.favorites() + slots = [] + for index in range(SLOT_COUNT): + if index < 3: + source = favorite.slots[index] + slots.append({"index": index, "label": source.label or f"Favorite {index + 1}", "key": source.key, "available": bool(source.available)}) + else: + key = document["slots"][index - 3] + action = catalog.get(key) if key else None + slots.append({"index": index, "label": action.label if action else key or f"Button {index - 2}", + "key": key, "available": bool(action and action.available and action.invoke is not None)}) + options = [{"key": action.key, "label": action.label, "section": action.section} for action in + sorted(catalog.values(), key=lambda item: (item.section.casefold(), item.label.casefold(), item.key))] + with self._lock: + learning = None if self._learning is None else {"slot": self._learning["slot"], + "expiresIn": max(0, (self._learning["until"] - now + 999_999_999) // 1_000_000_000)} + devices_out = sorted(self._devices.values(), key=lambda device: (device["name"].casefold(), device["id"])) + testing = bool(self._testing_until) + last_press = dict(self._last_press) if self._last_press else None + return {"version": 1, "available": True, "editable": bool(readable and self.parked()), + "enabled": document["enabled"] if valid else False, "revision": _revision(raw), "valid": valid, + "devices": devices_out, "slots": slots, "options": options, "bindings": document["bindings"] if valid else [], + "learning": learning, "testing": testing, "lastPress": last_press} + + def _save(self, document, raw): + encoded = json.dumps(_validate_document(document), separators=(",", ":"), allow_nan=False).encode() + if len(encoded) > MAX_BYTES: + raise ValueError("Controller bindings are too large") + result = commit_exact(self.params, key=CONTROLLER_BINDINGS_PARAM, max_bytes=MAX_BYTES, raw=encoded, + expected=raw, authorized=self.parked, temp_prefix=".controller-bindings-") + if not result.committed or not result.verified: + raise RuntimeError("Controller bindings changed or could not be saved") + + def action(self, payload): + if type(payload) is not dict or type(payload.get("operation")) is not str: + raise ValueError("Invalid controller request") + operation = payload["operation"] + fields = {"save": {"operation", "revision", "enabled", "slots"}, + "learn": {"operation", "revision", "slot"}, "cancel": {"operation"}, + "test": {"operation", "enabled"}, "remove": {"operation", "revision", "deviceId", "code"}} + if operation not in fields or set(payload) != fields[operation]: + raise ValueError("Invalid controller request") + if not self.parked(): + raise PermissionError("Turn off the vehicle to edit Controller Buttons") + with self._lock: + self.tick() + if operation == "cancel": + self._blocked_before = max(self._blocked_before, self.clock()) + self._learning = None + self._testing_until = 0 + elif operation == "test": + if type(payload["enabled"]) is not bool: + raise ValueError("Invalid test state") + self._blocked_before = max(self._blocked_before, self.clock()) + self._learning = None + self._testing_until = self.clock() + TEST_NS if payload["enabled"] else 0 + else: + document, raw, readable, valid = _read_document(self.params) + if not readable: + raise OSError("Controller bindings unavailable") + if type(payload["revision"]) is not str or payload["revision"] != _revision(raw): + raise RuntimeError("Controller bindings changed. Reload before editing") + if operation == "learn": + slot = payload["slot"] + if type(slot) is not int or not 0 <= slot < SLOT_COUNT: + raise ValueError("Invalid controller slot") + if not valid: + raise ValueError("Repair controller bindings before learning") + self._testing_until = 0 + started = self.clock() + self._learning = {"slot": slot, "revision": _revision(raw), "started": started, "until": started + LEARN_NS} + elif operation == "save": + enabled, slots = payload["enabled"], payload["slots"] + if type(enabled) is not bool or type(slots) is not list or len(slots) != OWN_SLOT_COUNT: + raise ValueError("Invalid controller slots") + if any(key is not None and not _text(key, 128) for key in slots): + raise ValueError("Invalid controller action") + catalog = self.actions() + previous = document["slots"] if valid else [None] * OWN_SLOT_COUNT + for index, key in enumerate(slots): + if key is not None and key not in catalog and key != previous[index]: + raise ValueError("Unsupported controller action") + self._save({"version": 1, "enabled": enabled, "slots": slots, + "bindings": document["bindings"] if valid else []}, raw) + self._blocked_before = max(self._blocked_before, self.clock()) + self._learning = None + self._testing_until = 0 + else: + device_id, code = payload["deviceId"], payload["code"] + if not _text(device_id) or type(code) is not int or not 0 <= code <= MAX_CODE: + raise ValueError("Invalid controller button") + if not valid: + raise ValueError("Repair controller bindings before removing") + bindings = [binding for binding in document["bindings"] if (binding["deviceId"], binding["code"]) != (device_id, code)] + if len(bindings) == len(document["bindings"]): + raise ValueError("Controller button is not assigned") + self._save({**document, "bindings": bindings}, raw) + self._blocked_before = max(self._blocked_before, self.clock()) + self._learning = None + return self.snapshot() + + def _press_status(self, device_id, code, slot, executed, message): + with self._lock: + self._last_press = {"deviceId": device_id, "code": code, "slot": slot, "executed": executed, "message": message} + + def feed(self, press): + device_id, code = _field(press, "device_id"), _field(press, "code") + timestamp = _field(press, "timestamp_ns") + now = self.clock() + if not _text(device_id) or type(code) is not int or not 0 <= code <= MAX_CODE or type(timestamp) is not int or not 0 <= now - timestamp <= PRESS_AGE_NS: + return + with self._lock: + self.tick(now) + identity = (device_id, code) + if device_id not in self._devices or timestamp <= self._blocked_before or timestamp <= self._sequences.get(identity, -1): + return + self._sequences[identity] = timestamp + if len(self._sequences) > 256: + self._sequences = {key: seen for key, seen in self._sequences.items() if seen >= now - PRESS_AGE_NS} + learning = self._learning + testing = bool(self._testing_until) + if learning is not None: + if timestamp < learning["started"]: + return + if not self.parked(): + with self._lock: + self._learning = None + return + document, raw, readable, valid = _read_document(self.params) + if not readable or not valid or _revision(raw) != learning["revision"]: + with self._lock: + self._learning = None + return + with self._lock: + if self._learning is not learning or device_id not in self._devices or not self.parked(): + return + binding = {"deviceId": device_id, "name": self._devices[device_id]["name"], "code": code, "slot": learning["slot"]} + bindings = [item for item in document["bindings"] if (item["deviceId"], item["code"]) != (device_id, code)] + bindings.append(binding) + try: + self._save({**document, "bindings": bindings}, raw) + except (OSError, RuntimeError, ValueError): + self._press_status(device_id, code, learning["slot"], False, "Button could not be learned") + return + if self._learning is learning: + self._learning = None + self._press_status(device_id, code, learning["slot"], False, "Button learned") + return + document, raw, readable, valid = _read_document(self.params) + binding = next((item for item in document["bindings"] if (item["deviceId"], item["code"]) == (device_id, code)), None) if valid else None + slot = binding["slot"] if binding else None + if testing: + self._press_status(device_id, code, slot, False, "Test press detected") + return + if not readable or not valid or not document["enabled"] or binding is None or slot is None: + return + if slot < 3: + favorite = self.favorites().slots[slot] + request = favorite.request + if not favorite.available or request is None: + self._press_status(device_id, code, slot, False, "Quick Select unavailable") + return + with self._lock: + if self._learning is not None or self._testing_until: + return + current, fresh = read_saved(self.params, CONTROLLER_BINDINGS_PARAM, MAX_BYTES) + renewed = self.favorites().slots[slot] + if not fresh or current != raw or not renewed.available or renewed.request != request: + self._press_status(device_id, code, slot, False, "Control changed") + return + result = self.invoke_favorite(request) + self._press_status(device_id, code, slot, result.success, result.message) + return + key = document["slots"][slot - 3] + first = self.actions().get(key) if key else None + if first is None or not first.available or first.invoke is None: + self._press_status(device_id, code, slot, False, "Control unavailable") + return + with self._lock: + if self._learning is not None or self._testing_until: + return + current, fresh = read_saved(self.params, CONTROLLER_BINDINGS_PARAM, MAX_BYTES) + second = self.actions().get(key) + if (not fresh or current != raw or second is None or not second.available or second.invoke is None or + second.key != first.key or second.token != first.token): + self._press_status(device_id, code, slot, False, "Control changed") + return + try: + success = bool(second.invoke()) + except (OSError, RuntimeError, ValueError, TypeError): + success = False + self._press_status(device_id, code, slot, success, second.label if success else "Control unavailable") diff --git a/openpilot/starpilot/controllers/selfie.py b/openpilot/starpilot/controllers/selfie.py new file mode 100644 index 0000000000..34757b3234 --- /dev/null +++ b/openpilot/starpilot/controllers/selfie.py @@ -0,0 +1,56 @@ +import threading +import time + +from openpilot.starpilot.conditional_mode.ui_action import fresh_service +from openpilot.starpilot.controllers.wheel_actions import cp_fingerprint, eligible +from openpilot.starpilot.galaxy.camera_snapshot import CameraSnapshot, SnapshotUnavailable +from openpilot.starpilot.sentry_mode.storage import EventStore, StorageUnavailable + + +def authority(sm, cp, *, drive_id, fingerprint, now_ns): + try: + return bool(eligible(cp) and cp_fingerprint(cp) == fingerprint and + fresh_service(sm, 'deviceState', drive_id, now_ns, 1_000_000_000) and + sm['deviceState'].started and int(sm['deviceState'].startedMonoTime) == drive_id and + fresh_service(sm, 'carState', drive_id, now_ns) and + sm['carState'].canValid and not sm['carState'].canTimeout) + except (AttributeError, KeyError, TypeError, ValueError, RuntimeError): + return False + + +class SelfieCapture: + def __init__(self, *, camera=None, store=None, clock=time.monotonic_ns): + self.camera = camera if camera is not None else CameraSnapshot() + self.store = store if store is not None else EventStore() + self.clock = clock + self.lock = threading.Lock() + self.last_result = None + self.worker = None + + @property + def busy(self): + return self.lock.locked() + + def submit(self, permitted): + if not permitted() or not self.lock.acquire(blocking=False): + return False + self.last_result = None + try: + self.worker = threading.Thread(target=self._capture, args=(permitted,), daemon=True) + self.worker.start() + except RuntimeError: + self.lock.release() + return False + return True + + def _capture(self, permitted): + try: + image = self.camera.capture('cabin', permitted=permitted) + if not permitted(): + return + receipt = self.store.record('selfie', self.clock(), permitted=permitted, images={'cabin': image}) + self.last_result = receipt + except (SnapshotUnavailable, StorageUnavailable, OSError, ValueError): + self.last_result = None + finally: + self.lock.release() diff --git a/openpilot/starpilot/controllers/tests/__init__.py b/openpilot/starpilot/controllers/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/starpilot/controllers/tests/test_input.py b/openpilot/starpilot/controllers/tests/test_input.py new file mode 100644 index 0000000000..297e671684 --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_input.py @@ -0,0 +1,262 @@ +import hashlib +import os +from pathlib import Path +import stat +import struct +from types import SimpleNamespace + +import pytest + +from openpilot.starpilot.controllers import input as source + + +NOW = 10_000_000_000 + + +def bitmap(codes): + width = struct.calcsize("L") * 8 + words = [0] * (max(codes, default=0) // width + 1) + for code in codes: + words[code // width] |= 1 << (code % width) + return " ".join(f"{word:x}" for word in reversed(words)) + + +def event(typ, code, value, stamp=NOW): + return source.EVENT.pack(stamp // 1_000_000_000, stamp % 1_000_000_000 // 1_000, typ, code, value) + + +class Kernel: + def __init__(self, monkeypatch, tmp_path): + self.dev = tmp_path / "dev" + self.sys = tmp_path / "sys" + self.dev.mkdir() + self.sys.mkdir() + self.next_fd = 100 + self.queue = {} + self.path_by_fd = {} + self.closed = [] + self.fail_read = set() + self.clocks = [] + self.held_keys = {} + self.hats = {} + self.unsupported_clock = set() + self.real_open = os.open + self.real_read = os.read + self.real_close = os.close + self.real_fstat = os.fstat + monkeypatch.setattr(source, "DEV_ROOT", self.dev) + monkeypatch.setattr(source, "SYS_ROOT", self.sys) + monkeypatch.setattr(source.os, "open", self.open) + monkeypatch.setattr(source.os, "read", self.read) + monkeypatch.setattr(source.os, "close", self.close) + monkeypatch.setattr(source.os, "fstat", self.fstat) + monkeypatch.setattr(source.fcntl, "ioctl", self.ioctl) + + def device(self, number, *, bus=3, vendor=0x1234, product=0x5678, name="Pad", uniq="01", keys=(304,), axes=(), rel=(), props=()): + path = self.dev / f"event{number}" + path.touch() + root = self.sys / path.name / "device" + (root / "capabilities").mkdir(parents=True) + (root / "modalias").write_text(f"input:b{bus:04X}v{vendor:04X}p{product:04X}e0001") + (root / "name").write_text(name) + (root / "uniq").write_text(uniq) + for kind, codes in (("key", keys), ("abs", axes), ("rel", rel)): + (root / "capabilities" / kind).write_text(bitmap(codes)) + (root / "properties").write_text(bitmap(props)) + self.queue[path] = bytearray() + self.held_keys[path] = set() + self.hats[path] = {} + return path + + def push(self, path, *events): + self.queue[path].extend(b"".join(events)) + + def open(self, path, flags, mode=0o777, *, dir_fd=None): + if Path(path) not in self.queue: + return self.real_open(path, flags, mode, dir_fd=dir_fd) + assert flags & os.O_NONBLOCK and flags & os.O_NOFOLLOW and flags & os.O_CLOEXEC + assert Path(path) in self.queue + self.next_fd += 1 + self.path_by_fd[self.next_fd] = Path(path) + return self.next_fd + + def read(self, fd, count): + if fd not in self.path_by_fd: + return self.real_read(fd, count) + if fd in self.fail_read: + raise OSError("disconnected") + data = self.queue[self.path_by_fd[fd]] + if not data: + raise BlockingIOError() + chunk = bytes(data[:count]) + del data[:count] + return chunk + + def close(self, fd): + if fd not in self.path_by_fd: + return self.real_close(fd) + self.closed.append(fd) + self.path_by_fd.pop(fd, None) + + def fstat(self, fd): + return SimpleNamespace(st_mode=stat.S_IFCHR) if fd in self.path_by_fd else self.real_fstat(fd) + + def ioctl(self, fd, code, value): + if code == source.EVIOCSCLOCKID: + if self.path_by_fd[fd] in self.unsupported_clock: + raise OSError("clock unavailable") + assert struct.unpack("i", value)[0] == source.time.CLOCK_MONOTONIC + self.clocks.append(fd) + elif code == source.EVIOCGKEY: + state = bytearray(source.KEY_STATE_BYTES) + for key in self.held_keys[self.path_by_fd[fd]]: + state[key // 8] |= 1 << (key % 8) + return bytes(state) + elif 0x80184540 + 16 <= code <= 0x80184540 + 23: + axis = code - 0x80184540 + value = self.hats[self.path_by_fd[fd]].get(axis, 0) + return source.ABS_INFO.pack(value, 0, 0, 0, 0, 0) + else: + raise AssertionError(code) + + +@pytest.fixture +def kernel(monkeypatch, tmp_path): + return Kernel(monkeypatch, tmp_path) + + +def test_external_identity_capability_and_duplicates(kernel): + kernel.device(0, bus=5, name="Road Pad", uniq="ABC") + kernel.device(1, bus=0x18, name="Internal") + kernel.device(2, keys=(0x110,), name="Mouse") + kernel.device(3, keys=(0x14A,), name="Touch") + kernel.device(4, keys=(115,), name="Media keys") + kernel.device(6, keys=(115,), rel=(0, 1), name="Mouse with media") + kernel.device(7, keys=(115,), axes=(0x35,), name="Touch with media") + kernel.device(8, keys=(115,), props=(1,), name="Direct touch") + reader = source.InputReader() + reader.poll(NOW) + devices = reader.devices() + expected = hashlib.sha256(b"0005:1234:5678:road pad:abc").hexdigest()[:20] + assert devices == [{"id": expected, "name": "Road Pad", "bus": 5}, + {"id": devices[1]["id"], "name": "Media keys", "bus": 3}] + assert len(kernel.clocks) == 2 + duplicate = kernel.device(5, bus=5, name="Road Pad", uniq="ABC") + reader.poll(NOW + source.RESCAN_NS) + assert [item["name"] for item in reader.devices()] == ["Media keys"] + assert kernel.closed + duplicate.unlink() + reader.poll(NOW + source.RESCAN_NS * 2) + assert [item["name"] for item in reader.devices()] == ["Road Pad", "Media keys"] + reader.close() + assert len(kernel.closed) == len(kernel.clocks) + assert reader.poll(NOW + source.RESCAN_NS * 3) == [] + + +def test_release_required_repeats_and_fresh_timestamps(kernel): + path = kernel.device(0) + kernel.held_keys[path].add(304) + reader = source.InputReader() + reader.poll(NOW) + kernel.push(path, event(1, 304, 1), event(1, 304, 2), event(1, 304, 0), event(1, 304, 1)) + assert reader.poll(NOW) == [source.Press(reader.devices()[0]["id"], 304, NOW)] + kernel.push(path, event(1, 304, 1), event(1, 304, 2)) + assert reader.poll(NOW) == [] + kernel.push(path, event(1, 304, 0, NOW - 300_000_000), event(1, 304, 1)) + assert reader.poll(NOW) == [] + kernel.push(path, event(1, 304, 0, NOW + 1_000_000), event(1, 304, 0), event(1, 304, 1)) + assert reader.poll(NOW) == [source.Press(reader.devices()[0]["id"], 304, NOW)] + reader.close() + + +def test_initial_neutral_key_accepts_first_press(kernel): + path = kernel.device(0) + reader = source.InputReader() + reader.poll(NOW) + kernel.push(path, event(1, 304, 1)) + assert reader.poll(NOW) == [source.Press(reader.devices()[0]["id"], 304, NOW)] + reader.close() + + +def test_unsupported_monotonic_clock_never_opens_device(kernel): + path = kernel.device(0) + kernel.unsupported_clock.add(path) + reader = source.InputReader() + kernel.push(path, event(1, 304, 1)) + assert reader.poll(NOW) == [] + assert reader.devices() == [] + assert kernel.closed == [101] + reader.close() + + +def test_hats_need_neutral_and_emit_edges(kernel): + path = kernel.device(0, keys=(), axes=(16,)) + kernel.hats[path][16] = 1 + reader = source.InputReader() + reader.poll(NOW) + kernel.push(path, event(3, 16, 1), event(3, 16, 0), event(3, 16, 1), event(3, 16, 1), event(3, 16, -1)) + identity = reader.devices()[0]["id"] + assert reader.poll(NOW) == [source.Press(identity, 0x10001, NOW), source.Press(identity, 0x10000, NOW)] + reader.close() + + +def test_dropped_and_overflow_close_until_reopened_and_neutral(kernel): + path = kernel.device(0) + kernel.held_keys[path].add(304) + reader = source.InputReader() + reader.poll(NOW) + kernel.push(path, event(1, 304, 0), event(1, 304, 1), event(0, 3, 0), event(1, 304, 1)) + assert reader.poll(NOW) == [] + assert reader.devices() == [] + reader.poll(NOW + source.RESCAN_NS) + assert reader.devices() + kernel.held_keys[path].add(304) + assert reader.poll(NOW + source.RESCAN_NS) == [] + kernel.push(path, event(1, 304, 0, NOW + source.RESCAN_NS), event(1, 304, 1, NOW + source.RESCAN_NS)) + assert len(reader.poll(NOW + source.RESCAN_NS)) == 1 + kernel.push(path, *(event(1, 304, 0, NOW + source.RESCAN_NS) for _ in range(65))) + assert reader.poll(NOW + source.RESCAN_NS) == [] + assert reader.devices() == [] + reader.close() + + +def test_one_device_error_and_disconnect_do_not_block_other(kernel): + a = kernel.device(0, name="A") + b = kernel.device(1, name="B") + reader = source.InputReader() + reader.poll(NOW) + kernel.push(a, event(1, 304, 0), event(1, 304, 1)) + kernel.push(b, event(1, 304, 0), event(1, 304, 1)) + fd_a = next(fd for fd, path in kernel.path_by_fd.items() if path == a) + kernel.fail_read.add(fd_a) + assert [item["name"] for item in reader.devices()] == ["A", "B"] + assert [press.device_id for press in reader.poll(NOW)] == [reader.devices()[0]["id"]] + assert [item["name"] for item in reader.devices()] == ["B"] + b.unlink() + reader.poll(NOW + source.RESCAN_NS) + assert reader.devices() == [{"id": reader.devices()[0]["id"], "name": "A", "bus": 3}] + reader.close() + + +def test_per_device_and_global_event_limits(kernel): + paths = [kernel.device(index, name=f"Pad {index}", uniq=str(index)) for index in range(5)] + reader = source.InputReader() + reader.poll(NOW) + for path in paths: + kernel.push(path, *(event(1, 304, value) for _ in range(32) for value in (0, 1))) + assert len(reader.poll(NOW)) == 128 + assert len(reader.devices()) == 4 + reader.close() + + +def test_device_and_scan_candidate_caps(kernel): + for index in range(17): + kernel.device(index, name=f"Pad {index}", uniq=str(index)) + reader = source.InputReader() + reader.poll(NOW) + assert len(reader.devices()) == source.MAX_DEVICES + for index in range(17, source.MAX_CANDIDATES + 1): + kernel.device(index, name=f"Pad {index}", uniq=str(index)) + reader.poll(NOW + source.RESCAN_NS) + assert reader.devices() == [] + reader.close() diff --git a/openpilot/starpilot/controllers/tests/test_mode_actions.py b/openpilot/starpilot/controllers/tests/test_mode_actions.py new file mode 100644 index 0000000000..7385718284 --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_mode_actions.py @@ -0,0 +1,232 @@ +from types import SimpleNamespace as NS +from unittest.mock import patch + +from openpilot.cereal import messaging +from openpilot.starpilot.controllers.mode_actions import ModeActionOwner, SwitchbackCooldown + +NOW = 10_000_000_000 +DRIVE = 1_000_000_000 + + +def fixture(): + class SM(dict): + pass + sm = SM(deviceState=NS(started=True, startedMonoTime=DRIVE), + carState=NS(canValid=True, canTimeout=False), + carControl=NS(enabled=True, longActive=True, latActive=True), + selfdriveState=NS(enabled=True)) + names = tuple(sm) + sm.seen = dict.fromkeys(names, True) + sm.valid = dict.fromkeys(names, True) + sm.alive = dict.fromkeys(names, True) + sm.logMonoTime = dict.fromkeys(names, NOW - 10_000_000) + sm.recv_time = dict.fromkeys(names, (NOW - 5_000_000) / 1e9) + cp = NS(carFingerprint='qualified-fixture', openpilotLongitudinalControl=True, + passive=False, dashcamOnly=False, notCar=False) + return sm, cp + + +def request(sm, cp, kind='trafficModeToggle', sequence=1, now=NOW): + msg = messaging.new_message('slcAction', valid=True) + msg.logMonoTime = now + msg.slcAction.kind = kind + msg.slcAction.controllerMode = {'version': 1, 'sessionId': 'a' * 32, 'sequence': sequence, + 'observedMonoTime': now, 'validUntilMonoTime': now + 250_000_000, + 'driveStartMonoTime': sm['deviceState'].startedMonoTime, + 'carFingerprint': cp.carFingerprint, 'sourceCarControlMonoTime': sm.logMonoTime['carControl']} + return msg + + +def test_request_replay_expiry_wrong_vehicle_and_drive_never_toggle(): + sm, cp = fixture() + owner = ModeActionOwner() + msg = request(sm, cp) + with patch('openpilot.starpilot.controllers.mode_actions.ioniq6_media_eligible', return_value=True): + assert owner.update(msg, sm, cp, now_ns=NOW) + assert not owner.update(msg, sm, cp, now_ns=NOW + 100_000_000) + assert owner.requested['traffic'] + assert not owner.update(request(sm, cp, sequence=2), sm, cp, now_ns=NOW + 251_000_000) + changed = request(sm, cp, sequence=3) + changed.slcAction.controllerMode.carFingerprint = 'other' + assert not owner.update(changed, sm, cp, now_ns=NOW + 100_000_000) + sm['deviceState'].startedMonoTime = DRIVE + 1 + assert not owner.update(msg, sm, cp, now_ns=NOW + 100_000_000) + assert not owner.requested['traffic'] + + +def test_temporary_authority_loss_pauses_effective_intent_without_stale_display(): + sm, cp = fixture() + owner = ModeActionOwner() + with patch('openpilot.starpilot.controllers.mode_actions.ioniq6_media_eligible', return_value=True): + assert owner.update(request(sm, cp), sm, cp, now_ns=NOW) + assert owner.sample('traffic', sm, cp, now_ns=NOW).effective + sm['carControl'].longActive = False + assert owner.sample('traffic', sm, cp, now_ns=NOW).requested + assert not owner.sample('traffic', sm, cp, now_ns=NOW).effective + sm['carControl'].longActive = True + assert owner.sample('traffic', sm, cp, now_ns=NOW).effective + sm['deviceState'].started = False + assert not owner.sample('traffic', sm, cp, now_ns=NOW).requested + + +def test_switchback_allows_qualified_lateral_only_but_unknown_vehicle_denied(): + sm, cp = fixture() + owner = ModeActionOwner() + sm['carControl'].longActive = False + sm['selfdriveState'].enabled = False + cp.openpilotLongitudinalControl = False + with patch('openpilot.starpilot.controllers.mode_actions.ioniq6_media_eligible', return_value=True): + assert owner.update(request(sm, cp, 'switchbackModeToggle'), sm, cp, now_ns=NOW) + assert owner.sample('switchback', sm, cp, now_ns=NOW).effective + assert not owner.update(request(sm, cp, sequence=2), sm, cp, now_ns=NOW + 100_000_000) + with patch('openpilot.starpilot.controllers.mode_actions.ioniq6_media_eligible', return_value=False): + assert not ModeActionOwner().update(request(sm, cp), sm, cp, now_ns=NOW) + + +def test_switchback_only_rate_limits_two_advisories_and_resets_per_drive(): + owner = SwitchbackCooldown() + def allow(event, now, drive=DRIVE, active=True): + return owner.allow(event, active=active, drive_id=drive, now_ns=now, cooldown_ns=300_000_000_000) + assert allow('steerSaturated', NOW) + assert not allow('steerSaturated', NOW + 1) + assert allow('belowSteerSpeed', NOW + 1) + assert allow('controlsMismatch', NOW + 1) + assert allow('steerUnavailable', NOW + 1) + assert allow('steerSaturated', NOW + 300_000_000_000) + assert allow('steerSaturated', NOW + 300_000_000_001, drive=DRIVE + 1) + assert allow('steerSaturated', NOW + 300_000_000_002, active=False) + + +def test_actual_alpha_off_wheel_switchback_mapping_and_replay_denial(): + from openpilot.starpilot.conditional_mode.tests import test_traffic as fixtures + from openpilot.starpilot.conditional_mode.manual import read_button_map + from openpilot.starpilot.conditional_mode.status import settings_fingerprint + from openpilot.starpilot.controllers.mode_actions import apply_switchback_gesture + f = fixtures.TrafficOwnerTests('runTest') + f.setUp() + try: + f.params.put('ModeButtonControl', 7, block=True) + f.cp.openpilotLongitudinalControl = False + f.sm['deviceState'].startedMonoTime = fixtures.DRIVE + f.sm['carControl'].latActive = True + f.sm['carControl'].enabled = False + f.sm['carControl'].longActive = False + f.sm['selfdriveState'].enabled = False + msg = messaging.new_message('slcCruiseEvent', valid=True) + stamp = fixtures.NOW - 5_000_000 + msg.logMonoTime = stamp + record = msg.slcCruiseEvent + record.kind, record.eventId = 'switchbackMode', 1 + record.producerSessionId, record.observedMonoTime = 'b' * 32, stamp + record.trafficMode = {'version': 1, 'sessionId': 'b' * 32, 'sequence': 1, + 'observedMonoTime': stamp, 'validUntilMonoTime': stamp + 100_000_000, + 'driveStartMonoTime': fixtures.DRIVE, 'sourceCarStateMonoTime': stamp, + 'settingsFingerprint': settings_fingerprint(f.settings.refresh(fixtures.NOW)), + 'buttonMapFingerprint': read_button_map(f.params, include_ioniq_media=True).fingerprint(), + 'sourceEpoch': 0, 'sourceBootTime': fixtures.BOOT - 5_000_000, + 'toggle': True, 'button': 'mode', 'press': 'short'} + owner = ModeActionOwner() + assert apply_switchback_gesture(owner, msg, params=f.params, settings=f.settings, sm=f.sm, + cp=f.cp, now_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT) + assert owner.sample('switchback', f.sm, f.cp, now_ns=fixtures.NOW).effective + assert not apply_switchback_gesture(owner, msg, params=f.params, settings=f.settings, sm=f.sm, + cp=f.cp, now_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT) + f.params.put('ModeButtonControl', 0, block=True) + assert not apply_switchback_gesture(ModeActionOwner(), msg, params=f.params, settings=f.settings, sm=f.sm, + cp=f.cp, now_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT) + finally: + f.doCleanups() + + +def test_controller_traffic_without_media_assignment_uses_existing_qualified_profile_owner(): + from openpilot.starpilot.conditional_mode.tests import test_traffic as fixtures + f = fixtures.TrafficOwnerTests('runTest') + f.setUp() + try: + verdict = f.owner.sample(None, params=f.params, settings=f.settings, sm=f.sm, cp=f.cp, + drive_id=fixtures.DRIVE, now_mono_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT, controller_toggle=True) + assert verdict.controller_source and verdict.requested and verdict.effective and verdict.profile_mode + f.sm['carControl'].longActive = False + paused = f.owner.sample(None, params=f.params, settings=f.settings, sm=f.sm, cp=f.cp, + drive_id=fixtures.DRIVE, now_mono_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT) + assert paused.requested and paused.effective is None and paused.profile_mode is None + f.sm['carControl'].longActive = True + assert f.owner.sample(None, params=f.params, settings=f.settings, sm=f.sm, cp=f.cp, + drive_id=fixtures.DRIVE, now_mono_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT).effective + f.cp.openpilotLongitudinalControl = False + assert not f.owner.sample(None, params=f.params, settings=f.settings, sm=f.sm, cp=f.cp, + drive_id=fixtures.DRIVE, now_mono_ns=fixtures.NOW, now_boot_ns=fixtures.BOOT).requested + finally: + f.doCleanups() + + +def test_leased_switchback_status_rejects_expiry_reorder_and_retired_session(): + from openpilot.starpilot.controllers.mode_actions import ModeIntent, publish_switchback, SwitchbackStatusOwner + status = SwitchbackStatusOwner() + first = publish_switchback(None, ModeIntent(DRIVE, True, True), session='a' * 32, sequence=2, + now_ns=NOW, source_car_control_ns=NOW - 1) + assert status.sample(first, drive_id=DRIVE, now_ns=NOW) + assert not status.sample(first, drive_id=DRIVE, now_ns=NOW + 100_000_001) + older = publish_switchback(None, ModeIntent(DRIVE, True, True), session='a' * 32, sequence=1, + now_ns=NOW, source_car_control_ns=NOW - 1) + assert not status.sample(older, drive_id=DRIVE, now_ns=NOW) + restart = publish_switchback(None, ModeIntent(DRIVE, True, True), session='b' * 32, sequence=1, + now_ns=NOW + 1, source_car_control_ns=NOW) + assert not status.sample(restart, drive_id=DRIVE, now_ns=NOW + 1) + assert status.sample(restart, drive_id=DRIVE, now_ns=NOW + 1) + assert not status.sample(first, drive_id=DRIVE, now_ns=NOW + 1) + + +def test_controller_dispatch_requires_live_producer_and_binds_current_control_stamp(): + from unittest.mock import Mock + from openpilot.starpilot.controllers.mode_actions import ModeActionPublisher, ModeIntent, publish_switchback + sm, cp = fixture() + publisher, sender = ModeActionPublisher(), Mock() + with patch('openpilot.starpilot.controllers.mode_actions.ioniq6_media_eligible', return_value=True): + assert not publisher.dispatch('switchback', sm, cp, sender, now_ns=NOW) + sender.send.assert_not_called() + state = publish_switchback(None, ModeIntent(DRIVE, False, False), session='b' * 32, sequence=1, + now_ns=NOW, source_car_control_ns=sm.logMonoTime['carControl']) + sm['slcState'] = state.slcState + for name in ('seen', 'valid', 'alive'): + getattr(sm, name)['slcState'] = True + sm.logMonoTime['slcState'] = NOW + sm.recv_time['slcState'] = NOW / 1e9 + assert publisher.dispatch('switchback', sm, cp, sender, now_ns=NOW) + event = sender.send.call_args.args[1] + assert str(event.slcAction.kind) == 'switchbackModeToggle' + assert event.slcAction.controllerMode.sourceCarControlMonoTime == sm.logMonoTime['carControl'] + assert not publisher.dispatch('traffic', sm, cp, sender, now_ns=NOW + 101_000_000) + assert sender.send.call_count == 1 + + +def test_actual_card_alpha_off_short_wheel_gesture_has_lateral_only_authority(): + from openpilot.selfdrive.car.tests import test_conditional_manual_event as fixtures + from opendbc.car.hyundai.values import CAR, HyundaiFlags + from openpilot.starpilot.conditional_mode.card_input import conditional_traffic_candidate + from openpilot.starpilot.conditional_mode.manual import ButtonTracker, Button, Gesture, Press + f = fixtures.CardManualEventTests('runTest') + f.setUp() + try: + f.cp.carFingerprint = CAR.HYUNDAI_IONIQ_6 + f.cp.flags = int(HyundaiFlags.CANFD_LKA_STEER_MSG) + f.cp.openpilotLongitudinalControl = False + f.sm['carControl'].enabled = f.sm['carControl'].longActive = False + f.sm['carControl'].latActive = True + f.params.put('ModeButtonControl', 7, block=True) + tracker = ButtonTracker() + def pulse(mode, stamp, action=7): + return conditional_traffic_candidate(fixtures.state(), tracker, f.params, f.owner, f.cp, f.sm, + now_ns=f.now, media=f.media((mode, False, stamp)), action=action) + assert not pulse(False, 1_000_000_000)[0] + assert not pulse(True, 1_200_000_000)[0] + result = pulse(False, 1_400_000_000) + assert result[0] and result[1] == Gesture(Button.MODE, Press.SHORT) + assert pulse(False, 1_600_000_000, action=6) is None + f.sm['carControl'].latActive = False + assert not pulse(True, 1_800_000_000)[0] + assert not pulse(False, 2_000_000_000)[0] + f.cp.carFingerprint = 'unknown' + assert pulse(False, 2_200_000_000) is None + finally: + f.doCleanups() diff --git a/openpilot/starpilot/controllers/tests/test_owner.py b/openpilot/starpilot/controllers/tests/test_owner.py new file mode 100644 index 0000000000..dc7393f8bb --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_owner.py @@ -0,0 +1,235 @@ +import json +from pathlib import Path +import tempfile +import unittest +from dataclasses import dataclass + +from openpilot.starpilot.controllers.owner import CONTROLLER_BINDINGS_PARAM, ControllerOwner +from openpilot.starpilot.favorites.state import FavoriteAction, FavoriteRequest, FavoriteResult, FavoriteSlot, FavoriteSnapshot + + +@dataclass(frozen=True) +class Press: + device_id: str + code: int + timestamp_ns: int + + +class FileParams: + def __init__(self, root): + self.root = Path(root) / "params" + self.root.mkdir() + + def get_param_path(self, key): + return str(self.root / key) + + +class TestControllerOwner(unittest.TestCase): + def setUp(self): + temporary = tempfile.TemporaryDirectory() + self.addCleanup(temporary.cleanup) + self.params = FileParams(temporary.name) + self.path = Path(self.params.get_param_path(CONTROLLER_BINDINGS_PARAM)) + self.now = 1_000_000_000 + self.parked = True + self.available = True + self.token = "original" + self.calls = 0 + self.favorite_calls = [] + self.device: dict = {"id": "stable-device", "name": "Test pad", "bus": 3} + self.owner = ControllerOwner(self.params, parked=lambda: self.parked, actions=self.actions, + favorites=self.favorites, invoke_favorite=self.invoke_favorite, clock=lambda: self.now) + self.owner.set_devices([self.device]) + + def actions(self): + return {"safe.action": FavoriteAction("safe.action", "Safe action", available=self.available, + token=self.token, invoke=self.invoke, section="Actions")} + + def invoke(self): + self.calls += 1 + return True + + def favorites(self): + request = FavoriteRequest(0, "favorite.action", "favorite-revision", "favorite-token") + slots = (FavoriteSlot(0, "favorite.action", "Favorite", True, True, available=self.available, request=request), + FavoriteSlot(1), FavoriteSlot(2)) + return FavoriteSnapshot(slots, (), "favorite-revision", True, True) + + def invoke_favorite(self, request): + self.favorite_calls.append(request) + return FavoriteResult(True, "Favorite") + + def configure(self, *, enabled=True, key="safe.action"): + snapshot = self.owner.snapshot() + return self.owner.action({"operation": "save", "revision": snapshot["revision"], "enabled": enabled, + "slots": [key] + [None] * 9}) + + def learn(self, slot, code=42): + snapshot = self.owner.snapshot() + self.owner.action({"operation": "learn", "revision": snapshot["revision"], "slot": slot}) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], code, self.now)) + + def test_default_persistence_revision_and_exact_shape(self): + before = self.owner.snapshot() + self.assertEqual(len(before["slots"]), 13) + self.assertEqual(before["devices"], [self.device]) + self.assertFalse(before["enabled"]) + self.assertFalse(self.path.exists()) + after = self.configure() + self.assertTrue(after["enabled"]) + self.assertNotEqual(after["revision"], before["revision"]) + self.assertEqual(json.loads(self.path.read_bytes()), {"version": 1, "enabled": True, + "slots": ["safe.action"] + [None] * 9, "bindings": []}) + with self.assertRaises(RuntimeError): + self.owner.action({"operation": "save", "revision": before["revision"], "enabled": False, "slots": [None] * 10}) + + def test_learning_only_trusted_new_press_then_fresh_execution(self): + self.configure() + self.owner.action({"operation": "learn", "revision": self.owner.snapshot()["revision"], "slot": 3}) + self.owner.feed(Press(self.device["id"], 42, self.now - 1)) + self.owner.feed(Press("forged", 42, self.now + 1)) + self.assertEqual(self.owner.snapshot()["bindings"], []) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.owner.snapshot()["bindings"], [{"deviceId": self.device["id"], "name": self.device["name"], + "code": 42, "slot": 3}]) + self.assertEqual(self.calls, 0) + self.now += 1_000_000 + press = Press(self.device["id"], 42, self.now) + self.owner.feed(press) + self.owner.feed(press) + self.assertEqual(self.calls, 1) + self.assertTrue(self.owner.snapshot()["lastPress"]["executed"]) + + def test_test_mode_and_park_transition_never_execute(self): + self.configure() + self.learn(3) + self.owner.action({"operation": "test", "enabled": True}) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.calls, 0) + self.assertEqual(self.owner.snapshot()["lastPress"]["message"], "Test press detected") + self.parked = False + self.owner.tick() + self.assertFalse(self.owner.snapshot()["testing"]) + self.assertFalse(self.owner.snapshot()["editable"]) + with self.assertRaises(PermissionError): + self.owner.action({"operation": "cancel"}) + + def test_favorite_reference_and_stale_or_unavailable_press(self): + self.configure() + self.learn(0) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(len(self.favorite_calls), 1) + self.owner.feed(Press(self.device["id"], 42, self.now - 251_000_000)) + self.owner.feed(Press(self.device["id"], 42, self.now + 1)) + self.assertEqual(len(self.favorite_calls), 1) + self.available = False + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(len(self.favorite_calls), 1) + + def test_invalid_saved_document_stays_inert_and_repair_uses_exact_revision(self): + self.path.write_bytes(b'{"version":1,"enabled":true,"enabled":false}') + corrupted = self.owner.snapshot() + self.assertFalse(corrupted["valid"]) + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.calls, 0) + self.assertEqual(self.path.read_bytes(), b'{"version":1,"enabled":true,"enabled":false}') + self.owner.action({"operation": "save", "revision": corrupted["revision"], "enabled": False, "slots": [None] * 10}) + self.assertTrue(self.owner.snapshot()["valid"]) + + def test_remove_and_invalid_payloads(self): + self.configure() + self.learn(3) + rev = self.owner.snapshot()["revision"] + self.owner.action({"operation": "remove", "revision": rev, "deviceId": self.device["id"], "code": 42}) + self.assertEqual(self.owner.snapshot()["bindings"], []) + with self.assertRaises(ValueError): + self.owner.action({"operation": "simulate", "deviceId": self.device["id"], "code": 42}) + with self.assertRaises(ValueError): + self.owner.action({"operation": "save", "revision": self.owner.snapshot()["revision"], + "enabled": True, "slots": ["unknown"] + [None] * 9}) + + def test_hat_code_and_changed_action_token_do_not_bypass_recheck(self): + self.configure() + self.learn(3, 65551) + original = self.owner.actions + calls = 0 + def changing_actions(): + nonlocal calls + calls += 1 + self.token = "before" if calls == 1 else "after" + return original() + self.owner.actions = changing_actions + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 65551, self.now)) + self.assertEqual(self.calls, 0) + self.assertEqual(self.owner.snapshot()["lastPress"]["message"], "Control changed") + + def test_learning_expiry_and_motion_cancel_are_inert(self): + self.configure() + self.owner.action({"operation": "learn", "revision": self.owner.snapshot()["revision"], "slot": 3}) + self.now += 20_000_000_000 + self.owner.tick() + self.assertIsNone(self.owner.snapshot()["learning"]) + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.owner.snapshot()["bindings"], []) + self.owner.action({"operation": "learn", "revision": self.owner.snapshot()["revision"], "slot": 3}) + self.parked = False + self.owner.tick() + self.parked = True + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.owner.snapshot()["bindings"], []) + + def test_queued_press_during_expiring_session_cannot_execute(self): + self.configure() + self.learn(3) + self.owner.action({"operation": "test", "enabled": True}) + session_press = self.now + 19_900_000_000 + self.now += 20_000_000_000 + self.owner.feed(Press(self.device["id"], 42, session_press)) + self.assertEqual(self.calls, 0) + self.assertFalse(self.owner.snapshot()["testing"]) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.calls, 1) + + def test_queued_presses_after_cancel_or_motion_do_not_execute(self): + self.configure() + self.learn(3) + self.owner.action({"operation": "test", "enabled": True}) + queued = self.now + 1_000_000 + self.now = queued + self.owner.action({"operation": "cancel"}) + self.owner.feed(Press(self.device["id"], 42, queued)) + self.assertEqual(self.calls, 0) + self.owner.action({"operation": "test", "enabled": True}) + self.parked = False + self.now += 1_000_000 + self.owner.tick() + self.parked = True + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.calls, 0) + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.calls, 1) + + def test_changed_saved_document_cancels_learning_without_overwrite(self): + self.configure() + self.owner.action({"operation": "learn", "revision": self.owner.snapshot()["revision"], "slot": 3}) + changed = json.loads(self.path.read_bytes()) + changed["enabled"] = False + self.path.write_text(json.dumps(changed)) + before = self.path.read_bytes() + self.now += 1_000_000 + self.owner.feed(Press(self.device["id"], 42, self.now)) + self.assertEqual(self.path.read_bytes(), before) + self.assertIsNone(self.owner.snapshot()["learning"]) + + +if __name__ == "__main__": + unittest.main() diff --git a/openpilot/starpilot/controllers/tests/test_selfie.py b/openpilot/starpilot/controllers/tests/test_selfie.py new file mode 100644 index 0000000000..4e68223470 --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_selfie.py @@ -0,0 +1,126 @@ +import threading +from types import SimpleNamespace + +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from openpilot.starpilot.controllers.selfie import SelfieCapture, authority +from openpilot.starpilot.controllers.wheel_actions import cp_fingerprint +from openpilot.starpilot.galaxy.camera_snapshot import SnapshotUnavailable +from openpilot.starpilot.galaxy.sentry_events import SentryEvents +from openpilot.starpilot.sentry_mode.storage import EventStore + + +class Camera: + def __init__(self): + self.entered = threading.Event() + self.release = threading.Event() + self.done = threading.Event() + + def capture(self, name, *, permitted): + assert name == 'cabin' + self.entered.set() + assert self.release.wait(2) + if not permitted(): + raise SnapshotUnavailable('Authority lost') + return b'\xff\xd8fixture\xff\xd9' + + +class Store(EventStore): + def __init__(self, root): + super().__init__(root) + self.done = threading.Event() + + def record(self, *args, **kwargs): + try: + return super().record(*args, **kwargs) + finally: + self.done.set() + + +def test_async_capture_records_real_index_and_galaxy_image(tmp_path): + camera, store = Camera(), Store(tmp_path / 'events') + owner = SelfieCapture(camera=camera, store=store, clock=lambda: 100) + assert owner.submit(lambda: True) + assert camera.entered.wait(2) + assert owner.busy + assert not owner.submit(lambda: True) + camera.release.set() + assert store.done.wait(2) + owner.worker.join(2) + assert not owner.busy + events = SentryEvents(store).snapshot()['events'] + assert len(events) == 1 + assert events[0]['kind'] == 'selfie' + assert events[0]['images'] == ['cabin'] + assert SentryEvents(store).image(events[0]['eventId'], 'cabin') == b'\xff\xd8fixture\xff\xd9' + + +def test_denied_or_revoked_capture_never_publishes_event(tmp_path): + camera, store = Camera(), Store(tmp_path / 'events') + owner = SelfieCapture(camera=camera, store=store) + allowed = [False] + assert not owner.submit(lambda: allowed[0]) + assert not camera.entered.is_set() + allowed[0] = True + assert owner.submit(lambda: allowed[0]) + assert camera.entered.wait(2) + allowed[0] = False + camera.release.set() + owner.worker.join(2) + assert not owner.busy + assert SentryEvents(store).snapshot()['events'] == [] + + +def test_actual_factory_identity_and_fresh_drive_sources_admit_only_same_session(): + cp = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + drive, now = 1_000_000_000, 2_000_000_000 + class Messages: + def __getitem__(self, name): + return self.data[name] + sm = Messages() + sm.data = {'deviceState': SimpleNamespace(started=True, startedMonoTime=drive), + 'carState': SimpleNamespace(canValid=True, canTimeout=False)} + sm.logMonoTime = dict.fromkeys(sm.data, now - 10_000_000) + sm.recv_time = dict.fromkeys(sm.data, (now - 10_000_000) / 1e9) + sm.seen = sm.alive = sm.valid = dict.fromkeys(sm.data, True) + args = {'drive_id': drive, 'fingerprint': cp_fingerprint(cp), 'now_ns': now} + assert authority(sm, cp, **args) + sm.data['deviceState'].startedMonoTime = drive + 1 + assert not authority(sm, cp, **args) + sm.data['deviceState'].startedMonoTime = drive + sm.data['carState'].canValid = False + assert not authority(sm, cp, **args) + sm.data['carState'].canValid = True + assert not authority(sm, cp, **{**args, 'now_ns': now + 300_000_000}) + cp.dashcamOnly = True + assert not authority(sm, cp, **args) + + +def test_selfie_is_local_even_when_motion_notifications_are_configured(tmp_path): + from openpilot.starpilot.sentry_mode.notifications import NotificationOwner + store = EventStore(tmp_path / 'events') + calls, clock = [], [1.] + owner = NotificationOwner(tmp_path / 'notifications', store, + clock=lambda: clock[0], transport=lambda *args: calls.append(args) or 200) + try: + owner.action({'action': 'configure', 'channel': 'ntfy', 'enabled': True, + 'url': 'https://example.test/events', 'token': ''}) + store.record('selfie', 100, permitted=lambda: True, images={'cabin': b'\xff\xd8fixture\xff\xd9'}) + clock[0] = store.snapshot()["events"][0]["wallTimeNs"] / 1e9 + 1 + owner.tick() + assert not calls + assert owner.state['jobs'] == {} + finally: + owner.close() + + +def test_no_fresh_camera_frame_releases_worker_without_record(tmp_path): + class MissingCamera: + def capture(self, name, *, permitted): + raise SnapshotUnavailable('No fresh camera frame') + store = EventStore(tmp_path / 'events') + owner = SelfieCapture(camera=MissingCamera(), store=store) + assert owner.submit(lambda: True) + owner.worker.join(2) + assert not owner.busy and owner.last_result is None + assert SentryEvents(store).snapshot()['events'] == [] diff --git a/openpilot/starpilot/controllers/tests/test_transport.py b/openpilot/starpilot/controllers/tests/test_transport.py new file mode 100644 index 0000000000..cc5b5b2ec6 --- /dev/null +++ b/openpilot/starpilot/controllers/tests/test_transport.py @@ -0,0 +1,122 @@ +from pathlib import Path +import socket +import struct +import tempfile +import threading +import time +import unittest +from unittest.mock import Mock, patch + +from openpilot.starpilot.controllers.transport import ( + ControllerDenied, ControllerInvalid, ControllerService, ControllerUnavailable, request_action, request_status, +) + + +class TestControllerTransport(unittest.TestCase): + def setUp(self): + temporary = tempfile.TemporaryDirectory() + self.addCleanup(temporary.cleanup) + self.path = Path(temporary.name) / 'ui.sock' + self.owner = Mock() + self.threads = [] + self.owner.snapshot.side_effect = lambda: (self.threads.append(threading.get_ident()) or {'version': 1}) + self.owner.action.side_effect = lambda payload: (self.threads.append(threading.get_ident()) or {'accepted': payload}) + self.service = ControllerService(self.owner, self.path) + self.service.start() + self.addCleanup(self.service.close) + + def pump(self, callback): + result = {} + def call(): + try: + result['value'] = callback() + except Exception as error: + result['error'] = error + client = threading.Thread(target=call) + client.start() + deadline = time.monotonic() + 2 + while client.is_alive() and time.monotonic() < deadline: + self.service.poll() + client.join(0.01) + self.assertFalse(client.is_alive()) + return result + + def test_status_and_configuration_run_on_poll_thread(self): + self.assertEqual(self.pump(lambda: request_status(self.path))['value'], {'version': 1}) + payload = {'operation': 'test', 'enabled': True} + self.assertEqual(self.pump(lambda: request_action(payload, self.path))['value']['accepted'], payload) + self.assertEqual(self.threads, [threading.get_ident()] * 2) + self.assertEqual(self.path.stat().st_mode & 0o777, 0o600) + self.owner.feed.assert_not_called() + + def test_owner_permission_and_validation_return_typed_errors(self): + self.owner.action.side_effect = PermissionError('Park to configure buttons') + self.assertIsInstance(self.pump(lambda: request_action({'operation': 'cancel'}, self.path))['error'], ControllerDenied) + self.owner.action.side_effect = ValueError('Unknown button') + self.assertIsInstance(self.pump(lambda: request_action({'operation': 'cancel'}, self.path))['error'], ControllerInvalid) + + def test_expired_request_cannot_execute_later(self): + with patch('openpilot.starpilot.controllers.transport.REQUEST_DEADLINE', 0.05): + with self.assertRaises(ControllerUnavailable): + request_action({'operation': 'learn', 'slot': 1}, self.path, timeout=0.5) + self.service.poll() + self.owner.action.assert_not_called() + + def test_claimed_slow_write_reports_indeterminate_result(self): + result = {} + def slow_action(payload): + time.sleep(0.1) + return {'saved': True} + self.owner.action.side_effect = slow_action + with patch('openpilot.starpilot.controllers.transport.REQUEST_DEADLINE', 0.04): + result = self.pump(lambda: request_action({'operation': 'cancel'}, self.path)) + self.assertIsInstance(result['error'], ControllerUnavailable) + self.assertIn('may have changed', str(result['error'])) + self.owner.action.assert_called_once() + + def test_remote_cannot_inject_physical_input(self): + with self.assertRaises(ControllerInvalid): + request_action({'operation': 'press', 'code': 304}, self.path) + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as peer: + peer.settimeout(1) + peer.connect(str(self.path)) + data = b'{"operation":"feed","code":304}' + peer.sendall(struct.pack('!I', len(data)) + data) + response = peer.recv(1024) + self.assertIn(b'invalid', response) + self.owner.feed.assert_not_called() + self.owner.action.assert_not_called() + + def test_duplicate_fields_and_oversize_fail_without_dispatch(self): + for data, size in ((b'{"operation":"status","operation":"cancel"}', None), (b'', 65537)): + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as peer: + peer.settimeout(1) + peer.connect(str(self.path)) + peer.sendall(struct.pack('!I', len(data) if size is None else size) + data) + self.assertIn(b'invalid', peer.recv(1024)) + self.service.poll() + self.owner.action.assert_not_called() + self.owner.snapshot.assert_not_called() + + def test_contender_does_not_unlink_running_service(self): + contender = ControllerService(self.owner, self.path) + with self.assertRaises(ControllerUnavailable): + contender.start() + contender.close() + self.assertTrue(self.path.exists()) + self.assertIn('value', self.pump(lambda: request_status(self.path))) + + def test_unexpected_owner_error_is_logged_and_not_returned(self): + self.owner.snapshot.side_effect = KeyError('private details') + with self.assertLogs('openpilot.starpilot.controllers.transport', level='ERROR') as logged: + first = self.pump(lambda: request_status(self.path)) + self.pump(lambda: request_status(self.path)) + self.assertEqual(len(logged.output), 1) + self.assertNotIn('private details', str(first['error'])) + self.assertIsInstance(first['error'], ControllerUnavailable) + + def test_close_and_missing_ui_are_bounded(self): + self.service.close() + self.assertFalse(self.path.exists()) + with self.assertRaises(ControllerUnavailable): + request_status(self.path, timeout=0.05) diff --git a/openpilot/starpilot/controllers/toyota_cruise.py b/openpilot/starpilot/controllers/toyota_cruise.py new file mode 100644 index 0000000000..f72827ffe9 --- /dev/null +++ b/openpilot/starpilot/controllers/toyota_cruise.py @@ -0,0 +1,32 @@ +"""Toyota PCM cruise step preference for the recurring ACC_CONTROL owner.""" + +import time +from openpilot.starpilot.saved_source import read_saved + + +def capability(cp): + try: + if (cp is None or cp.brand != "toyota" or not cp.pcmCruise or not cp.openpilotLongitudinalControl or + cp.passive or cp.dashcamOnly or cp.notCar or not cp.carFingerprint): + return None + return (cp.brand, cp.carFingerprint, bool(cp.pcmCruise), bool(cp.openpilotLongitudinalControl), + bool(cp.passive), bool(cp.dashcamOnly), bool(cp.notCar)) + except (AttributeError, TypeError, ValueError): + return None + + +class ToyotaCruisePreference: + def __init__(self, cp, params): + self.cp, self.params = cp, params + self.next_read = 0.0 + self.enabled = False + + def update(self): + now = time.monotonic() + if now >= self.next_read: + self.next_read = now + 1.0 + choice, choice_valid = read_saved(self.params, "ReverseCruise", 8) + safe, safe_valid = read_saved(self.params, "SafeMode", 8) + self.enabled = bool(capability(self.cp) is not None and choice_valid and safe_valid and + choice == b"1" and safe in (None, b"0")) + return self.enabled diff --git a/openpilot/starpilot/controllers/transport.py b/openpilot/starpilot/controllers/transport.py new file mode 100644 index 0000000000..ed4e3e519f --- /dev/null +++ b/openpilot/starpilot/controllers/transport.py @@ -0,0 +1,256 @@ +import errno +import json +import logging +import os +from pathlib import Path +import socket +import stat +import struct +import threading +import time + +DEFAULT_SOCKET_PATH = Path(f'/tmp/starpilot-controllers-{os.getuid()}') / 'ui.sock' +MAX_BYTES = 65536 +REQUEST_DEADLINE = 1.0 +LOGGER = logging.getLogger(__name__) + + +class ControllerUnavailable(RuntimeError): + pass + + +class ControllerBusy(ControllerUnavailable): + pass + + +class ControllerDenied(PermissionError): + pass + + +class ControllerInvalid(ValueError): + pass + + +def _receive(peer, count): + data = bytearray() + while len(data) < count: + part = peer.recv(count - len(data)) + if not part: + raise ControllerUnavailable('Controller connection closed') + data.extend(part) + return bytes(data) + + +def _decode(data): + def unique(pairs): + result = {} + for key, value in pairs: + if key in result: + raise ControllerInvalid('Duplicate controller field') + result[key] = value + return result + return json.loads(data, object_pairs_hook=unique, + parse_constant=lambda _: (_ for _ in ()).throw(ControllerInvalid('Invalid controller value'))) + + +def _encode(value): + data = json.dumps(value, separators=(',', ':'), allow_nan=False).encode() + if not 0 < len(data) <= MAX_BYTES: + raise ControllerInvalid('Controller message exceeds limit') + return data + + +def _request(payload, socket_path, timeout): + encoded = _encode(payload) + try: + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as peer: + peer.settimeout(timeout) + peer.connect(str(socket_path or DEFAULT_SOCKET_PATH)) + peer.sendall(struct.pack('!I', len(encoded)) + encoded) + size = struct.unpack('!I', _receive(peer, 4))[0] + if not 0 < size <= MAX_BYTES: + raise ControllerUnavailable('Invalid controller response size') + response = _decode(_receive(peer, size)) + if type(response) is not dict or set(response) != {'status', 'result'}: + raise ControllerUnavailable('Invalid controller response') + status, result = response['status'], response['result'] + if status == 'ok' and type(result) is dict: + return result + errors = {'invalid': ControllerInvalid, 'denied': ControllerDenied, 'busy': ControllerBusy} + raise errors.get(status, ControllerUnavailable)(result if type(result) is str else 'Controller buttons are unavailable') + except (ControllerInvalid, ControllerDenied, ControllerUnavailable): + raise + except (OSError, ValueError, RecursionError) as error: + raise ControllerUnavailable('Controller buttons are unavailable. Refresh to check their state.') from error + + +def request_status(socket_path=None, timeout=1.5): + return _request({'operation': 'status'}, socket_path, timeout) + + +def request_action(payload, socket_path=None, timeout=1.5): + if type(payload) is not dict or payload.get('operation') not in {'save', 'learn', 'cancel', 'test', 'remove'}: + raise ControllerInvalid('Invalid controller operation') + return _request(payload, socket_path, timeout) + + +class _Job: + def __init__(self, payload): + self.payload = payload + self.deadline = time.monotonic() + REQUEST_DEADLINE + self.done = threading.Event() + self.cancelled = False + self.started = False + self.response = {'status': 'unavailable', 'result': 'Controller buttons are unavailable'} + + +class ControllerService: + """The socket accepts configuration; physical button execution stays on the UI thread.""" + def __init__(self, owner, socket_path=None): + self.owner = owner + self.path = Path(socket_path or DEFAULT_SOCKET_PATH) + self._lock = threading.Lock() + self._pending = None + self._closed = threading.Event() + self._listener = None + self._thread = None + self._identity = None + self._last_error = float('-inf') + + def start(self): + if self._listener is not None or self._closed.is_set(): + raise ControllerUnavailable('Controller service already started or closed') + self.path.parent.mkdir(mode=0o700, parents=True, exist_ok=True) + parent = self.path.parent.lstat() + if parent.st_uid != os.getuid() or not stat.S_ISDIR(parent.st_mode) or parent.st_mode & 0o077: + raise ControllerUnavailable('Controller socket directory is not private') + try: + existing = self.path.lstat() + except FileNotFoundError: + pass + else: + if existing.st_uid != os.getuid() or not stat.S_ISSOCK(existing.st_mode): + raise ControllerUnavailable('Controller socket path is occupied') + with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as peer: + peer.settimeout(0.1) + try: + peer.connect(str(self.path)) + except (ConnectionRefusedError, FileNotFoundError): + self.path.unlink() + else: + raise ControllerUnavailable('Controller service is already active') + listener = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) + try: + listener.bind(str(self.path)) + info = self.path.lstat() + self._identity = info.st_dev, info.st_ino + os.chmod(self.path, 0o600) + listener.listen(8) + listener.settimeout(0.1) + except BaseException: + listener.close() + self._unlink_owned() + raise + self._listener = listener + self._thread = threading.Thread(target=self._serve, args=(listener,), name='controller-settings', daemon=True) + self._thread.start() + + def _serve(self, listener): + while not self._closed.is_set(): + try: + peer, _ = listener.accept() + except TimeoutError: + continue + except OSError as error: + if error.errno in (errno.ECONNABORTED, errno.EINTR): + continue + break + with peer: + peer.settimeout(0.5) + try: + size = struct.unpack('!I', _receive(peer, 4))[0] + if not 0 < size <= MAX_BYTES: + raise ControllerInvalid('Invalid controller request size') + payload = _decode(_receive(peer, size)) + if type(payload) is not dict or payload.get('operation') not in {'status', 'save', 'learn', 'cancel', 'test', 'remove'}: + raise ControllerInvalid('Invalid controller operation') + job = _Job(payload) + with self._lock: + self._pending = job + if not job.done.wait(REQUEST_DEADLINE): + with self._lock: + if not job.started: + job.cancelled = True + if self._pending is job: + self._pending = None + uncertain = job.started + job.response = {'status': 'unavailable', 'result': ( + 'Controller settings may have changed. Refresh before trying again.' if uncertain else + 'Controller request expired before it ran. Refresh to try again.')} + encoded = _encode(job.response) + peer.sendall(struct.pack('!I', len(encoded)) + encoded) + except (ValueError, RecursionError) as error: + try: + encoded = _encode({'status': 'invalid', 'result': str(error)[:160]}) + peer.sendall(struct.pack('!I', len(encoded)) + encoded) + except OSError: + pass + except (OSError, ControllerUnavailable): + pass + + def poll(self): + with self._lock: + job = self._pending + self._pending = None + if job is None or job.cancelled or self._closed.is_set() or time.monotonic() >= job.deadline: + return + job.started = True + try: + payload = job.payload + if payload == {'operation': 'status'}: + result = self.owner.snapshot() + elif payload.get('operation') == 'status': + raise ControllerInvalid('Invalid controller status request') + else: + result = self.owner.action(payload) + job.response = {'status': 'ok', 'result': result} + _encode(job.response) + except PermissionError as error: + job.response = {'status': 'denied', 'result': str(error)[:160]} + except ValueError as error: + job.response = {'status': 'invalid', 'result': str(error)[:160]} + except (OSError, RuntimeError) as error: + job.response = {'status': 'unavailable', 'result': str(error)[:160]} + except Exception: + now = time.monotonic() + if now - self._last_error >= 10: + self._last_error = now + LOGGER.exception('Controller settings failed') + job.response = {'status': 'unavailable', 'result': 'Controller buttons are unavailable'} + finally: + job.done.set() + + def close(self): + self._closed.set() + with self._lock: + if self._pending is not None: + self._pending.cancelled = True + self._pending.done.set() + self._pending = None + if self._listener is not None: + self._listener.close() + self._listener = None + if self._thread is not None: + self._thread.join(REQUEST_DEADLINE + 0.2) + self._thread = None + self._unlink_owned() + + def _unlink_owned(self): + identity, self._identity = self._identity, None + if identity is not None: + try: + info = self.path.lstat() + if (info.st_dev, info.st_ino) == identity: + self.path.unlink() + except FileNotFoundError: + pass diff --git a/openpilot/starpilot/controllers/wheel_actions.py b/openpilot/starpilot/controllers/wheel_actions.py new file mode 100644 index 0000000000..c6330cf503 --- /dev/null +++ b/openpilot/starpilot/controllers/wheel_actions.py @@ -0,0 +1,247 @@ +import hashlib +import re +import secrets +from dataclasses import dataclass + +from opendbc.car import structs +from openpilot.starpilot.conditional_mode.manual import Button, ButtonTracker, Press, ioniq6_media_eligible +from openpilot.starpilot.saved_source import read_saved +from openpilot.starpilot.conditional_mode.ui_action import fresh_service + + +ACTIONS = { + "Off": 0, "Cycle personality": 1, "Force coast": 2, "Pause steering": 3, "Pause longitudinal": 4, + "Experimental override": 5, "Traffic mode": 6, "Switchback Mode": 7, "Bookmark": 8, + "Toggle AOL": 9, "Adopt speed limit": 10, "Quick Select 1": 11, "Quick Select 2": 12, + "Quick Select 3": 13, "Pulse and glide": 14, +} +KEYS = ("LKASButtonControl", "MainCruiseButtonControl", "DistanceButtonControl", "LongDistanceButtonControl", + "VeryLongDistanceButtonControl", "CancelButtonControl", "LongCancelButtonControl", "VeryLongCancelButtonControl", + "ModeButtonControl", "LongModeButtonControl", "VeryLongModeButtonControl", "StarButtonControl", + "LongStarButtonControl", "VeryLongStarButtonControl") +LIFETIME_NS = 250_000_000 +SESSION = re.compile(r"[0-9a-f]{32}\Z") +DELIVERED_ACTIONS = frozenset((1, 2, 5, 6, 7, 8, 10, 11, 12, 13, 14)) +DEFAULTS = {"DistanceButtonControl": 1, "LongDistanceButtonControl": 5, "VeryLongDistanceButtonControl": 6} +BUTTON_KEYS = { + Button.LKAS: (KEYS[0],), Button.DISTANCE: KEYS[2:5], Button.MODE: KEYS[8:11], Button.CUSTOM: KEYS[11:14], +} + + +def eligible(cp): + try: + return bool(cp is not None and not cp.notCar and not cp.passive and not cp.dashcamOnly and cp.carFingerprint) + except (AttributeError, TypeError, ValueError): + return False + + +def cp_fingerprint(cp): + reader = cp.as_reader() if hasattr(cp, "as_reader") else cp + return hashlib.sha256(reader.as_builder().to_bytes()).hexdigest() + + +def capture(params): + result = {} + for key in KEYS: + raw, readable = read_saved(params, key, 8) + if not readable or raw is not None and raw not in tuple(str(value).encode() for value in range(15)): + return None + result[key] = int(raw) if raw is not None else DEFAULTS.get(key, 0) + return result + + +def key_for(gesture): + keys = BUTTON_KEYS[gesture.button] + index = (Press.SHORT, Press.LONG, Press.VERY_LONG).index(gesture.press) + return keys[index] if index < len(keys) else None + + +@dataclass(frozen=True) +class WheelCommand: + session: str + sequence: int + observed_ns: int + drive_id: int + source_car_ns: int + key: str + action: int + + +class WheelPublisher: + def __init__(self, params=None): + self.session = secrets.token_hex(16) + self.sequence = 0 + self.drive = 0 + self.tracker = ButtonTracker() + self.cancel_tracker = ButtonTracker() + self.main_tracker = ButtonTracker() + self.mapping = None + self.last_heartbeat = 0 + self.checked_ns = 0 + self.media_sample_state = None + self.suppress_distance_release = False + initial = capture(params) if params is not None else None + self.axis_keys = {key for key, action in (initial or {}).items() if action in (3, 4, 9)} + self.initial_axis_read = initial is not None + + def media_audit_needed(self, media): + if media is None: + return False + if not media.valid: + self.media_sample_state = None + return bool(media.samples) + if not media.samples: + return False + previous = self.media_sample_state + changed = previous is None or previous[0] != media.source_epoch + for sample in media.samples: + current = (media.source_epoch, sample.mode_held, sample.custom_held) + # Keep auditing a held gesture: an assignment can change and return to + # its original value before release, which must still cancel that press. + changed |= current != previous or sample.mode_held or sample.custom_held + previous = current + self.media_sample_state = previous + # Neutral media packets also used to audit assignments during conventional + # button holds. Preserve those revocations until every gesture is idle. + held = any(tracker.distance_held or tracker.media_held is not None + for tracker in (self.tracker, self.cancel_tracker, self.main_tracker)) + return changed or bool(media.samples and held) + + def observe(self, params, cp, state, *, now_ns, drive_id, media=None, blocked_keys=frozenset()): + self.suppress_distance_release = any(event.type == structs.CarState.ButtonEvent.Type.gapAdjustCruise and not event.pressed + for event in state.buttonEvents) + # Repeated neutral packets are not button edges. Keep held-input audits, + # the periodic audit, and the final read before emitted commands below. + media_edge = self.media_audit_needed(media) + edge = bool(state.buttonEvents) or media_edge + audit = self.mapping is None or drive_id != self.drive or edge or now_ns - self.checked_ns >= 1_000_000_000 + source_eligible = bool(eligible(cp) and state.canValid and not state.canTimeout and type(now_ns) is int and + type(drive_id) is int and 0 < drive_id < now_ns) + mapping = capture(params) if audit and source_eligible else self.mapping + if audit: + self.checked_ns = now_ns + if not source_eligible or mapping is None: + self.tracker = ButtonTracker() + self.cancel_tracker = ButtonTracker() + self.main_tracker = ButtonTracker() + self.mapping = None + return () + if not self.initial_axis_read: + self.axis_keys = {key for key, action in mapping.items() if action in (3, 4, 9)} + self.initial_axis_read = True + if drive_id != self.drive or mapping != self.mapping: + self.tracker = ButtonTracker() + self.cancel_tracker = ButtonTracker() + self.main_tracker = ButtonTracker() + self.drive, self.mapping = drive_id, mapping + gestures = [(key_for(gesture), gesture) for gesture in self.tracker.observe( + state, media if ioniq6_media_eligible(cp) else None)] + for tracker, source, keys in ((self.cancel_tracker, structs.CarState.ButtonEvent.Type.cancel, KEYS[5:8]), + (self.main_tracker, structs.CarState.ButtonEvent.Type.mainCruise, KEYS[1:2])): + if not any(mapping[key] in DELIVERED_ACTIONS for key in keys): + continue + copy = structs.CarState(canValid=state.canValid, canTimeout=state.canTimeout) + copy.buttonEvents = [structs.CarState.ButtonEvent(type=structs.CarState.ButtonEvent.Type.gapAdjustCruise, + pressed=event.pressed) + for event in state.buttonEvents if event.type == source] + for gesture in tracker.observe(copy.as_reader()): + index = (Press.SHORT, Press.LONG, Press.VERY_LONG).index(gesture.press) + if index < len(keys): + gestures.append((keys[index], gesture)) + commands = [(key, mapping[key]) for key, _ in gestures if key is not None and key not in blocked_keys and + key not in self.axis_keys and mapping[key] in DELIVERED_ACTIONS and + (key != "MainCruiseButtonControl" or mapping[key] == 10)] + if commands and not audit: + current = capture(params) + self.checked_ns = now_ns + if current != mapping: + self.tracker = ButtonTracker() + self.cancel_tracker = ButtonTracker() + self.main_tracker = ButtonTracker() + self.mapping = current + return () + if len(commands) > 1: + return () + if not commands and now_ns - self.last_heartbeat >= 100_000_000: + commands.append(("", 0)) + self.last_heartbeat = now_ns + return tuple(commands) + + def publish(self, commands, cp, publisher, *, now_ns, drive_id, source_car_ns, source_control_ns): + from openpilot.cereal import messaging + + for key, action in commands: + self.sequence += 1 + event = messaging.new_message("slcCruiseEvent", valid=True, logMonoTime=now_ns) + event.slcCruiseEvent.kind = "wheelAction" + event.slcCruiseEvent.eventId = self.sequence + event.slcCruiseEvent.producerSessionId = self.session + event.slcCruiseEvent.observedMonoTime = now_ns + event.slcCruiseEvent.wheelAction = { + "version": 1, "sessionId": self.session, "sequence": self.sequence, "observedMonoTime": now_ns, + "validUntilMonoTime": now_ns + LIFETIME_NS, "driveStartMonoTime": drive_id, + "carFingerprint": cp.carFingerprint, "sourceCarControlMonoTime": source_control_ns, + "carParamsFingerprint": cp_fingerprint(cp), + "sourceCarStateMonoTime": source_car_ns, "buttonKey": key, "actionCode": action, + } + publisher.send("slcCruiseEvent", event) + + +class WheelConsumer: + def __init__(self): + self.drive = 0 + self.session = None + self.retired = set() + self.sequence = 0 + self.last_heartbeat = 0 + + def accept(self, event, params, cp, sm, *, now_ns): + try: + if not event.valid or str(event.slcCruiseEvent.kind) != "wheelAction" or not eligible(cp): + return None + wire = event.slcCruiseEvent.wheelAction + drive = int(sm["deviceState"].startedMonoTime) + if drive != self.drive: + self.__init__() + self.drive = drive + observed = int(wire.observedMonoTime) + source = int(wire.sourceCarStateMonoTime) + session = str(wire.sessionId) + key, action = str(wire.buttonKey), int(wire.actionCode) + if (wire.version != 1 or int(event.slcCruiseEvent.eventId) != int(wire.sequence) or + str(event.slcCruiseEvent.producerSessionId) != session or + int(event.slcCruiseEvent.observedMonoTime) != observed or not SESSION.fullmatch(session) or session in self.retired or + int(wire.driveStartMonoTime) != drive or str(wire.carFingerprint) != cp.carFingerprint or + str(wire.carParamsFingerprint) != cp_fingerprint(cp) or + not sm["deviceState"].started or not sm["carState"].canValid or sm["carState"].canTimeout or + not drive < source <= observed <= now_ns <= int(wire.validUntilMonoTime) or + int(wire.validUntilMonoTime) - observed != LIFETIME_NS or int(event.logMonoTime) != observed or + source > int(sm.logMonoTime["carState"]) or now_ns - source > LIFETIME_NS or + not all(fresh_service(sm, name, drive, now_ns, LIFETIME_NS) for name in ("carState", "carControl")) or + not fresh_service(sm, "deviceState", drive, now_ns, 1_000_000_000) or + not drive < int(wire.sourceCarControlMonoTime) <= observed or + int(wire.sourceCarControlMonoTime) > int(sm.logMonoTime["carControl"]) or + now_ns - int(wire.sourceCarControlMonoTime) > LIFETIME_NS): + return None + if action == 0: + if key: + return None + elif key not in KEYS or action not in DELIVERED_ACTIONS: + return None + else: + mapping = capture(params) + if mapping is None or mapping[key] != action: + return None + if session != self.session: + if self.session is not None: + if len(self.retired) >= 8: + return None + self.retired.add(self.session) + self.session, self.sequence = session, 0 + if wire.sequence <= self.sequence: + return None + self.sequence = int(wire.sequence) + self.last_heartbeat = observed + return WheelCommand(session, self.sequence, observed, drive, source, key, action) + except (AttributeError, KeyError, TypeError, ValueError, OverflowError): + return None diff --git a/openpilot/starpilot/longitudinal/niro_follow.py b/openpilot/starpilot/longitudinal/niro_follow.py new file mode 100644 index 0000000000..5da1bb6497 --- /dev/null +++ b/openpilot/starpilot/longitudinal/niro_follow.py @@ -0,0 +1,75 @@ +"""First-generation Niro EV far-follow output comfort, with urgent bypass.""" +import math + +from opendbc.car.hyundai.values import CAR + +SOURCE_MAX_AGE_NS = 250_000_000 +SOURCES = ('carState', 'carControl', 'controlsState', 'selfdriveState', 'radarState', 'modelV2') + + +def eligible(cp): + return bool(cp.brand == 'hyundai' and cp.carFingerprint == CAR.KIA_NIRO_EV and cp.openpilotLongitudinalControl and + not cp.passive and not cp.dashcamOnly and not cp.notCar) + + +def close_fast(lead, speed, follow): + closing = max(0., speed - lead.vLead) + desired_gap = (speed * speed - lead.vLead * lead.vLead) / 5. + follow * speed + 6. + # Do not soften braking inside the fast-closing follow window, even before + # model confidence drops. + return bool(closing >= max(2., .08 * speed) and lead.dRel <= desired_gap + max(8., .35 * speed)) + + +class NiroFarFollow: + def __init__(self, dt): + if not math.isfinite(dt) or not 0 < dt <= .1: + raise ValueError('invalid model interval') + self.dt = dt + self.reset() + + def reset(self): + self.safe = False + self.drive_id = 0 + self.last_model_ns = 0 + + def sample(self, sm, *, now_ns, drive_id, active, target, previous_target, follow_seconds, blocked=False): + try: + if drive_id != self.drive_id: + self.reset() + self.drive_id = drive_id + fresh = (type(drive_id) is int and drive_id > 0 and type(now_ns) is int and now_ns > drive_id and + 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)) + model_ns = sm.logMonoTime['modelV2'] + car = sm['carState'] + if (not fresh or not active or blocked or model_ns <= self.last_model_ns or + not car.canValid or car.canTimeout or car.gasPressed or car.brakePressed or car.standstill or + sm['controlsState'].forceDecel or sm['selfdriveState'].experimentalMode or not sm['selfdriveState'].enabled or + not sm['carControl'].longActive or sm['modelV2'].action.shouldStop): + self.safe = False + return target + if self.last_model_ns and model_ns - self.last_model_ns > SOURCE_MAX_AGE_NS: + self.safe = False + self.last_model_ns = model_ns + speed = float(car.vEgo) + if not all(math.isfinite(value) for value in (speed, target, previous_target, follow_seconds)) or not .5 <= follow_seconds <= 3: + self.safe = False + return target + leads = [lead for lead in (sm['radarState'].leadOne, sm['radarState'].leadTwo) if lead.present] + if not all(math.isfinite(float(getattr(lead, key))) for lead in leads for key in ('dRel', 'vLead', 'yRel')): + self.safe = False + return target + centered = [lead for lead in leads if abs(lead.yRel) <= 1.5] + safe = bool(centered and speed >= 5 and not any(close_fast(lead, speed, follow_seconds) for lead in leads)) + for lead in centered: + closing = max(0., speed - lead.vLead) + ttc = lead.dRel / max(closing, .1) if closing > .1 else math.inf + safe &= bool(lead.dRel >= max(25., 1.35 * speed) and lead.dRel / max(speed, .001) >= 1.35 and ttc >= 8.) + was_safe = self.safe + self.safe = safe + if not safe or not was_safe: + return target + return min(previous_target + 1.25 * self.dt, max(previous_target - 2. * self.dt, target)) + except (AttributeError, KeyError, ValueError, TypeError, OverflowError): + self.safe = False + return target diff --git a/openpilot/starpilot/longitudinal/tests/test_bolt_acc_pedal_stop_hold.py b/openpilot/starpilot/longitudinal/tests/test_bolt_acc_pedal_stop_hold.py new file mode 100644 index 0000000000..18c7b07a1f --- /dev/null +++ b/openpilot/starpilot/longitudinal/tests/test_bolt_acc_pedal_stop_hold.py @@ -0,0 +1,149 @@ +"""The registered Bolt interceptor controller keeps admitted stops pedal-neutral.""" + +import pytest + +from opendbc.can import CANParser +from opendbc.car import Bus, structs +from opendbc.car.gm import gmcan +from opendbc.car.gm.tests import test_bolt_acc_pedal_friction as fixtures +from opendbc.car.gm.values import BOLT_PEDAL_WORDS, CAR, DBC, is_bolt_pedal_profile + +NOW = 1_000_000_000 +STOPPING = structs.CarControl.Actuators.LongControlState.stopping +PID = structs.CarControl.Actuators.LongControlState.pid + + +def stop_fixture(candidate=CAR.CHEVROLET_BOLT_ACC_2022_2023_PEDAL): + fixture = fixtures.TestBoltAccPedalFriction(methodName='runTest').fixture(candidate) + controller, control, state, _ = fixture + assert is_bolt_pedal_profile(controller.CP) + assert controller.CP.safetyConfigs[0].safetyParam == BOLT_PEDAL_WORDS[candidate] + state.canValid = True + state.vEgo = 0.0 + state.standstill = True + control.actuators.accel = 0.0 + control.actuators.longControlState = STOPPING + return fixture + + +def step(fixture, frame): + controller, control, state, cs = fixture + controller.frame = frame + cs.out = state.as_reader() + return controller.update(control.as_reader(), cs, NOW) + + +def pedal_fields(controller, messages, frame): + pedal = [message for message in messages if message[0] == 0x200] + assert len(pedal) == 1 + message = pedal[0] + assert message[2] == 0 + assert len(message[1]) == 6 + assert message[1][5] == gmcan.pedal_crc(message[1]) + parser = CANParser(DBC[controller.CP.carFingerprint][Bus.pt], [('GAS_COMMAND', 0)], 0) + parser.update([(NOW, pedal)]) + fields = parser.vl['GAS_COMMAND'] + assert fields['COUNTER_PEDAL'] == (frame // 4) % 16 + return message[1], fields + + +def neutral(controller, output, messages, frame): + raw, fields = pedal_fields(controller, messages, frame) + assert fields['ENABLE'] == 0 + assert raw[:4] == bytes(4) # Native inactive encoding, rather than decoded offset-valued fields. + assert output.gas == 0.0 + assert controller.pedal_steady == 0.0 + + +def test_stop_hold_enters_and_survives_standstill_signal_release(): + fixture = stop_fixture() + controller, _, state, _ = fixture + for frame, standstill in ((4, True), (8, False), (12, False)): + state.standstill = standstill + output, messages = step(fixture, frame) + neutral(controller, output, messages, frame) + assert controller.bolt_acc_pedal_stop_hold + + +@pytest.mark.parametrize('resume, long_state', [(True, STOPPING), (False, PID)]) +def test_explicit_launch_releases_hold_and_existing_calculation(resume, long_state): + fixture = stop_fixture() + controller, control, state, _ = fixture + output, messages = step(fixture, 4) + neutral(controller, output, messages, 4) + state.standstill = False + control.cruiseControl.resume = resume + control.actuators.longControlState = long_state + output, messages = step(fixture, 8) + _, fields = pedal_fields(controller, messages, 8) + assert not controller.bolt_acc_pedal_stop_hold + assert fields['ENABLE'] == 1 + assert output.gas > 0.0 + + +@pytest.mark.parametrize( + 'gate', ['long_inactive', 'gas', 'brake', 'regen', 'stock_acc', 'main_off', 'gear', 'sensor_bad', 'sensor_stale', 'stock_stale', 'gear_stale', 'main_stale'] +) +def test_admission_loss_clears_hold_and_never_sends_active_pedal(gate): + fixture = stop_fixture() + controller, control, state, cs = fixture + output, messages = step(fixture, 4) + neutral(controller, output, messages, 4) + state.standstill = False + if gate == 'long_inactive': + control.longActive = False + elif gate == 'gas': + state.gasPressed = True + elif gate == 'brake': + state.brakePressed = True + elif gate == 'regen': + state.regenBraking = True + elif gate == 'stock_acc': + state.cruiseState.enabled = True + elif gate == 'main_off': + state.cruiseState.available = False + elif gate == 'gear': + state.gearShifter = structs.CarState.GearShifter.neutral + elif gate == 'sensor_bad': + cs.pedal_sensor_healthy = False + else: + name = { + 'sensor_stale': 'pedal_sensor_ts_nanos', + 'stock_stale': 'stock_acc_status_ts_nanos', + 'gear_stale': 'bolt_pedal_gear_ts_nanos', + 'main_stale': 'bolt_pedal_main_ts_nanos', + }[gate] + setattr(cs, name, 1) + # The state is cleared even on the 100 Hz tick with no 25 Hz interceptor emission. + _, messages = step(fixture, 5) + assert not controller.bolt_acc_pedal_stop_hold + assert not any(message[0] == 0x200 for message in messages) + output, messages = step(fixture, 8) + neutral(controller, output, messages, 8) + assert not controller.bolt_acc_pedal_stop_hold + + +def test_moving_stopping_does_not_start_hold(): + fixture = stop_fixture() + controller, _, state, _ = fixture + state.standstill = False + state.vEgo = 1.0 + output, messages = step(fixture, 4) + _, fields = pedal_fields(controller, messages, 4) + assert not controller.bolt_acc_pedal_stop_hold + assert fields['ENABLE'] == 1 + assert output.gas > 0.0 + + +def test_no_acc_fixed_stop_keeps_existing_memory_semantics(): + fixture = stop_fixture(CAR.CHEVROLET_BOLT_CC_2022_2023) + controller = fixture[0] + controller.pedal_steady = 0.2 + controller.pedal_active_last = True + output, messages = step(fixture, 4) + raw, fields = pedal_fields(controller, messages, 4) + assert not controller.bolt_acc_pedal_stop_hold + assert controller.pedal_steady == 0.2 + assert output.gas == 0.0 + assert fields['ENABLE'] == 0 + assert raw[:4] == bytes(4)