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.
This commit is contained in:
@@ -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)
|
||||
@@ -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
|
||||
@@ -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 `<Command>_<arg>` 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.<arg>` 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
|
||||
@@ -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": "<B", "i8": "<b", "u16": "<H", "i16": "<h", "u32": "<I", "i32": "<i",
|
||||
"u64": "<Q", "i64": "<q", "float": "<f", "double": "<d", "enum": "<i",
|
||||
} # fmt: skip
|
||||
|
||||
_C2D_BUFFERS = frozenset(
|
||||
{BufferId.C2D_NON_ACK, BufferId.C2D_ACK, BufferId.C2D_HIGH_PRIO, BufferId.C2D_VIDEO_ACK}
|
||||
)
|
||||
|
||||
_ALL_STATES = (0, 4, 0)
|
||||
_ALL_SETTINGS = (0, 2, 0)
|
||||
|
||||
# Sensor self-test order as the aircraft reports it, with the one that fails.
|
||||
_SENSORS = ("IMU", "barometer", "ultrasound", "GPS", "magnetometer", "vertical_camera")
|
||||
_FAULTY_SENSOR = "magnetometer"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Received:
|
||||
"""One command the controller sent us, as the sim saw it."""
|
||||
|
||||
ids: tuple[int, int, int]
|
||||
args: bytes
|
||||
buffer_id: int
|
||||
data_type: int
|
||||
seq: int
|
||||
|
||||
|
||||
def _xml_root() -> 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("<Q", tick))
|
||||
wobble = math.radians(0.6) * math.sin(t)
|
||||
self.emit(
|
||||
"ardrone3.PilotingState.AttitudeChanged",
|
||||
roll=wobble,
|
||||
pitch=-wobble / 2,
|
||||
yaw=math.radians(37 + t),
|
||||
)
|
||||
self.emit("ardrone3.PilotingState.SpeedChanged", speedX=0.0, speedY=0.0, speedZ=0.0)
|
||||
self.emit("ardrone3.PilotingState.AltitudeChanged", altitude=0.0)
|
||||
if tick % 5 == 0:
|
||||
self.emit("common.CommonState.WifiSignalChanged", rssi=-48 - int(4 * math.sin(t / 3)))
|
||||
self.emit(
|
||||
"ardrone3.GPSState.NumberOfSatelliteChanged",
|
||||
numberOfSatellite=min(12, 3 + int(t // 2)),
|
||||
)
|
||||
# 500.0 is the no-position sentinel, which is what a Bebop 2
|
||||
# on a bench indoors actually reports.
|
||||
self.emit(
|
||||
"ardrone3.PilotingState.PositionChanged",
|
||||
latitude=500.0,
|
||||
longitude=500.0,
|
||||
altitude=500.0,
|
||||
)
|
||||
if tick % 25 == 0:
|
||||
self._battery = max(0, self._battery - 1)
|
||||
self.emit("common.CommonState.BatteryStateChanged", acked=True, percent=self._battery)
|
||||
|
||||
|
||||
def serve(seconds: float = 0.0, *, host: str = "127.0.0.1", discovery_port: int = 44444) -> 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
|
||||
@@ -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("<Q", 9)),
|
||||
]
|
||||
assert Frame.decode_all(b"".join(f.encode() for f in frames)) == frames
|
||||
|
||||
|
||||
def test_zero_size_does_not_loop_forever():
|
||||
# A size below the header length can never advance the cursor. The parse
|
||||
# has to stop rather than spin, which is why decode_all checks it.
|
||||
bogus = FRAME_HEADER.pack(2, 126, 0, 0) + b"junk"
|
||||
assert Frame.decode_all(bogus) == []
|
||||
|
||||
|
||||
def test_size_shorter_than_header_after_a_good_frame():
|
||||
good = Frame(DataType.DATA, BufferId.D2C_NON_ACK, 7, b"ok")
|
||||
data = good.encode() + FRAME_HEADER.pack(2, 126, 8, 3)
|
||||
assert Frame.decode_all(data) == [good]
|
||||
|
||||
|
||||
def test_truncated_frame_is_dropped_not_guessed():
|
||||
# A frame claiming more than the datagram holds is a short read, and
|
||||
# half a command decodes into a plausible-looking wrong one.
|
||||
whole = Frame(DataType.DATA, BufferId.D2C_NON_ACK, 1, b"0123456789").encode()
|
||||
assert Frame.decode_all(whole[:-4]) == []
|
||||
|
||||
|
||||
def test_trailing_bytes_below_header_length_are_ignored():
|
||||
good = Frame(DataType.DATA, BufferId.D2C_NON_ACK, 1, b"hi")
|
||||
assert Frame.decode_all(good.encode() + b"\x02\x7e") == [good]
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
("buffer_id", "ack_buffer"),
|
||||
[
|
||||
(BufferId.C2D_ACK, 139), # the one pyparrot hardcodes, so a good check
|
||||
(BufferId.C2D_NON_ACK, 138),
|
||||
(BufferId.C2D_HIGH_PRIO, 140),
|
||||
(BufferId.D2C_ACK, 255), # 127 + 128 lands on the last valid buffer id
|
||||
(BufferId.PING, 128),
|
||||
],
|
||||
)
|
||||
def test_ack_buffer_is_offset_by_128(buffer_id, ack_buffer):
|
||||
assert BufferId.ack_for(buffer_id) == ack_buffer
|
||||
|
||||
|
||||
def test_ack_buffer_wraps_rather_than_overflowing():
|
||||
# 128 upwards would exceed the one-byte field; the modulo is the point.
|
||||
assert BufferId.ack_for(200) == 72
|
||||
assert all(0 <= BufferId.ack_for(b) < 256 for b in range(256))
|
||||
|
||||
|
||||
def test_command_id_is_two_bytes():
|
||||
# ardrone3.MediaStreaming.VideoEnable is (1, 21, 0); the ids that matter
|
||||
# for this are the ones above 255, which a u8 command field would mangle.
|
||||
raw = COMMAND_HEADER.pack(1, 21, 0)
|
||||
assert COMMAND_HEADER.unpack(raw) == (1, 21, 0)
|
||||
assert COMMAND_HEADER.unpack(COMMAND_HEADER.pack(0, 5, 300)) == (0, 5, 300)
|
||||
|
||||
|
||||
def test_command_payload_sits_inside_a_frame():
|
||||
payload = COMMAND_HEADER.pack(0, 4, 0) # common.Common.AllStates
|
||||
frame = Frame(DataType.DATA_WITH_ACK, BufferId.C2D_ACK, 0, payload)
|
||||
(decoded,) = Frame.decode_all(frame.encode())
|
||||
assert COMMAND_HEADER.unpack(decoded.payload) == (0, 4, 0)
|
||||
|
||||
|
||||
def test_data_type_numbers_match_the_captures():
|
||||
assert (DataType.ACK, DataType.DATA, DataType.LOW_LATENCY, DataType.DATA_WITH_ACK) == (1, 2, 3, 4)
|
||||
@@ -0,0 +1,399 @@
|
||||
"""Session against the simulator, which is the whole reason the sim exists.
|
||||
|
||||
There is no real drone in CI, and a transport is exactly the kind of code
|
||||
that passes unit tests and then fails on a wire. So these run a `FakeBebop`
|
||||
on localhost and talk to it over real sockets: real handshake, real UDP, real
|
||||
acknowledgements, real receive thread.
|
||||
|
||||
`protocol/codec.py` is being written in a parallel stream, so the encoder and
|
||||
decoder here are small local ones built from the same vendored XML. The last
|
||||
test in this file uses the real codec once it exists, and skips until then.
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import struct
|
||||
import threading
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from mcbebop.arsdk import discovery
|
||||
from mcbebop.arsdk.session import ALL_SETTINGS, ALL_STATES, DroneSession, Timeouts
|
||||
from mcbebop.arsdk.types import COMMAND_HEADER, BufferId, Event, HandshakeError, NotConnected
|
||||
from mcbebop.protocol.types import Buffer, CommandSpec, Expectation
|
||||
from mcbebop.sim import FakeBebop, _fallback_encode_args, load_specs
|
||||
|
||||
_SPECS = load_specs()
|
||||
_BY_IDS = {spec.ids: spec for spec in _SPECS.values()}
|
||||
_FORMATS = {
|
||||
"u8": "<B", "i8": "<b", "u16": "<H", "i16": "<h", "u32": "<I", "i32": "<i",
|
||||
"u64": "<Q", "i64": "<q", "float": "<f", "double": "<d", "enum": "<i",
|
||||
} # fmt: skip
|
||||
|
||||
VIDEO_ENABLE = _SPECS["ardrone3.MediaStreaming.VideoEnable"]
|
||||
PCMD = _SPECS["ardrone3.Piloting.PCMD"]
|
||||
EMERGENCY = _SPECS["ardrone3.Piloting.Emergency"]
|
||||
ATTITUDE = (1, 4, 6)
|
||||
|
||||
|
||||
def fake_encode(spec, args):
|
||||
"""The client-side encoder under test is not ours, so reuse the sim's."""
|
||||
return _fallback_encode_args(spec, args)
|
||||
|
||||
|
||||
def fake_decode(payload: bytes) -> Event:
|
||||
"""Decode a drone event the way the real codec is specified to.
|
||||
|
||||
Keys are `<Command>_<arg>`, 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)
|
||||
@@ -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("<i", 2)
|
||||
assert _fallback_encode_args(spec, {"state": "landed"}) == struct.pack("<i", 0)
|
||||
|
||||
|
||||
def test_mixed_widths_pack_little_endian():
|
||||
spec = SPECS["ardrone3.SettingsState.MotorFlightsStatusChanged"]
|
||||
raw = _fallback_encode_args(
|
||||
spec, {"nbFlights": 42, "lastFlightDuration": 611, "totalFlightDuration": 26_340}
|
||||
)
|
||||
assert raw == struct.pack("<HHI", 42, 611, 26_340)
|
||||
|
||||
|
||||
def test_doubles_keep_the_no_position_sentinel_exactly():
|
||||
spec = SPECS["ardrone3.PilotingState.PositionChanged"]
|
||||
raw = _fallback_encode_args(spec, {"latitude": 500.0, "longitude": 500.0, "altitude": 500.0})
|
||||
assert struct.unpack("<ddd", raw) == (500.0, 500.0, 500.0)
|
||||
|
||||
|
||||
def test_an_unknown_enum_member_is_an_error_not_a_zero():
|
||||
spec = SPECS["ardrone3.PilotingState.FlyingStateChanged"]
|
||||
with pytest.raises(KeyError):
|
||||
_wire_value(spec.args[0], "hovvering")
|
||||
|
||||
|
||||
# -- handshake -----------------------------------------------------------
|
||||
def test_handshake_is_accepted_and_names_a_c2d_port(sim, controller):
|
||||
assert controller.reply["status"] == 0
|
||||
assert controller.reply["c2d_port"] == sim.c2d_port
|
||||
assert controller.reply["arstream2_server_stream_port"] == 5004
|
||||
assert sim.handshakes[0]["controller_name"] == "test"
|
||||
|
||||
|
||||
def test_a_second_controller_is_refused(sim, controller):
|
||||
second = Controller(sim)
|
||||
try:
|
||||
assert second.reply["status"] == 1
|
||||
finally:
|
||||
second.close()
|
||||
|
||||
|
||||
def test_the_slot_is_free_again_after_release(sim, controller):
|
||||
assert sim.occupied
|
||||
sim.release()
|
||||
third = Controller(sim)
|
||||
try:
|
||||
assert third.reply["status"] == 0
|
||||
finally:
|
||||
third.close()
|
||||
|
||||
|
||||
# -- what the drone sends ------------------------------------------------
|
||||
def test_identity_burst_arrives_on_the_acknowledged_buffer(controller):
|
||||
frames = controller.collect(1.0)
|
||||
version = SPECS["common.SettingsState.ProductVersionChanged"].ids
|
||||
matching = [payload for ids, payload in events(frames) if ids == version]
|
||||
assert matching == [b"4.7.1\x00HW_05\x00"]
|
||||
assert all(
|
||||
f.data_type == DataType.DATA_WITH_ACK
|
||||
for f in frames
|
||||
if f.buffer_id == BufferId.D2C_ACK # identity is sent acked, telemetry is not
|
||||
)
|
||||
|
||||
|
||||
def test_one_sensor_self_test_deliberately_fails(controller):
|
||||
sensors = SPECS["common.CommonState.SensorsStatesListChanged"]
|
||||
names = sensors.args[0].members
|
||||
results = {}
|
||||
for ids, payload in events(controller.collect(1.0)):
|
||||
if ids == sensors.ids:
|
||||
index, state = struct.unpack("<iB", payload)
|
||||
results[names[index].name] = state
|
||||
assert len(results) == 6, results
|
||||
# A simulator where everything passes never exercises the code that
|
||||
# notices something has not.
|
||||
assert results["magnetometer"] == 0
|
||||
assert sum(results.values()) == 5
|
||||
|
||||
|
||||
def test_telemetry_streams_at_roughly_five_hertz(controller):
|
||||
attitude = SPECS["ardrone3.PilotingState.AttitudeChanged"].ids
|
||||
count = sum(1 for ids, _ in events(controller.collect(1.2)) if ids == attitude)
|
||||
assert 3 <= count <= 9, f"{count} attitude events in 1.2s"
|
||||
|
||||
|
||||
def test_the_drone_pings_us(controller):
|
||||
ping = controller.wait_for(lambda f: f.buffer_id == BufferId.PING)
|
||||
assert len(ping.payload) == 8 # the drone's own clock, to be echoed back
|
||||
assert ping.data_type == DataType.DATA
|
||||
|
||||
|
||||
# -- what the drone receives ---------------------------------------------
|
||||
def test_an_acknowledged_command_is_acknowledged(sim, controller):
|
||||
payload = COMMAND_HEADER.pack(1, 21, 0) + b"\x01"
|
||||
controller.send(DataType.DATA_WITH_ACK, BufferId.C2D_ACK, payload, seq=7)
|
||||
ack = controller.wait_for(lambda f: f.data_type == DataType.ACK)
|
||||
assert ack.buffer_id == BufferId.ack_for(BufferId.C2D_ACK) == 139
|
||||
assert ack.payload == b"\x07" # the sequence acked, not the ack's own
|
||||
assert sim.received_ids().count((1, 21, 0)) == 1
|
||||
assert [r.args for r in sim.received if r.ids == (1, 21, 0)] == [b"\x01"]
|
||||
|
||||
|
||||
def test_a_non_ack_command_is_recorded_but_not_acknowledged(sim, controller):
|
||||
controller.collect(0.4) # drain the identity burst and its own frames
|
||||
controller.send(DataType.DATA, BufferId.C2D_NON_ACK, COMMAND_HEADER.pack(1, 0, 2) + bytes(9))
|
||||
deadline = time.monotonic() + 1.0
|
||||
while time.monotonic() < deadline and (1, 0, 2) not in sim.received_ids():
|
||||
time.sleep(0.02)
|
||||
assert (1, 0, 2) in sim.received_ids()
|
||||
assert not [f for f in controller.collect(0.4) if f.data_type == DataType.ACK]
|
||||
|
||||
|
||||
def test_a_pong_is_counted_not_mistaken_for_a_command(sim, controller):
|
||||
ping = controller.wait_for(lambda f: f.buffer_id == BufferId.PING)
|
||||
before = len(sim.received)
|
||||
controller.send(DataType.DATA, BufferId.PONG, ping.payload)
|
||||
deadline = time.monotonic() + 1.0
|
||||
while time.monotonic() < deadline and sim.pongs == 0:
|
||||
time.sleep(0.02)
|
||||
assert sim.pongs >= 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("<BBBI", 4, 11, 1, 400) + b"short", controller.c2d)
|
||||
controller.send(DataType.DATA_WITH_ACK, BufferId.C2D_ACK, COMMAND_HEADER.pack(1, 21, 0) + b"\x00", 2)
|
||||
assert controller.wait_for(lambda f: f.data_type == DataType.ACK) is not None
|
||||
|
||||
|
||||
# -- house rules ---------------------------------------------------------
|
||||
def test_ports_are_ephemeral_by_default(sim):
|
||||
assert sim.discovery_port != 0 and sim.c2d_port != 0
|
||||
assert sim.discovery_port != sim.c2d_port
|
||||
|
||||
|
||||
def test_serve_writes_nothing_to_stdout(capsys):
|
||||
# stdout is the MCP server's JSON-RPC transport and this module is
|
||||
# importable from it, so a stray print would corrupt the protocol.
|
||||
serve(0.3, discovery_port=0)
|
||||
assert capsys.readouterr().out == ""
|
||||
Reference in New Issue
Block a user