From 96ab07889da559b50e549163193374317eb480e3 Mon Sep 17 00:00:00 2001 From: Ryan Malloy Date: Fri, 2 Oct 2026 00:22:06 -0600 Subject: [PATCH 1/3] arsdk: discovery, transport, session, and the ported simulator The transport is threaded and the session puts an async face on it, so pings and acks are answered whether or not anyone is awaiting a coroutine. Telemetry is stored per key with a timestamp, because a drone that has stopped reporting otherwise reads identically to one repeating itself. Encoding goes through protocol/codec lazily, so this lands without waiting for that stream; the sim carries a small encoder of its own for the events it sends, which also keeps it from agreeing with the client about a shared mistake. Ported from bebop-2's sim.py, retargeted at the vendored XML and with the identity burst now sent per controller attach rather than once per process. --- src/mcbebop/arsdk/connection.py | 308 +++++++++++++++++++++ src/mcbebop/arsdk/discovery.py | 130 +++++++++ src/mcbebop/arsdk/session.py | 469 ++++++++++++++++++++++++++++++++ src/mcbebop/sim.py | 441 ++++++++++++++++++++++++++++++ tests/test_arsdk_framing.py | 109 ++++++++ tests/test_arsdk_session.py | 399 +++++++++++++++++++++++++++ tests/test_sim.py | 286 +++++++++++++++++++ 7 files changed, 2142 insertions(+) create mode 100644 src/mcbebop/arsdk/connection.py create mode 100644 src/mcbebop/arsdk/discovery.py create mode 100644 src/mcbebop/arsdk/session.py create mode 100644 src/mcbebop/sim.py create mode 100644 tests/test_arsdk_framing.py create mode 100644 tests/test_arsdk_session.py create mode 100644 tests/test_sim.py diff --git a/src/mcbebop/arsdk/connection.py b/src/mcbebop/arsdk/connection.py new file mode 100644 index 0000000..1d3495d --- /dev/null +++ b/src/mcbebop/arsdk/connection.py @@ -0,0 +1,308 @@ +"""The ARSDK3 transport: one TCP handshake, then UDP both ways. + +This is deliberately synchronous and thread-based. The receive side has to +answer pings and acknowledgements within milliseconds whether or not anyone is +awaiting a coroutine, so it owns a daemon thread and hands frames to a +callback; `session.py` puts the async face on top with `asyncio.to_thread`. + +Everything the drone sends arrives on one socket, so one thread and one lock +is the whole concurrency story. +""" + +from __future__ import annotations + +import json +import logging +import socket +import threading +from collections.abc import Callable + +from mcbebop.arsdk.types import ( + D2C_PORT, + DISCOVERY_PORT, + VIDEO_CONTROL_PORT, + VIDEO_STREAM_PORT, + BufferId, + DataType, + Frame, + HandshakeError, + NotConnected, +) + +log = logging.getLogger(__name__) + +# How long a blocking recvfrom waits before looping. It is not a link timeout: +# it only bounds how long `disconnect()` waits for the thread to notice. +_RECV_POLL = 0.25 +_MAX_DATAGRAM = 65535 + + +class Connection: + """One controller session's worth of transport. + + Not reusable across connects on purpose: the sockets and the sequence + counters belong to a single link, and a fresh object is cheaper to reason + about than a reset. + """ + + def __init__( + self, + ip: str, + *, + discovery_port: int = DISCOVERY_PORT, + d2c_port: int = D2C_PORT, + controller_name: str = "mcbebop", + timeout: float = 5.0, + video_stream_port: int = VIDEO_STREAM_PORT, + video_control_port: int = VIDEO_CONTROL_PORT, + on_frame: Callable[[Frame], None] | None = None, + ) -> None: + self.ip = ip + self.discovery_port = discovery_port + self.d2c_port = d2c_port + self.controller_name = controller_name + self.timeout = timeout + self.video_stream_port = video_stream_port + self.video_control_port = video_control_port + self.on_frame = on_frame + + self.handshake: dict[str, object] = {} + self.c2d_port: int | None = None + + self._udp: socket.socket | None = None + self._lock = threading.Lock() + self._seq: dict[int, int] = {} + self._stop = threading.Event() + self._thread: threading.Thread | None = None + + # Acks, keyed (buffer id we sent on, sequence we sent). An Event per + # outstanding send; the receive thread sets it. Entries are removed by + # whoever was waiting, and dropped on disconnect. + self._acks: dict[tuple[int, int], threading.Event] = {} + + self.frames_in = 0 + self.frames_out = 0 + self.pings_answered = 0 + + # -- lifecycle ------------------------------------------------------- + @property + def connected(self) -> bool: + return self._udp is not None and not self._stop.is_set() + + def connect(self) -> dict[str, object]: + """Handshake, bind, and start receiving. Returns the drone's reply.""" + if self.connected: + raise HandshakeError("this Connection is already open") + + # Bind before handshaking, in that order on purpose: the handshake + # tells the drone where to send, and the Bebop 2 starts sending + # immediately. Binding afterwards drops the first events, and it also + # means a port clash is discovered only after the drone has committed + # its single controller slot to us. A d2c_port of 0 asks the OS for a + # free one, which is how a test runs two sessions at once. + udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + try: + udp.bind(("", self.d2c_port)) + except OSError as exc: + udp.close() + raise HandshakeError( + f"cannot bind UDP port {self.d2c_port} to receive telemetry: {exc}. " + "Another controller process on this machine is probably still running." + ) from exc + self.d2c_port = udp.getsockname()[1] + udp.settimeout(_RECV_POLL) + + try: + self.handshake = self._do_handshake() + except Exception: + udp.close() + raise + self.c2d_port = int(self.handshake["c2d_port"]) # type: ignore[arg-type] + + self._udp = udp + self._stop.clear() + self._thread = threading.Thread(target=self._receive_loop, name="arsdk-recv", daemon=True) + self._thread.start() + log.info("connected to %s, sending to c2d port %d", self.ip, self.c2d_port) + return self.handshake + + def disconnect(self) -> None: + """Stop the receive thread and release the sockets. Idempotent.""" + self._stop.set() + thread, self._thread = self._thread, None + if thread is not None and thread is not threading.current_thread(): + # A little longer than one poll interval, so a thread parked in + # recvfrom has time to see the flag rather than being abandoned. + thread.join(timeout=_RECV_POLL * 4) + udp, self._udp = self._udp, None + if udp is not None: + udp.close() + with self._lock: + for event in self._acks.values(): + event.set() # wake anyone waiting; they will see `connected` is False + self._acks.clear() + + def __enter__(self) -> Connection: + self.connect() + return self + + def __exit__(self, *exc: object) -> None: + self.disconnect() + + # -- handshake ------------------------------------------------------- + def _do_handshake(self) -> dict[str, object]: + request = { + "d2c_port": self.d2c_port, + "controller_type": "computer", + "controller_name": self.controller_name, + "arstream2_client_stream_port": self.video_stream_port, + "arstream2_client_control_port": self.video_control_port, + } + try: + sock = socket.create_connection((self.ip, self.discovery_port), timeout=self.timeout) + except OSError as exc: + raise HandshakeError( + f"no answer on {self.ip}:{self.discovery_port}: {exc}. Check this machine has joined " + "the drone's access point and that the aircraft is powered on." + ) from exc + + with sock: + sock.settimeout(self.timeout) + sock.sendall(json.dumps(request).encode()) + raw = self._read_until_nul(sock) + + try: + reply = json.loads(raw.decode("utf-8", "replace")) + except json.JSONDecodeError as exc: + raise HandshakeError(f"the drone's handshake reply was not JSON: {raw!r}") from exc + + status = int(reply.get("status", -1)) + if status != 0: + raise HandshakeError( + f"the drone refused the connection (status {status}). It serves one controller at a " + "time, so close FreeFlight on any phone or tablet holding the link." + ) + if "c2d_port" not in reply: + raise HandshakeError(f"the drone accepted but named no c2d_port: {reply!r}") + return reply + + def _read_until_nul(self, sock: socket.socket) -> bytes: + """The reply is JSON terminated by a NUL, which may need several reads. + + Treating one recv as the whole reply is the usual bug here: TCP is a + stream and the drone's reply has grown over firmware versions. + """ + buf = bytearray() + while b"\x00" not in buf: + try: + chunk = sock.recv(4096) + except TimeoutError as exc: + raise HandshakeError( + f"the drone accepted the TCP connection but sent no reply within {self.timeout}s" + ) from exc + if not chunk: + raise HandshakeError("the drone closed the handshake connection without replying") + buf += chunk + return bytes(buf[: buf.index(b"\x00")]) + + # -- sending --------------------------------------------------------- + def next_seq(self, buffer_id: int) -> int: + """Per-buffer counter, wrapping at 256 as the one-byte field demands.""" + with self._lock: + seq = (self._seq.get(buffer_id, -1) + 1) % 256 + self._seq[buffer_id] = seq + return seq + + def send_frame( + self, + data_type: DataType, + buffer_id: int, + payload: bytes = b"", + *, + seq: int | None = None, + expect_ack: bool = False, + ) -> int: + """Send one frame; returns the sequence number used. + + `seq` is for a retransmission, which must reuse the original sequence + number or the drone treats it as a new command. `expect_ack` registers + the waiter *before* the datagram leaves, because the answer can arrive + while this call is still returning. + """ + if self._udp is None or self.c2d_port is None: + raise NotConnected("the transport is not open") + if seq is None: + seq = self.next_seq(buffer_id) + if expect_ack: + with self._lock: + self._acks.setdefault((buffer_id, seq), threading.Event()).clear() + frame = Frame(data_type, buffer_id, seq, payload) + self._udp.sendto(frame.encode(), (self.ip, self.c2d_port)) + self.frames_out += 1 + return seq + + def wait_ack(self, buffer_id: int, seq: int, timeout: float) -> bool: + """Did the drone acknowledge that frame? Never waits past `timeout`.""" + with self._lock: + event = self._acks.get((buffer_id, seq)) + if event is None: + return False + got = event.wait(timeout) + with self._lock: + self._acks.pop((buffer_id, seq), None) + return got and self.connected + + # -- receiving ------------------------------------------------------- + def _receive_loop(self) -> None: + udp = self._udp + assert udp is not None + while not self._stop.is_set(): + try: + data, _addr = udp.recvfrom(_MAX_DATAGRAM) + except TimeoutError: + # Nothing arrived in this window. pyparrot's equivalent loop + # has fallen through here and re-handled the previous + # datagram, which replays stale telemetry every few seconds + # and hides a link that has actually gone away. + continue + except OSError as exc: + if not self._stop.is_set(): + log.debug("receive socket error: %s", exc) + continue + + for frame in Frame.decode_all(data): + self.frames_in += 1 + try: + self._handle(frame) + except Exception: # pragma: no cover - a bad frame must not kill the link + log.exception("failed to handle frame on buffer %d", frame.buffer_id) + + def _handle(self, frame: Frame) -> None: + if frame.buffer_id == BufferId.PING: + # Echo the payload verbatim: it is the drone's own timestamp and + # it uses the round trip to estimate the link's latency. + self.send_frame(DataType.DATA, BufferId.PONG, frame.payload) + self.pings_answered += 1 + return + + if frame.data_type == DataType.ACK: + # The ack arrives on our buffer + 128 and its single byte is the + # sequence being acknowledged, not the ack's own sequence. + if not frame.payload: + return + acked_seq = frame.payload[0] + origin = (frame.buffer_id - 128) % 256 + with self._lock: + event = self._acks.get((origin, acked_seq)) + if event is not None: + event.set() + return + + if frame.data_type == DataType.DATA_WITH_ACK and frame.buffer_id == BufferId.D2C_ACK: + # Acknowledge before dispatching: the drone retransmits until it + # hears back, and whatever the callback does could be slow. + self.send_frame(DataType.ACK, BufferId.ack_for(frame.buffer_id), bytes([frame.seq])) + + if self.on_frame is not None: + self.on_frame(frame) diff --git a/src/mcbebop/arsdk/discovery.py b/src/mcbebop/arsdk/discovery.py new file mode 100644 index 0000000..9256b63 --- /dev/null +++ b/src/mcbebop/arsdk/discovery.py @@ -0,0 +1,130 @@ +"""Finding the aircraft. + +The Bebop 2 is its own access point, so once this machine has joined that +SSID the drone is always at `192.168.42.1` and discovery is a formality. The +fixed address is therefore the fast path, and mDNS is the fallback for the +case that actually happens: the drone has been joined to an existing network, +or someone changed its address. + +Nothing here opens a session. `probe()` only proves that something is +listening on the discovery port, which is as much as can be learned without +taking the single controller slot. +""" + +from __future__ import annotations + +import logging +import socket +import time +from dataclasses import dataclass + +from mcbebop.arsdk.types import DEFAULT_IP, DISCOVERY_PORT, MDNS_SERVICE + +log = logging.getLogger(__name__) + + +@dataclass(frozen=True) +class Candidate: + """Somewhere a drone might be, and how we came to believe it.""" + + ip: str + port: int = DISCOVERY_PORT + name: str | None = None + via: str = "fixed" # "fixed", "mdns", or "given" + + @property + def address(self) -> tuple[str, int]: + return (self.ip, self.port) + + +def probe(ip: str = DEFAULT_IP, port: int = DISCOVERY_PORT, timeout: float = 1.0) -> bool: + """Is anything accepting TCP on the discovery port? + + A refused connection is a clear "not here"; a timeout usually means the + address is on a network this machine cannot reach. Both are False, and the + timeout is what keeps an unreachable address from costing minutes. + """ + try: + with socket.create_connection((ip, port), timeout=timeout): + return True + except OSError as exc: + log.debug("probe %s:%d failed: %s", ip, port, exc) + return False + + +def browse(timeout: float = 3.0, service: str = MDNS_SERVICE) -> list[Candidate]: + """Browse mDNS for `_arsdk-090c._udp`, the Bebop 2's product id. + + Returns everything seen within `timeout`; an empty list is a normal + answer, because the drone only advertises while it is powered on and the + interface is up. + """ + try: + from zeroconf import ServiceBrowser, ServiceListener, Zeroconf + except ImportError: # pragma: no cover - zeroconf is a hard dependency + log.warning("zeroconf is not installed, so mDNS discovery is unavailable") + return [] + + found: dict[str, Candidate] = {} + + class _Listener(ServiceListener): + # Modern zeroconf calls update_service for every record refresh and + # warns (older versions raised) when a listener does not define it. + # The drone re-announces often, so this is not a rare path. + def _record(self, zc: Zeroconf, type_: str, name: str) -> None: + info = zc.get_service_info(type_, name, timeout=int(timeout * 1000)) + if info is None: + return + for addr in info.parsed_addresses(): + if ":" in addr: # ARSDK discovery is IPv4 only + continue + found[name] = Candidate(ip=addr, port=info.port or DISCOVERY_PORT, name=name, via="mdns") + break + + def add_service(self, zc: Zeroconf, type_: str, name: str) -> None: + self._record(zc, type_, name) + + def update_service(self, zc: Zeroconf, type_: str, name: str) -> None: + self._record(zc, type_, name) + + def remove_service(self, zc: Zeroconf, type_: str, name: str) -> None: + found.pop(name, None) + + zc = Zeroconf() + try: + ServiceBrowser(zc, service, _Listener()) + # ServiceBrowser works on its own thread; there is nothing to await, + # so the browse window is simply how long we let it run. + time.sleep(timeout) + finally: + zc.close() + + log.debug("mDNS browse found %d candidate(s)", len(found)) + return list(found.values()) + + +def find( + ip: str | None = None, + *, + port: int = DISCOVERY_PORT, + timeout: float = 1.0, + mdns: bool = True, +) -> Candidate | None: + """The one the caller should connect to, or None. + + An explicit `ip` is trusted without probing, because a caller who names an + address wants the connection attempt's error, not a discovery verdict. + """ + if ip is not None: + return Candidate(ip=ip, port=port, via="given") + + if probe(DEFAULT_IP, port, timeout=timeout): + return Candidate(ip=DEFAULT_IP, port=port, via="fixed") + + if not mdns: + return None + + for cand in browse(timeout=max(timeout, 2.0)): + if probe(cand.ip, cand.port, timeout=timeout): + return cand + return None diff --git a/src/mcbebop/arsdk/session.py b/src/mcbebop/arsdk/session.py new file mode 100644 index 0000000..23df17c --- /dev/null +++ b/src/mcbebop/arsdk/session.py @@ -0,0 +1,469 @@ +"""The long-lived drone session: transport plus everything we know about it. + +A session is the only stateful object in the package. It owns a `Connection`, +decodes every event the drone sends into a flat telemetry store, and turns a +`CommandSpec` plus arguments into a frame on the right buffer. + +Two design points worth stating, because both are load-bearing: + +The telemetry store keeps a timestamp per key rather than only a value. A +Bebop 2 that has stopped reporting looks identical to one reporting the same +number, and an agent asking "is it still landed?" deserves to know the answer +is four minutes old. + +Nothing here waits forever. Every wait is bounded and every bound is a +constructor argument, because the failure this transport actually exhibits is +a link that goes quiet rather than one that refuses. +""" + +from __future__ import annotations + +import asyncio +import logging +import threading +import time +from collections.abc import Callable, Iterable +from dataclasses import dataclass, field +from typing import Any + +from mcbebop.arsdk.connection import Connection +from mcbebop.arsdk.types import ( + COMMAND_HEADER, + D2C_PORT, + DEFAULT_IP, + DISCOVERY_PORT, + BufferId, + DataType, + Event, + Frame, + NotConnected, +) +from mcbebop.protocol.types import Buffer, CommandSpec + +log = logging.getLogger(__name__) + +# Encoders and decoders are injectable so this module can be exercised +# without the protocol layer, and so a test can watch exactly what was +# encoded. The defaults reach for `mcbebop.protocol.codec` on first use. +Encoder = Callable[[CommandSpec, dict[str, Any]], bytes] +Decoder = Callable[[bytes], Any] + +# AllStates and AllSettings are spelled out rather than looked up in the +# protocol index: they are the two commands a session must be able to send +# before anything else works, their ids are recorded in bebop-2's +# protocol.md, and neither takes arguments. +ALL_STATES = CommandSpec(project="common", klass="Common", name="AllStates", ids=(0, 4, 0)) +ALL_SETTINGS = CommandSpec(project="common", klass="Settings", name="AllSettings", ids=(0, 2, 0)) + + +@dataclass(frozen=True) +class Timeouts: + """Every bound in one place, so none of them is a literal in a method.""" + + handshake: float = 5.0 + ack: float = 1.0 # one attempt's wait for an acknowledgement + ack_attempts: int = 3 # including the first, so two retransmissions + confirm: float = 2.0 # wait for the drone's own report of the change + full_state: float = 3.0 # how long request_full_state lets events stream in + + +@dataclass +class Reading: + """One telemetry key: the value, and when the drone last said so.""" + + value: Any + at: float # time.monotonic() + count: int = 1 + + def age(self, now: float | None = None) -> float: + return (now if now is not None else time.monotonic()) - self.at + + +@dataclass +class _Waiter: + """A one-shot wait for a confirming event.""" + + ids: set[tuple[int, int, int]] + predicate: Callable[[Event], bool] + event: threading.Event = field(default_factory=threading.Event) + hit: Event | None = None + + +def _default_encoder(spec: CommandSpec, args: dict[str, Any]) -> bytes: + from mcbebop.protocol.codec import encode_args # deferred: parallel stream + + return encode_args(spec, args) + + +def _default_decoder(payload: bytes) -> Any: + from mcbebop.protocol.codec import decode_event # deferred: parallel stream + + return decode_event(payload) + + +def _as_event(decoded: Any, payload: bytes) -> Event | None: + """Normalise whatever the codec returned into an `Event`. + + The codec is written by another stream against the same `Event` type, so + the first branch is the expected one. The rest exist because a decoder + that returns a plain tuple or mapping is a reasonable thing to write, and + a transport that crashes on the shape of a decode result would be a silly + way to lose a link. + """ + if decoded is None: + return None + if isinstance(decoded, Event): + return decoded + ids = getattr(decoded, "ids", None) + name = getattr(decoded, "name", None) + values = getattr(decoded, "values", None) + if ids is None and isinstance(decoded, dict): + ids, name, values = decoded.get("ids"), decoded.get("name"), decoded.get("values") + if ids is None and isinstance(decoded, tuple | list) and len(decoded) == 3: + ids, name, values = decoded + if ids is None: + log.debug("decoder returned an unusable shape %r for %s", type(decoded), payload[:8].hex()) + return None + if name is None: + name = f"cmd_{tuple(ids)}" + return Event( + ids=tuple(ids), # type: ignore[arg-type] + name=str(name), + values=dict(values or {}), + at=getattr(decoded, "at", None) or time.monotonic(), + ) + + +class DroneSession: + """What the tools layer holds. Implements the `Session` protocol.""" + + def __init__( + self, + ip: str = DEFAULT_IP, + *, + discovery_port: int = DISCOVERY_PORT, + d2c_port: int = D2C_PORT, + controller_name: str = "mcbebop", + timeouts: Timeouts | None = None, + encoder: Encoder | None = None, + decoder: Decoder | None = None, + ) -> None: + self.ip = ip + self.discovery_port = discovery_port + self._d2c_port = d2c_port + self.controller_name = controller_name + self.timeouts = timeouts or Timeouts() + self._encode = encoder or _default_encoder + self._decode = decoder or _default_decoder + + self._conn: Connection | None = None + self._lock = threading.Lock() # guards the store, the waiters, the subscribers + self._store: dict[str, Reading] = {} + self._waiters: list[_Waiter] = [] + self._subscribers: list[Callable[[Event], None]] = [] + self._undecodable = 0 + self.connected_at: float | None = None + + # -- lifecycle ------------------------------------------------------- + @property + def connected(self) -> bool: + return self._conn is not None and self._conn.connected + + @property + def d2c_port(self) -> int: + """The port we are actually receiving on, which a 0 makes the OS pick.""" + return self._conn.d2c_port if self._conn is not None else self._d2c_port + + @property + def handshake(self) -> dict[str, Any]: + return dict(self._conn.handshake) if self._conn is not None else {} + + async def connect(self) -> dict[str, Any]: + """Open the link. Blocking socket work goes to a worker thread.""" + if self.connected: + raise RuntimeError(f"already connected to {self.ip}") + conn = Connection( + self.ip, + discovery_port=self.discovery_port, + d2c_port=self._d2c_port, + controller_name=self.controller_name, + timeout=self.timeouts.handshake, + on_frame=self._on_frame, + ) + reply = await asyncio.to_thread(conn.connect) + self._conn = conn + self.connected_at = time.monotonic() + return dict(reply) + + async def disconnect(self) -> None: + conn, self._conn = self._conn, None + self.connected_at = None + if conn is not None: + await asyncio.to_thread(conn.disconnect) + with self._lock: + for waiter in self._waiters: + waiter.event.set() # unblock; the caller sees no hit + self._waiters.clear() + + # -- telemetry ------------------------------------------------------- + def _on_frame(self, frame: Frame) -> None: + """Called from the receive thread for every data frame.""" + if frame.buffer_id not in (BufferId.D2C_ACK, BufferId.D2C_NON_ACK): + return # video and ack buffers are not events + if len(frame.payload) < COMMAND_HEADER.size: + return + try: + event = _as_event(self._decode(frame.payload), frame.payload) + except Exception: + # An event we cannot decode is a gap in coverage, not a reason to + # drop the link: the next frame may be the one that matters. + self._undecodable += 1 + log.debug("undecodable event payload %s", frame.payload[:8].hex(), exc_info=True) + return + if event is None: + self._undecodable += 1 + return + self._record(event) + + def _record(self, event: Event) -> None: + with self._lock: + for key, value in event.values.items(): + prior = self._store.get(key) + self._store[key] = Reading(value, event.at, (prior.count + 1) if prior else 1) + hit = [w for w in self._waiters if event.ids in w.ids and w.predicate(event)] + for waiter in hit: + waiter.hit = event + waiter.event.set() + self._waiters.remove(waiter) + subscribers = list(self._subscribers) + + for fn in subscribers: + try: + fn(event) + except Exception: # pragma: no cover - a subscriber must not break telemetry + log.exception("telemetry subscriber raised") + + def state(self, keys: Iterable[str] | None = None) -> dict[str, Any]: + """Current telemetry, each value with how stale it is. + + `keys` may name exact keys or prefixes (`"BatteryStateChanged"` finds + `BatteryStateChanged_percent`), because the `_` keys are + not what an agent naturally asks for. + """ + now = time.monotonic() + with self._lock: + items = dict(self._store) + if keys is not None: + wanted = list(keys) + items = {k: v for k, v in items.items() if k in wanted or any(k.startswith(w) for w in wanted)} + return {k: {"value": v.value, "age": round(v.age(now), 3)} for k, v in sorted(items.items())} + + def values(self, keys: Iterable[str] | None = None) -> dict[str, Any]: + """The same thing without the staleness wrapper, for internal checks.""" + return {k: v["value"] for k, v in self.state(keys).items()} + + def subscribe(self, fn: Callable[[Event], None]) -> Callable[[], None]: + """Push every decoded event to `fn`. Returns the unsubscribe. + + `fn` runs on the receive thread, so it should hand work off rather + than do it: anything slow here delays pings and acknowledgements. + """ + with self._lock: + self._subscribers.append(fn) + + def unsubscribe() -> None: + with self._lock: + if fn in self._subscribers: + self._subscribers.remove(fn) + + return unsubscribe + + def link_stats(self) -> dict[str, Any]: + """Enough to tell a quiet link from a broken one.""" + conn = self._conn + now = time.monotonic() + with self._lock: + last = max((r.at for r in self._store.values()), default=None) + keys = len(self._store) + return { + "connected": self.connected, + "ip": self.ip, + "c2d_port": conn.c2d_port if conn else None, + "uptime": round(now - self.connected_at, 1) if self.connected_at else None, + "frames_in": conn.frames_in if conn else 0, + "frames_out": conn.frames_out if conn else 0, + "pings_answered": conn.pings_answered if conn else 0, + "telemetry_keys": keys, + "last_event_age": round(now - last, 3) if last is not None else None, + "undecodable_events": self._undecodable, + } + + # -- sending --------------------------------------------------------- + def _route(self, spec: CommandSpec) -> tuple[DataType, int]: + """Which frame type and buffer a command travels on. + + The emergency buffer is acknowledged, unlike pyparrot's LOW_LATENCY + guess: libARController configures buffer 12 as DATA_WITH_ACK with + unlimited retries, which is the behaviour you want from the command + that cuts the motors. + """ + if spec.buffer == Buffer.NON_ACK: + return DataType.DATA, BufferId.C2D_NON_ACK + if spec.buffer == Buffer.HIGH_PRIO: + return DataType.DATA_WITH_ACK, BufferId.C2D_HIGH_PRIO + return DataType.DATA_WITH_ACK, BufferId.C2D_ACK + + async def send( + self, spec: CommandSpec, args: dict[str, Any] | None = None, *, confirm: bool = True + ) -> dict[str, Any]: + """Encode, send, and report what came back. + + The acknowledgement proves the drone's network layer received the + frame. It does not prove the drone did anything, which is what + `confirm` is for: 82 of the 101 sendable commands declare an event + that reports the change, and that event is the real answer. + """ + if self._conn is None or not self._conn.connected: + raise NotConnected("not connected to a drone") + args = dict(args or {}) + data_type, buffer_id = self._route(spec) + payload = COMMAND_HEADER.pack(*spec.ids) + self._encode(spec, args) + + waiter = self._arm_confirmation(spec, args) if confirm else None + started = time.monotonic() + seq = -1 + acked: bool | None = None + try: + if data_type == DataType.DATA: + seq = self._conn.send_frame(data_type, buffer_id, payload) + else: + seq, acked = await self._send_with_ack(data_type, buffer_id, payload) + confirmed = await self._await_confirmation(waiter) if waiter is not None else None + finally: + self._disarm(waiter) + + result: dict[str, Any] = { + "command": spec.full_name, + "ids": list(spec.ids), + "args": args, + "buffer": int(buffer_id), + "seq": seq, + "acked": acked, + "elapsed_ms": round((time.monotonic() - started) * 1000, 1), + } + if confirm: + # Always present when confirmation was asked for, so a caller can + # read it without first working out whether the command declares + # an expectation. + result["confirmed"] = confirmed + if waiter is None: + result["note"] = "this command declares no confirming event, so the ack is all there is" + elif confirmed is None: + # Said plainly, because "sent but unconfirmed" is a real and + # common state: the drone takes commands it then ignores. + result["note"] = ( + "the drone acknowledged the frame but did not report the change within " + f"{self.timeouts.confirm}s" + if acked + else "no acknowledgement and no confirming event" + ) + return result + + async def _send_with_ack(self, data_type: DataType, buffer_id: int, payload: bytes) -> tuple[int, bool]: + """Send and wait, retransmitting the same sequence number on silence.""" + assert self._conn is not None + seq = self._conn.send_frame(data_type, buffer_id, payload, expect_ack=True) + for attempt in range(self.timeouts.ack_attempts): + if attempt: + # Same sequence number: a fresh one would read as a second + # command, which for a takeoff is not a harmless duplicate. + self._conn.send_frame(data_type, buffer_id, payload, seq=seq, expect_ack=True) + log.debug("retransmitting buffer %d seq %d (attempt %d)", buffer_id, seq, attempt + 1) + if await asyncio.to_thread(self._conn.wait_ack, buffer_id, seq, self.timeouts.ack): + return seq, True + if not self._conn.connected: + break + return seq, False + + def _arm_confirmation(self, spec: CommandSpec, args: dict[str, Any]) -> _Waiter | None: + """Register interest in the events this command says it will cause. + + Registered before the frame goes out: the Bebop 2 answers some + commands faster than the send call returns. + """ + ids: set[tuple[int, int, int]] = set() + checks: dict[tuple[int, int, int], dict[str, Any]] = {} + for expectation in _flatten(spec.expectations): + ids.add(tuple(expectation.ids)) + checks[tuple(expectation.ids)] = { + name: args[want[5:]] + for name, want in expectation.fields.items() + # `this.` means "echoes what you sent", which is the only + # half of the expectation grammar worth asserting on: enum + # literals are spelled as member names and the decoder is free + # to report them as either name or number. + if want.startswith("this.") and want[5:] in args + } + if not ids: + return None + + def predicate(event: Event) -> bool: + for name, expected in checks.get(event.ids, {}).items(): + got = event.values.get(f"{event.name}_{name}", event.values.get(name)) + if got is not None and got != expected: + return False + return True + + waiter = _Waiter(ids=ids, predicate=predicate) + with self._lock: + self._waiters.append(waiter) + return waiter + + async def _await_confirmation(self, waiter: _Waiter) -> dict[str, Any] | None: + got = await asyncio.to_thread(waiter.event.wait, self.timeouts.confirm) + if not got or waiter.hit is None: + return None + return {"event": waiter.hit.name, "ids": list(waiter.hit.ids), "values": waiter.hit.values} + + def _disarm(self, waiter: _Waiter | None) -> None: + if waiter is None: + return + with self._lock: + if waiter in self._waiters: + self._waiters.remove(waiter) + + async def request_full_state(self) -> dict[str, Any]: + """Ask for everything, then let it arrive. + + The drone answers these two with a burst of a hundred-odd events, and + there is no "that's all" marker, so the only honest finish condition + is a short quiet period. + """ + sent = [ + await self.send(ALL_STATES, {}, confirm=False), + await self.send(ALL_SETTINGS, {}, confirm=False), + ] + deadline = time.monotonic() + self.timeouts.full_state + quiet_for = 0.4 + while time.monotonic() < deadline: + with self._lock: + last = max((r.at for r in self._store.values()), default=0.0) + if last and time.monotonic() - last > quiet_for: + break + await asyncio.sleep(0.1) + with self._lock: + keys = len(self._store) + return { + "sent": [s["command"] for s in sent], + "acked": [s["acked"] for s in sent], + "telemetry_keys": keys, + } + + +def _flatten(expectations: Iterable[Any]) -> list[Any]: + """Expectations nest: `alternatives` holds the other acceptable events.""" + out: list[Any] = [] + for expectation in expectations: + out.append(expectation) + out.extend(_flatten(getattr(expectation, "alternatives", ()) or ())) + return out diff --git a/src/mcbebop/sim.py b/src/mcbebop/sim.py new file mode 100644 index 0000000..beb5494 --- /dev/null +++ b/src/mcbebop/sim.py @@ -0,0 +1,441 @@ +"""A fake Bebop 2 that speaks ARSDK3 on localhost. + +Ported from bebop-2's `sim.py`, which was written against the live aircraft's +traffic. It answers the discovery handshake, pings the controller, acks what +it is sent, and streams a plausible landed-on-the-bench state, so everything +above the transport can be developed and tested without an aircraft. + +This ships in the package rather than in `tests/` because `connect(target= +"sim")` is a documented way to rehearse a command that would otherwise move a +real drone. + +Two deliberate properties: + +Event payloads are encoded from Parrot's own XML, so ids and argument types +match what a real Bebop 2 sends rather than what this package expects to +receive. A simulator that encoded through the same assumptions as the client +would agree with it about a shared mistake. + +One deliberate fault is baked in: the magnetometer self-test reports failure, +because "all six sensors fine" is the one answer that never exercises the code +that reads them. +""" + +from __future__ import annotations + +import json +import logging +import math +import socket +import struct +import threading +import time +import xml.etree.ElementTree as ET +from dataclasses import dataclass, field +from pathlib import Path +from typing import Any + +from mcbebop.arsdk.types import COMMAND_HEADER, FRAME_HEADER, BufferId, DataType +from mcbebop.protocol.types import ArgSpec, Buffer, CommandSpec, EnumSpec + +log = logging.getLogger(__name__) + +# The sim is the drone side, so it must put the wire value on the wire itself +# rather than rely on the client encoder being lenient about enum spellings. +_FORMATS = { + "u8": " Path: + """Where Parrot's XML lives. + + Checked in the order that survives both a checkout and an install: inside + the package first, because that is where it has to end up for a wheel to + be useful, then the repository root it is vendored at today. + """ + here = Path(__file__).resolve() + candidates = [here.parent / "arsdk-xml", here.parents[2] / "arsdk-xml", here.parents[3] / "arsdk-xml"] + for path in candidates: + if (path / "common.xml").is_file(): + return path + raise FileNotFoundError(f"cannot find arsdk-xml; looked in {[str(c) for c in candidates]}") + + +def load_specs(root: Path | None = None) -> dict[str, CommandSpec]: + """Index every command in the vendored XML by `project.Class.Cmd`. + + Fully qualified because the two projects reuse class names: `SettingsState` + exists in both `common` and `ardrone3`, with different commands in each. + + Only what the sim needs: ids, argument types, enum members, and the + buffer. Expectations, tiers and support strings are the protocol layer's + business, and a drone has no use for them. + """ + root = root or _xml_root() + specs: dict[str, CommandSpec] = {} + for filename in ("common.xml", "ardrone3.xml"): + project = ET.parse(root / filename).getroot() + pid = int(project.get("id", "0")) + pname = project.get("name", "") + for klass in project.iter("class"): + cid = int(klass.get("id", "0")) + for cmd in klass.findall("cmd"): + args = tuple( + ArgSpec( + name=arg.get("name", ""), + type=arg.get("type", "u8"), + members=tuple( + EnumSpec(name=e.get("name", ""), value=i) + for i, e in enumerate(arg.findall("enum")) + ), + ) + for arg in cmd.findall("arg") + ) + # The buffer attribute is absent on 248 of the 264 commands + # and means the acknowledged buffer. It is read here because + # it decides the frame type, not because the sim sends it. + buffer = cmd.get("buffer") + spec = CommandSpec( + project=pname, + klass=klass.get("name", ""), + name=cmd.get("name", ""), + ids=(pid, cid, int(cmd.get("id", "0"))), + args=args, + buffer=Buffer(buffer.lower()) if buffer else Buffer.ACK, + ) + specs[spec.full_name] = spec + return specs + + +def _wire_value(arg: ArgSpec, value: Any) -> Any: + """Enum members travel as their position among siblings.""" + if arg.is_enum and isinstance(value, str): + for member in arg.members: + if member.name == value: + return member.value + raise KeyError(f"{value!r} is not a member of {arg.name}") + return value + + +def _fallback_encode_args(spec: CommandSpec, values: dict[str, Any]) -> bytes: + """Enough of an encoder for the events the sim emits. + + Used until `mcbebop.protocol.codec` exists, and as the answer to "did the + sim break or did the codec?" afterwards. Lists and bitfields are not here + because no event the sim sends needs them. + """ + out = bytearray() + for arg in spec.args: + value = values[arg.name] + if arg.type == "string": + out += str(value).encode() + b"\x00" + else: + out += struct.pack(_FORMATS[arg.type], _wire_value(arg, value)) + return bytes(out) + + +def _encode_args(spec: CommandSpec, values: dict[str, Any]) -> bytes: + try: + from mcbebop.protocol.codec import encode_args # deferred: parallel stream + except ImportError: + return _fallback_encode_args(spec, values) + return encode_args(spec, {k: _wire_value(_arg(spec, k), v) for k, v in values.items()}) + + +def _arg(spec: CommandSpec, name: str) -> ArgSpec: + for arg in spec.args: + if arg.name == name: + return arg + raise KeyError(f"{spec.full_name} has no argument {name!r}") + + +@dataclass +class FakeBebop: + """A drone-shaped thing on a socket. + + Use it as a context manager; the threads are daemons but the sockets are + not, so leaving it open leaks a port. + """ + + host: str = "127.0.0.1" + discovery_port: int = 0 # 0 asks the OS, which is what parallel tests want + c2d_port: int = 0 + single_controller: bool = True + stream_hz: float = 5.0 + battery_start: int = 87 + + received: list[Received] = field(default_factory=list) + handshakes: list[dict[str, Any]] = field(default_factory=list) + pongs: int = 0 + + def __post_init__(self) -> None: + self.specs = load_specs() + self._stop = threading.Event() + self._seq: dict[int, int] = {} + self._d2c: tuple[str, int] | None = None + self._lock = threading.Lock() + self._battery = self.battery_start + + self._udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._udp.bind((self.host, self.c2d_port)) + self.c2d_port = self._udp.getsockname()[1] + self._udp.settimeout(0.2) + + self._tcp = socket.create_server((self.host, self.discovery_port)) + self.discovery_port = self._tcp.getsockname()[1] + self._tcp.settimeout(0.2) + + self._threads = [ + threading.Thread(target=target, name=f"sim-{name}", daemon=True) + for name, target in ( + ("discovery", self._discovery_loop), + ("commands", self._command_loop), + ("stream", self._stream_loop), + ) + ] + + # -- lifecycle ------------------------------------------------------- + def __enter__(self) -> FakeBebop: + self.start() + return self + + def __exit__(self, *exc: object) -> None: + self.stop() + + def start(self) -> FakeBebop: + for thread in self._threads: + thread.start() + log.info("sim listening on %s:%d (c2d %d)", self.host, self.discovery_port, self.c2d_port) + return self + + def stop(self) -> None: + self._stop.set() + for thread in self._threads: + thread.join(timeout=2) + self._udp.close() + self._tcp.close() + + @property + def occupied(self) -> bool: + return self._d2c is not None + + def release(self) -> None: + """Forget the current controller, as a real drone does on link loss.""" + self._d2c = None + + def wait_for_controller(self, timeout: float = 5.0) -> bool: + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + if self._d2c is not None: + return True + time.sleep(0.02) + return False + + def received_ids(self) -> list[tuple[int, int, int]]: + return [r.ids for r in self.received] + + # -- transport ------------------------------------------------------- + def _send(self, data_type: int, buffer_id: int, payload: bytes) -> None: + target = self._d2c + if target is None: + return + with self._lock: + seq = self._seq[buffer_id] = (self._seq.get(buffer_id, -1) + 1) % 256 + frame = FRAME_HEADER.pack(data_type, buffer_id, seq, FRAME_HEADER.size + len(payload)) + payload + try: + self._udp.sendto(frame, target) + except OSError as exc: # the socket can close under us at teardown + log.debug("sim send failed: %s", exc) + + def emit(self, event: str, *, acked: bool = False, **values: Any) -> None: + """Send one drone-to-controller event, named `project.Class.Cmd`.""" + spec = self.specs[event] + payload = COMMAND_HEADER.pack(*spec.ids) + _encode_args(spec, values) + if acked: + self._send(DataType.DATA_WITH_ACK, BufferId.D2C_ACK, payload) + else: + self._send(DataType.DATA, BufferId.D2C_NON_ACK, payload) + + # -- threads --------------------------------------------------------- + def _discovery_loop(self) -> None: + while not self._stop.is_set(): + try: + conn, addr = self._tcp.accept() + except (TimeoutError, OSError): + continue + with conn: + try: + request = json.loads(conn.recv(4096).decode()) + except (ValueError, OSError) as exc: + log.debug("sim got a malformed handshake: %s", exc) + continue + self.handshakes.append(request) + if self.single_controller and self.occupied: + # What the aircraft does to a second controller, and the + # only way to test that path without two phones. + conn.sendall(json.dumps({"status": 1}).encode() + b"\x00") + continue + self._d2c = (addr[0], int(request["d2c_port"])) + reply = { + "status": 0, + "c2d_port": self.c2d_port, + "arstream_fragment_size": 65000, + "arstream_fragment_maximum_number": 128, + "arstream_max_ack_interval": -1, + "arstream2_server_stream_port": 5004, + "arstream2_server_control_port": 5005, + } + conn.sendall(json.dumps(reply).encode() + b"\x00") + # The burst goes out per controller attach, not once per + # process: a reconnecting controller knows nothing, and the + # aircraft tells it everything again. + self._identity() + + def _command_loop(self) -> None: + while not self._stop.is_set(): + try: + data, _addr = self._udp.recvfrom(65535) + except (TimeoutError, OSError): + continue # a quiet window is not a reason to re-handle the last one + while len(data) >= FRAME_HEADER.size: + data_type, buffer_id, seq, size = FRAME_HEADER.unpack_from(data) + if size < FRAME_HEADER.size or size > len(data): + break + self._on_frame(data_type, buffer_id, seq, data[FRAME_HEADER.size : size]) + data = data[size:] + + def _on_frame(self, data_type: int, buffer_id: int, seq: int, body: bytes) -> None: + if data_type == DataType.DATA_WITH_ACK: + self._send(DataType.ACK, BufferId.ack_for(buffer_id), bytes([seq])) + if buffer_id == BufferId.PONG: + self.pongs += 1 + return + if buffer_id not in _C2D_BUFFERS or len(body) < COMMAND_HEADER.size: + return + ids = COMMAND_HEADER.unpack_from(body) + self.received.append(Received(ids, body[COMMAND_HEADER.size :], buffer_id, int(data_type), int(seq))) + if ids in (_ALL_STATES, _ALL_SETTINGS): + # The real drone answers these with a burst of its current state, + # which is what makes request_full_state worth calling. + self._identity() + + def _identity(self) -> None: + """Everything the aircraft volunteers once a controller attaches.""" + self.emit("common.SettingsState.ProductNameChanged", acked=True, name="Bebop2-SIM") + self.emit( + "common.SettingsState.ProductVersionChanged", acked=True, software="4.7.1", hardware="HW_05" + ) + self.emit("common.SettingsState.ProductSerialHighChanged", acked=True, high="PI04") + self.emit("common.SettingsState.ProductSerialLowChanged", acked=True, low="0000000SIM") + self.emit("ardrone3.SettingsState.MotorFlightsStatusChanged", acked=True, + nbFlights=42, lastFlightDuration=611, totalFlightDuration=26_340) # fmt: skip + self.emit("common.CommonState.MassStorageInfoStateListChanged", acked=True, + mass_storage_id=0, size=7500, used_size=1830, plugged=1, full=0, internal=1) # fmt: skip + self.emit( + "ardrone3.PilotingSettingsState.MaxAltitudeChanged", + acked=True, + current=50.0, + min=0.5, + max=150.0, + ) + self.emit( + "ardrone3.PilotingSettingsState.MaxDistanceChanged", + acked=True, + current=100.0, + min=10.0, + max=2000.0, + ) + self.emit( + "ardrone3.PilotingSettingsState.NoFlyOverMaxDistanceChanged", acked=True, shouldNotFlyOver=1 + ) + self.emit("ardrone3.SpeedSettingsState.OutdoorChanged", acked=True, outdoor=1) + self.emit("ardrone3.PilotingState.FlyingStateChanged", acked=True, state="landed") + self.emit("common.CommonState.BatteryStateChanged", acked=True, percent=self._battery) + self.emit("ardrone3.PilotingState.AlertStateChanged", acked=True, state="none") + self.emit( + "ardrone3.SettingsState.MotorErrorStateChanged", acked=True, motorIds=0, motorError="noError" + ) + self.emit("ardrone3.GPSSettingsState.GPSFixStateChanged", acked=True, fixed=0) + self.emit("ardrone3.GPSState.HomeTypeChosenChanged", acked=True, type="TAKEOFF") + for sensor in _SENSORS: + self.emit("common.CommonState.SensorsStatesListChanged", acked=True, + sensorName=sensor, sensorState=0 if sensor == _FAULTY_SENSOR else 1) # fmt: skip + + def _stream_loop(self) -> None: + while self._d2c is None and not self._stop.is_set(): + time.sleep(0.02) + if self._stop.is_set(): + return + t0, tick, period = time.monotonic(), 0, 1.0 / self.stream_hz + while not self._stop.wait(period): + t = time.monotonic() - t0 + tick += 1 + # The drone pings us, not the other way round; the payload is its + # own clock and the controller echoes it back untouched. + self._send(DataType.DATA, BufferId.PING, struct.pack(" None: + """Run a sim until interrupted. Logs; nothing goes to stdout. + + stdout is the MCP server's JSON-RPC transport, and this module is + importable from it. + """ + with FakeBebop(host=host, discovery_port=discovery_port) as sim: + log.info("fake Bebop 2 on %s:%d", sim.host, sim.discovery_port) + end = time.monotonic() + seconds if seconds else None + try: + while end is None or time.monotonic() < end: + time.sleep(0.25) + except KeyboardInterrupt: + pass diff --git a/tests/test_arsdk_framing.py b/tests/test_arsdk_framing.py new file mode 100644 index 0000000..869b9f6 --- /dev/null +++ b/tests/test_arsdk_framing.py @@ -0,0 +1,109 @@ +"""Framing, with no sockets involved. + +These are the cases that cost real debugging time on the live aircraft: a +datagram carrying several frames, a size field that would never advance the +cursor, and the command id being two bytes rather than one. +""" + +import struct + +import pytest + +from mcbebop.arsdk.types import COMMAND_HEADER, FRAME_HEADER, BufferId, DataType, Frame + + +def test_header_is_seven_bytes(): + # Everything about the size field depends on this, including the sim. + assert FRAME_HEADER.size == 7 + assert COMMAND_HEADER.size == 4 + + +def test_encode_declares_size_including_header(): + frame = Frame(DataType.DATA, BufferId.C2D_NON_ACK, 3, b"abcd") + raw = frame.encode() + data_type, buffer_id, seq, size = FRAME_HEADER.unpack_from(raw) + assert (data_type, buffer_id, seq) == (2, 10, 3) + assert size == len(raw) == 11 + + +def test_round_trip(): + frame = Frame(DataType.DATA_WITH_ACK, BufferId.C2D_ACK, 255, bytes(range(16))) + assert Frame.decode_all(frame.encode()) == [frame] + + +def test_empty_payload_round_trips(): + frame = Frame(DataType.ACK, 139, 0, b"") + assert Frame.decode_all(frame.encode()) == [frame] + + +def test_several_frames_in_one_datagram(): + frames = [ + Frame(DataType.DATA, BufferId.D2C_NON_ACK, 1, b"\x01\x02"), + Frame(DataType.DATA_WITH_ACK, BufferId.D2C_ACK, 2, b"xyz"), + Frame(DataType.DATA, BufferId.PING, 3, struct.pack(" Event: + """Decode a drone event the way the real codec is specified to. + + Keys are `_`, matching the captures in bebop-2's notes. + """ + ids = COMMAND_HEADER.unpack_from(payload) + spec = _BY_IDS[ids] + offset = COMMAND_HEADER.size + values = {} + for arg in spec.args: + if arg.type == "string": + end = payload.index(b"\x00", offset) + value = payload[offset:end].decode() + offset = end + 1 + else: + fmt = _FORMATS[arg.type] + (value,) = struct.unpack_from(fmt, payload, offset) + offset += struct.calcsize(fmt) + if arg.is_enum and 0 <= value < len(arg.members): + value = arg.members[value].name + values[f"{spec.name}_{arg.name}"] = value + return Event(ids=ids, name=spec.name, values=values, at=time.monotonic()) + + +def quick_timeouts() -> Timeouts: + """Short but not instant: the sim streams at 5 Hz.""" + return Timeouts(handshake=2.0, ack=0.5, ack_attempts=2, confirm=1.5, full_state=1.5) + + +async def until(session: DroneSession, key: str, timeout: float = 3.0): + deadline = time.monotonic() + timeout + while time.monotonic() < deadline: + values = session.values() + if key in values: + return values[key] + await asyncio.sleep(0.05) + pytest.fail(f"{key} never arrived; saw {sorted(session.values())}") + + +@pytest.fixture +def sim(): + with FakeBebop() as fake: + yield fake + + +@pytest.fixture +async def session(sim): + drone = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, # the OS picks, so tests can run side by side + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=fake_decode, + ) + await drone.connect() + yield drone + await drone.disconnect() + + +# -- handshake ----------------------------------------------------------- +async def test_handshake_yields_a_c2d_port(sim, session): + assert session.connected + assert session.handshake["status"] == 0 + assert session.handshake["c2d_port"] == sim.c2d_port + + +async def test_handshake_describes_this_controller(sim, session): + request = sim.handshakes[0] + assert request["controller_name"] == "mcbebop" + assert request["controller_type"] == "computer" + # The video ports are named in the handshake, not in a URL: firmware + # 4.7.1 serves no RTSP. + assert request["arstream2_client_stream_port"] == 55004 + assert request["arstream2_client_control_port"] == 55005 + assert request["d2c_port"] == session.d2c_port != 0 + + +async def test_second_controller_is_refused(sim, session): + other = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=fake_decode, + ) + with pytest.raises(HandshakeError, match="one controller"): + await other.connect() + assert not other.connected + + +async def test_unreachable_address_fails_fast_rather_than_hanging(): + drone = DroneSession("127.0.0.1", discovery_port=1, timeouts=Timeouts(handshake=1.0), d2c_port=0) + started = time.monotonic() + with pytest.raises(HandshakeError): + await drone.connect() + assert time.monotonic() - started < 3.0 + + +# -- telemetry ----------------------------------------------------------- +async def test_identity_and_attitude_populate_state(session): + assert await until(session, "ProductVersionChanged_software") == "4.7.1" + assert await until(session, "ProductVersionChanged_hardware") == "HW_05" + assert await until(session, "BatteryStateChanged_percent") == 87 + assert await until(session, "FlyingStateChanged_state") == "landed" + assert abs(await until(session, "AttitudeChanged_roll")) < 0.1 + + +async def test_gps_reports_the_no_position_sentinel(session): + assert await until(session, "PositionChanged_latitude", timeout=4.0) == 500.0 + assert await until(session, "GPSFixStateChanged_fixed") == 0 + + +async def test_state_reports_staleness(session): + await until(session, "BatteryStateChanged_percent") + reading = session.state(["BatteryStateChanged"])["BatteryStateChanged_percent"] + assert reading["value"] == 87 + first = reading["age"] + await asyncio.sleep(0.3) + # The same value, older. That difference is the whole reason for the + # timestamp: a drone that has stopped talking reads identically without it. + assert session.state(["BatteryStateChanged"])["BatteryStateChanged_percent"]["age"] > first + + +async def test_state_filters_by_key_or_prefix(session): + await until(session, "AttitudeChanged_roll") + assert set(session.state(["AttitudeChanged_roll"])) == {"AttitudeChanged_roll"} + assert set(session.state(["AttitudeChanged"])) >= { + "AttitudeChanged_roll", + "AttitudeChanged_pitch", + "AttitudeChanged_yaw", + } + assert session.state(["NothingLikeThis"]) == {} + + +async def test_subscribe_pushes_events_and_unsubscribe_stops_them(session): + seen: list[str] = [] + stop = session.subscribe(lambda event: seen.append(event.name)) + await until(session, "AttitudeChanged_yaw") + await asyncio.sleep(0.3) + assert "AttitudeChanged" in seen + stop() + count = len(seen) + await asyncio.sleep(0.4) + assert len(seen) == count + + +async def test_ping_is_answered(sim, session): + await until(session, "AttitudeChanged_roll") + await asyncio.sleep(0.3) + # Both halves: we sent a pong, and the drone side received it. + assert session.link_stats()["pings_answered"] > 0 + assert sim.pongs > 0 + + +async def test_link_stats_describe_a_live_link(session): + await until(session, "BatteryStateChanged_percent") + stats = session.link_stats() + assert stats["connected"] and stats["frames_in"] > 0 and stats["frames_out"] > 0 + assert stats["telemetry_keys"] > 10 + assert stats["last_event_age"] < 2.0 + assert stats["undecodable_events"] == 0 + + +# -- sending ------------------------------------------------------------- +async def test_command_reaches_the_drone_with_the_right_ids(sim, session): + result = await session.send(VIDEO_ENABLE, {"enable": 1}, confirm=False) + assert result["acked"] is True + assert result["buffer"] == BufferId.C2D_ACK + sent = [r for r in sim.received if r.ids == (1, 21, 0)] + assert sent, f"sim saw {sim.received_ids()}" + assert sent[0].args == b"\x01" + assert sent[0].data_type == 4 # DATA_WITH_ACK + + +async def test_non_ack_commands_go_out_on_buffer_ten_unacknowledged(sim, session): + assert PCMD.buffer == Buffer.NON_ACK + args = {"flag": 1, "roll": 0, "pitch": 10, "yaw": 0, "gaz": 0, "timestampAndSeqNum": 0} + result = await session.send(PCMD, args, confirm=False) + assert result["buffer"] == BufferId.C2D_NON_ACK + assert result["acked"] is None # nothing to wait for, and we did not + deadline = time.monotonic() + 1.0 + while time.monotonic() < deadline and not any(r.ids == PCMD.ids for r in sim.received): + await asyncio.sleep(0.02) + assert [r.data_type for r in sim.received if r.ids == PCMD.ids] == [2] + + +async def test_emergency_goes_out_acknowledged_on_buffer_twelve(sim, session): + # libARController configures buffer 12 as DATA_WITH_ACK with unlimited + # retries. pyparrot sends LOW_LATENCY there, which is fire and forget. + assert EMERGENCY.buffer == Buffer.HIGH_PRIO + result = await session.send(EMERGENCY, {}, confirm=False) + assert result["buffer"] == BufferId.C2D_HIGH_PRIO + assert result["acked"] is True + assert [r.data_type for r in sim.received if r.ids == (1, 0, 4)] == [4] + + +async def test_sequence_numbers_advance_per_buffer(sim, session): + first = await session.send(VIDEO_ENABLE, {"enable": 1}, confirm=False) + second = await session.send(VIDEO_ENABLE, {"enable": 0}, confirm=False) + assert second["seq"] == first["seq"] + 1 + + +async def test_confirmation_comes_from_the_drone_report(session): + spec = CommandSpec( + project="ardrone3", + klass="MediaStreaming", + name="VideoEnable", + ids=VIDEO_ENABLE.ids, + args=VIDEO_ENABLE.args, + expectations=(Expectation(ids=ATTITUDE),), + ) + result = await session.send(spec, {"enable": 1}) + assert result["confirmed"]["ids"] == list(ATTITUDE) + assert "AttitudeChanged_yaw" in result["confirmed"]["values"] + + +async def test_a_command_with_no_expectation_says_so_rather_than_timing_out(session): + result = await session.send(VIDEO_ENABLE, {"enable": 1}, confirm=True) + assert result["acked"] is True + assert result["confirmed"] is None + assert "no confirming event" in result["note"] + # It must not have sat out the confirm window waiting for an event that + # was never coming. + assert result["elapsed_ms"] < 1000 + + +async def test_an_expectation_that_does_not_match_is_not_reported_as_confirmed(session): + # `this.enable` means the event should echo what we sent. Attitude never + # will, so this must come back unconfirmed rather than take any event + # with the right ids. + spec = CommandSpec( + project="ardrone3", + klass="MediaStreaming", + name="VideoEnable", + ids=VIDEO_ENABLE.ids, + args=VIDEO_ENABLE.args, + expectations=(Expectation(ids=ATTITUDE, fields={"roll": "this.enable"}),), + ) + result = await session.send(spec, {"enable": 1}) + assert result["acked"] is True + assert result["confirmed"] is None + assert "did not report the change" in result["note"] + + +async def test_request_full_state_asks_for_both_and_collects_the_burst(sim, session): + await until(session, "AttitudeChanged_roll") + summary = await session.request_full_state() + assert summary["sent"] == ["common.Common.AllStates", "common.Settings.AllSettings"] + assert all(summary["acked"]) + assert ALL_STATES.ids in sim.received_ids() + assert ALL_SETTINGS.ids in sim.received_ids() + assert summary["telemetry_keys"] > 20 + + +async def test_a_dead_link_gives_up_instead_of_blocking(sim, session): + await until(session, "BatteryStateChanged_percent") + sim.stop() # the drone is gone; UDP will not notice, so the timeout must + started = time.monotonic() + result = await session.send(VIDEO_ENABLE, {"enable": 1}, confirm=False) + elapsed = time.monotonic() - started + assert result["acked"] is False + # Two attempts at half a second, with room for scheduling. + assert elapsed < 3.0, f"gave up only after {elapsed:.1f}s" + + +async def test_sending_without_a_session_is_refused(sim): + drone = DroneSession("127.0.0.1", discovery_port=sim.discovery_port, d2c_port=0, encoder=fake_encode) + with pytest.raises(NotConnected): + await drone.send(VIDEO_ENABLE, {"enable": 1}) + + +async def test_disconnect_stops_the_receive_thread(sim): + drone = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=fake_decode, + ) + await drone.connect() + await until(drone, "AttitudeChanged_roll") + await drone.disconnect() + assert not drone.connected + names = {thread.name for thread in threading.enumerate()} + assert "arsdk-recv" not in names + + +async def test_reconnect_after_the_drone_releases_the_slot(sim): + first = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=fake_decode, + ) + await first.connect() + await first.disconnect() + sim.release() # a real aircraft frees the slot when the link drops + second = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=fake_decode, + ) + await second.connect() + try: + assert await until(second, "BatteryStateChanged_percent") == 87 + finally: + await second.disconnect() + + +# -- the real codec, once it lands --------------------------------------- +async def test_against_the_real_codec(sim): + """Same session, but through `protocol/codec.py` rather than local fakes. + + Skipped until the protocol stream lands. When it starts failing, the two + streams disagree about the wire, which is exactly what this is for. + """ + pytest.importorskip("mcbebop.protocol.codec", reason="protocol stream has not landed yet") + drone = DroneSession( + "127.0.0.1", discovery_port=sim.discovery_port, d2c_port=0, timeouts=quick_timeouts() + ) + await drone.connect() + try: + assert await until(drone, "ProductVersionChanged_software") == "4.7.1" + result = await drone.send(VIDEO_ENABLE, {"enable": 1}, confirm=False) + assert result["acked"] is True + assert [r.args for r in sim.received if r.ids == VIDEO_ENABLE.ids] == [b"\x01"] + finally: + await drone.disconnect() + + +# -- discovery ----------------------------------------------------------- +def test_probe_sees_a_listening_drone(sim): + assert discovery.probe("127.0.0.1", sim.discovery_port, timeout=1.0) + + +def test_probe_gives_up_on_a_closed_port_quickly(): + started = time.monotonic() + assert not discovery.probe("127.0.0.1", 1, timeout=0.5) + assert time.monotonic() - started < 2.0 + + +def test_an_explicit_address_is_trusted_without_probing(): + # A caller who names an address wants the connection error, not a + # discovery verdict, so find() must not quietly return None here. + found = discovery.find("10.1.2.3") + assert found is not None + assert (found.ip, found.via) == ("10.1.2.3", "given") + assert found.address == ("10.1.2.3", 44444) diff --git a/tests/test_sim.py b/tests/test_sim.py new file mode 100644 index 0000000..109ed59 --- /dev/null +++ b/tests/test_sim.py @@ -0,0 +1,286 @@ +"""The simulator, checked with raw sockets rather than through our own client. + +If the sim were only ever exercised by `arsdk/`, the two could agree about a +mistake and nothing would notice. So these tests do the handshake and the +framing by hand, and assert on bytes. +""" + +import json +import socket +import struct +import time + +import pytest + +from mcbebop.arsdk.types import COMMAND_HEADER, BufferId, DataType, Frame +from mcbebop.protocol.types import Buffer +from mcbebop.sim import FakeBebop, _fallback_encode_args, _wire_value, load_specs, serve + +SPECS = load_specs() + + +class Controller: + """A hand-rolled controller: TCP handshake, then UDP both ways.""" + + def __init__(self, sim: FakeBebop) -> None: + self.udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.udp.bind(("127.0.0.1", 0)) + self.udp.settimeout(0.3) + self.d2c_port = self.udp.getsockname()[1] + request = { + "d2c_port": self.d2c_port, + "controller_type": "computer", + "controller_name": "test", + "arstream2_client_stream_port": 55004, + "arstream2_client_control_port": 55005, + } + with socket.create_connection(("127.0.0.1", sim.discovery_port), timeout=2) as tcp: + tcp.sendall(json.dumps(request).encode()) + raw = b"" + while b"\x00" not in raw: + raw += tcp.recv(4096) + self.reply = json.loads(raw.split(b"\x00")[0].decode()) + self.c2d = ("127.0.0.1", self.reply.get("c2d_port", 0)) + + def send(self, data_type: int, buffer_id: int, payload: bytes, seq: int = 0) -> None: + self.udp.sendto(Frame(DataType(data_type), buffer_id, seq, payload).encode(), self.c2d) + + def collect(self, seconds: float = 1.0) -> list[Frame]: + frames: list[Frame] = [] + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + try: + data, _ = self.udp.recvfrom(65535) + except TimeoutError: + continue + frames.extend(Frame.decode_all(data)) + return frames + + def wait_for(self, predicate, seconds: float = 2.0) -> Frame: + deadline = time.monotonic() + seconds + while time.monotonic() < deadline: + for frame in self.collect(0.3): + if predicate(frame): + return frame + pytest.fail("the sim never sent a matching frame") + + def close(self) -> None: + self.udp.close() + + +@pytest.fixture +def sim(): + with FakeBebop() as fake: + yield fake + + +@pytest.fixture +def controller(sim): + client = Controller(sim) + yield client + client.close() + + +def events(frames: list[Frame]) -> list[tuple[tuple[int, int, int], bytes]]: + return [ + (COMMAND_HEADER.unpack_from(f.payload), f.payload[COMMAND_HEADER.size :]) + for f in frames + if f.buffer_id in (BufferId.D2C_ACK, BufferId.D2C_NON_ACK) and len(f.payload) >= COMMAND_HEADER.size + ] + + +# -- the spec table ------------------------------------------------------ +def test_every_command_is_indexed(): + assert len(SPECS) == 264 + + +@pytest.mark.parametrize( + ("name", "ids"), + [ + # The table in bebop-2's protocol.md, which came off the aircraft. + ("common.Common.AllStates", (0, 4, 0)), + ("common.Settings.AllSettings", (0, 2, 0)), + ("ardrone3.Piloting.TakeOff", (1, 0, 1)), + ("ardrone3.Piloting.PCMD", (1, 0, 2)), + ("ardrone3.MediaStreaming.VideoEnable", (1, 21, 0)), + ], +) +def test_ids_match_the_observed_table(name, ids): + assert SPECS[name].ids == ids + + +def test_class_names_are_reused_across_projects(): + # Why the index is keyed by full name: these are different commands. + assert "common.SettingsState.ProductNameChanged" in SPECS + assert "ardrone3.SettingsState.MotorErrorStateChanged" in SPECS + assert SPECS["common.SettingsState.ProductNameChanged"].ids[0] == 0 + assert SPECS["ardrone3.SettingsState.MotorErrorStateChanged"].ids[0] == 1 + + +def test_buffer_attribute_decides_the_frame_type(): + assert SPECS["ardrone3.Piloting.PCMD"].buffer == Buffer.NON_ACK + assert SPECS["ardrone3.Piloting.Emergency"].buffer == Buffer.HIGH_PRIO + assert SPECS["ardrone3.MediaStreaming.VideoEnable"].buffer == Buffer.ACK # attribute absent + + +# -- encoding ------------------------------------------------------------ +def test_strings_are_nul_terminated(): + spec = SPECS["common.SettingsState.ProductVersionChanged"] + assert _fallback_encode_args(spec, {"software": "4.7.1", "hardware": "HW_05"}) == b"4.7.1\x00HW_05\x00" + + +def test_enums_travel_as_four_byte_positions(): + # pyparrot read these as one byte, which is wrong and shifts every + # argument after them. + spec = SPECS["ardrone3.PilotingState.FlyingStateChanged"] + assert _fallback_encode_args(spec, {"state": "hovering"}) == struct.pack("= 1 + # An 8-byte pong body parses as a command header and would otherwise be + # filed as a command the controller never sent. + assert len(sim.received) == before + + +def test_all_states_triggers_a_fresh_burst(sim, controller): + controller.collect(0.6) + version = SPECS["common.SettingsState.ProductVersionChanged"].ids + controller.send(DataType.DATA_WITH_ACK, BufferId.C2D_ACK, COMMAND_HEADER.pack(0, 4, 0), seq=1) + frames = controller.collect(1.0) + assert any(ids == version for ids, _ in events(frames)) + assert (0, 4, 0) in sim.received_ids() + + +def test_a_truncated_datagram_does_not_stop_the_sim(sim, controller): + # A size field larger than the datagram used to be an infinite loop. + controller.udp.sendto(struct.pack(" Date: Fri, 2 Oct 2026 00:29:20 -0600 Subject: [PATCH 2/3] arsdk: read the codec's (ids, values) decode shape protocol/codec.py landed with decode_event returning ids and a values dict, so the session looks the command name up in the index rather than expecting it in the result. Verified end to end against that stream's codec in a stitched tree: the sim encodes events through it, the session decodes through it, and the test that skips until it exists now passes. The argument name cannot be recovered by splitting a telemetry key, because an argument can contain an underscore, so the expectation predicate matches on the suffix instead. --- src/mcbebop/arsdk/session.py | 50 +++++++++++++++++++++++++++--------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/src/mcbebop/arsdk/session.py b/src/mcbebop/arsdk/session.py index 23df17c..d0e8440 100644 --- a/src/mcbebop/arsdk/session.py +++ b/src/mcbebop/arsdk/session.py @@ -24,6 +24,7 @@ import threading import time from collections.abc import Callable, Iterable from dataclasses import dataclass, field +from functools import cache from typing import Any from mcbebop.arsdk.connection import Connection @@ -101,14 +102,31 @@ def _default_decoder(payload: bytes) -> Any: return decode_event(payload) +@cache +def _name_for(ids: tuple[int, int, int]) -> str: + """The command's own name, for an event that arrived without one. + + `protocol.codec.decode_event` returns ids and values; the name lives in + the index. It cannot be recovered from the keys, because an argument may + itself contain an underscore (`MassStorageInfoStateListChanged_mass_storage_id`). + """ + try: + from mcbebop.protocol import xml_index # deferred, and optional + + spec = xml_index.index().by_ids(ids) + except Exception: # pragma: no cover - only without the protocol layer + spec = None + return spec.name if spec is not None else f"cmd_{ids[0]}_{ids[1]}_{ids[2]}" + + def _as_event(decoded: Any, payload: bytes) -> Event | None: """Normalise whatever the codec returned into an `Event`. - The codec is written by another stream against the same `Event` type, so - the first branch is the expected one. The rest exist because a decoder - that returns a plain tuple or mapping is a reasonable thing to write, and - a transport that crashes on the shape of a decode result would be a silly - way to lose a link. + `decode_event` returns `(ids, values)`, which is the second branch. The + others are here because an `Event`, a 3-tuple and a mapping are all + reasonable things for a decoder or a test fake to hand back, and a + transport that died on the shape of a decode result would be a silly way + to lose a link. """ if decoded is None: return None @@ -119,16 +137,18 @@ def _as_event(decoded: Any, payload: bytes) -> Event | None: values = getattr(decoded, "values", None) if ids is None and isinstance(decoded, dict): ids, name, values = decoded.get("ids"), decoded.get("name"), decoded.get("values") - if ids is None and isinstance(decoded, tuple | list) and len(decoded) == 3: - ids, name, values = decoded + if ids is None and isinstance(decoded, tuple | list): + if len(decoded) == 2: + ids, values = decoded + elif len(decoded) == 3: + ids, name, values = decoded if ids is None: log.debug("decoder returned an unusable shape %r for %s", type(decoded), payload[:8].hex()) return None - if name is None: - name = f"cmd_{tuple(ids)}" + ids = tuple(ids) return Event( - ids=tuple(ids), # type: ignore[arg-type] - name=str(name), + ids=ids, # type: ignore[arg-type] + name=str(name) if name else _name_for(ids), # type: ignore[arg-type] values=dict(values or {}), at=getattr(decoded, "at", None) or time.monotonic(), ) @@ -409,7 +429,13 @@ class DroneSession: def predicate(event: Event) -> bool: for name, expected in checks.get(event.ids, {}).items(): - got = event.values.get(f"{event.name}_{name}", event.values.get(name)) + got = event.values.get(f"{event.name}_{name}") + if got is None: + # Telemetry keys are `_`, but an argument + # can contain an underscore, so match on the suffix + # rather than trusting a split. + matches = [v for k, v in event.values.items() if k.endswith(f"_{name}") or k == name] + got = matches[0] if len(matches) == 1 else None if got is not None and got != expected: return False return True From 376ccaa732d0824476e966f3296f7820962c6e3c Mon Sep 17 00:00:00 2001 From: Ryan Malloy Date: Fri, 2 Oct 2026 00:33:15 -0600 Subject: [PATCH 3/3] arsdk: prove one unnameable event does not take the link down decode_event raises on an id triple the XML does not carry, and the XML is missing at least FlatTrim, so this happens on a real aircraft. The receive loop already counted and carried on; now something checks it. --- tests/test_arsdk_session.py | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/tests/test_arsdk_session.py b/tests/test_arsdk_session.py index a7c814f..1e0ef6e 100644 --- a/tests/test_arsdk_session.py +++ b/tests/test_arsdk_session.py @@ -397,3 +397,40 @@ def test_an_explicit_address_is_trusted_without_probing(): assert found is not None assert (found.ip, found.via) == ("10.1.2.3", "given") assert found.address == ("10.1.2.3", 44444) + + +# -- an event we cannot name --------------------------------------------- +async def test_an_undecodable_event_is_counted_not_fatal(sim): + """`decode_event` raises on an id triple the XML does not have. + + The XML is demonstrably incomplete, so this happens on a real aircraft. + One such event must not take the receive thread, and therefore the link, + down with it. + """ + battery = _SPECS["common.CommonState.BatteryStateChanged"].ids + + def decoder(payload): + ids = COMMAND_HEADER.unpack_from(payload) + if ids == battery: + raise ValueError(f"no command with ids {ids}") + return fake_decode(payload) + + drone = DroneSession( + "127.0.0.1", + discovery_port=sim.discovery_port, + d2c_port=0, + timeouts=quick_timeouts(), + encoder=fake_encode, + decoder=decoder, + ) + await drone.connect() + try: + assert await until(drone, "AttitudeChanged_roll") is not None + stats = drone.link_stats() + assert stats["undecodable_events"] > 0 + assert "BatteryStateChanged_percent" not in drone.values() + # The rest of the stream kept arriving, and commands still work. + result = await drone.send(VIDEO_ENABLE, {"enable": 1}, confirm=False) + assert result["acked"] is True + finally: + await drone.disconnect()