diff --git a/cflib/crazyflie/__init__.py b/cflib/crazyflie/__init__.py index ebf508aea..4c20a998d 100644 --- a/cflib/crazyflie/__init__.py +++ b/cflib/crazyflie/__init__.py @@ -359,6 +359,14 @@ def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): if not pk.is_data_size_valid(): raise Exception('Data part of packet is too large') + if self.link is not None and self.link.get_name() == 'ble': + # ATT writes plus null polling can exceed the radio's 200 ms. + timeout = max(timeout, 0.8) + from cflib.crtp.blepolicy import bench_allowed + if not bench_allowed(bytes([pk.get_header()]) + bytes(pk.data)): + # Block before scheduling retries or acquiring the send lock. + self.link.send_packet(pk) + return self._send_lock.acquire() if self.link is not None: if len(expected_reply) > 0 and not resend and \ diff --git a/cflib/crazyflie/log.py b/cflib/crazyflie/log.py index 021e230e0..20c49dfd5 100644 --- a/cflib/crazyflie/log.py +++ b/cflib/crazyflie/log.py @@ -524,7 +524,14 @@ def refresh_toc(self, refresh_done_callback, toc_cache): self._refresh_callback = refresh_done_callback self.toc = None - self._send_reset_packet() + if self.cf.link and self.cf.link.get_name() == 'ble': + # Bench BLE never deletes another client's log blocks globally. + self.toc = Toc() + toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING, + self.toc, self._refresh_callback, self._toc_cache) + toc_fetcher.start() + else: + self._send_reset_packet() def _send_reset_packet(self): pk = CRTPPacket() diff --git a/cflib/crtp/__init__.py b/cflib/crtp/__init__.py index 12143a776..8041e40d9 100644 --- a/cflib/crtp/__init__.py +++ b/cflib/crtp/__init__.py @@ -43,7 +43,7 @@ CLASSES = [] -def init_drivers(enable_debug_driver=False, enable_serial_driver=False): +def init_drivers(enable_debug_driver=False, enable_serial_driver=False, enable_ble_driver=False): """Initialize all the drivers.""" env = os.getenv('USE_CFLINK') @@ -60,6 +60,10 @@ def init_drivers(enable_debug_driver=False, enable_serial_driver=False): CLASSES.append(SerialDriver) CLASSES.extend([UdpDriver, PrrtDriver, TcpDriver]) + if enable_ble_driver: + from .bledriver import BleDriver + if BleDriver not in CLASSES: + CLASSES.append(BleDriver) def scan_interfaces(address=None): diff --git a/cflib/crtp/bledriver.py b/cflib/crtp/bledriver.py new file mode 100644 index 000000000..f046e0f27 --- /dev/null +++ b/cflib/crtp/bledriver.py @@ -0,0 +1,336 @@ +"""Experimental macOS BLE CRTP driver, bench-only, corrected firmware required. + +Copyright (C) 2026. Licensed under GPL-3.0-or-later, as part of cflib. +""" +import asyncio +import logging +import queue +import threading +from urllib.parse import parse_qs +from urllib.parse import urlencode +from urllib.parse import urlsplit +from uuid import UUID + +from .blepolicy import bench_allowed +from .bletransport import DownDecoder +from .bletransport import fragment_up +from .bletransport import FrameError +from .crtpdriver import CRTPDriver +from .crtpstack import CRTPPacket +from .exceptions import WrongUriType + +logger = logging.getLogger(__name__) +SERVICE = '00000201-1c7f-4f9e-947b-43b7c00a9a08' +CRTP = '00000202-1c7f-4f9e-947b-43b7c00a9a08' +UP = '00000203-1c7f-4f9e-947b-43b7c00a9a08' +DOWN = '00000204-1c7f-4f9e-947b-43b7c00a9a08' +_END = object() +_HEALTH = b'\xfcCFBLE' + + +class FirmwareCompatibilityError(RuntimeError): + pass + + +def parse_uri(uri): + if not uri.startswith('ble://'): + raise WrongUriType('Not a BLE URI') + value = urlsplit(uri) + if value.path or value.fragment or value.username or value.port: + raise ValueError('Expected ble://?name=') + identifier = str(UUID(value.hostname)) + query = parse_qs(value.query, strict_parsing=True) + if set(query) != {'name'} or len(query['name']) != 1 or not query['name'][0].startswith('Crazyflie'): + raise ValueError('An exact Crazyflie name is required; unsupported URI options') + return identifier, query['name'][0] + + +def validate_services(services): + service = next((s for s in services if s.uuid.lower() == SERVICE), None) + chars = {c.uuid.lower(): set(c.properties) for c in service.characteristics} if service else {} + for uuid, required in ((CRTP, {'write'}), (UP, {'write'}), (DOWN, {'notify'})): + if not required <= chars.get(uuid, set()): + raise FirmwareCompatibilityError('Crazyflie GATT profile not found or incompatible') + + +class BleDriver(CRTPDriver): + """Dedicated BLE event-loop thread; no flight-enable switch is provided.""" + + def __init__(self): + super().__init__() + self.needs_resending = True + self._thread = None + self._stop = threading.Event() + self._ready = threading.Event() + self._rx = queue.Queue(maxsize=512) + self._tx = queue.Queue(maxsize=128) + self._error = None + self._callback = None + self._closed = True + self._owned_blocks = set() + self._pending_blocks = set() + self.stats = {'sent': 0, 'received': 0, 'nulls': 0, 'blocked': 0, 'cleanup': []} + + def connect(self, uri, radio_link_statistics_callback=None, link_error_callback=None): + identifier, name = parse_uri(uri) + if self._thread: + raise RuntimeError('BLE driver already used; create a new instance') + self._callback = link_error_callback + self._closed = False + self._thread = threading.Thread(target=self._worker, args=(identifier, name), name='CrazyflieBLE', daemon=True) + self._thread.start() + if not self._ready.wait(60): + self.close() + raise TimeoutError('BLE discovery/connection/probe timed out') + if self._error: + self.close() + raise self._error + + def _worker(self, identifier, name): + connected = False + try: + asyncio.run(self._session(identifier, name)) + connected = self._ready.is_set() + except Exception as exc: + connected = self._ready.is_set() + self._error = exc + logger.error('BLE: %s', exc) + finally: + self._closed = True + self._ready.set() + try: + self._rx.put_nowait(_END) + except queue.Full: + pass + # Callback after event-loop cleanup, not from a Bleak delegate. + if connected and self._error and not self._stop.is_set() and self._callback: + self._callback(str(self._error)) + + async def _session(self, identifier, name): + from bleak import BleakClient, BleakScanner + found = await BleakScanner.discover(timeout=5, return_adv=True, scanning_mode='active') + selected = [d for d, a in found.values() if d.address.lower() == identifier.lower() + and (a.local_name or d.name) == name] + if len(selected) != 1: + raise RuntimeError('Selected UUID and exact name were not discovered') + fault = None + + def disconnected(_client): + nonlocal fault + if not self._stop.is_set(): + fault = RuntimeError('BLE disconnected unexpectedly') + async with BleakClient(selected[0], timeout=12, disconnected_callback=disconnected) as client: + validate_services(client.services) + raw_frames = asyncio.Queue(maxsize=128) + + def notify(_characteristic, data): + nonlocal fault + try: + raw_frames.put_nowait(bytes(data)) + except asyncio.QueueFull: + fault = RuntimeError('BLE notification queue overflow') + await asyncio.wait_for(client.start_notify(DOWN, notify), 5) + try: + decoder = DownDecoder() + # Drain startup console and replies left by a previous session + # before issuing fresh integrity probes. Still validate framing. + warm_deadline = asyncio.get_running_loop().time() + 20 + last_frame = asyncio.get_running_loop().time() + while asyncio.get_running_loop().time() < warm_deadline: + await self._write(client, b'\xff', 0) + await asyncio.sleep(0.04) + while not raw_frames.empty(): + try: + decoder.feed(raw_frames.get_nowait()) + except FrameError as exc: + raise FirmwareCompatibilityError('Unsupported BLE framing during startup drain') from exc + last_frame = asyncio.get_running_loop().time() + if decoder.pid is None and asyncio.get_running_loop().time() - last_frame >= 0.5: + break + else: + raise TimeoutError('BLE startup stream did not become quiet') + # Test exact bytes across the fragmentation boundary before + # cflib downloads any TOC or initializes subsystems. + for size in (8, 20, 31): + expected = bytes([0xFC]) + bytes(range(0x40, 0x40 + size - 1)) + await self._write(client, expected, size & 3) + matched = False + for _ in range(250 if size == 8 else 25): + await asyncio.sleep(0.04) + await self._write(client, b'\xff', 0) + while not raw_frames.empty(): + frame = raw_frames.get_nowait() + if (size == 8 and len(frame) == 9 and frame[0] == 0x88 and frame[1:] == expected): + raise FirmwareCompatibilityError( + 'Legacy BLE framing detected; corrected nRF firmware is required. ' + 'See the experimental BLE driver documentation.') + packet = decoder.feed(frame) + if packet is not None: + if packet[0] & 0xF3 != 0xF0: + continue + if packet != expected: + raise FirmwareCompatibilityError('BLE echo failed byte-for-byte integrity check') + matched = True + if matched: + break + if not matched: + raise FirmwareCompatibilityError('BLE integrity probe did not receive a complete echo') + decoder.reset() + pid = 0 + self._ready.set() + last_rx = asyncio.get_running_loop().time() + last_health = last_rx + while not self._stop.is_set(): + if fault: + raise fault + while not raw_frames.empty(): + packet = decoder.feed(raw_frames.get_nowait()) + if packet is not None: + last_rx = asyncio.get_running_loop().time() + if packet == _HEALTH: + continue + self._track_reply(packet) + self._rx.put_nowait(CRTPPacket(header=packet[0], data=packet[1:])) + self.stats['received'] += 1 + try: + packet = self._tx.get_nowait() + except queue.Empty: + if asyncio.get_running_loop().time() - last_health >= 2: + packet = _HEALTH + last_health = asyncio.get_running_loop().time() + else: + packet = b'\xff' + await self._write(client, packet, pid) + pid = (pid + 1) & 3 + if packet == b'\xff': + self.stats['nulls'] += 1 + else: + self.stats['sent'] += 1 + if asyncio.get_running_loop().time() - last_rx > 10: + raise TimeoutError('BLE did not receive packets or health echoes for 10 seconds') + await asyncio.sleep(0.02) + finally: + if client.is_connected: + await self._cleanup(client, raw_frames, decoder) + await asyncio.wait_for(client.stop_notify(DOWN), 3) + + def _track_reply(self, packet): + if len(packet) == 4 and packet[0] & 0xF3 == 0x51: + command, block, status = packet[1:] + if command == 6 and block in self._pending_blocks: + self._pending_blocks.discard(block) + if status == 0: + self._owned_blocks.add(block) + elif command == 2 and status == 0: + self._owned_blocks.discard(block) + + async def _cleanup(self, client, frames, decoder): + for block in list(self._owned_blocks): + for command in (4, 2): + confirmed = False + await self._write(client, bytes([0x5D, command, block]), 0) + for _ in range(10): + await asyncio.sleep(0.04) + await self._write(client, b'\xff', 0) + while not frames.empty(): + try: + packet = decoder.feed(frames.get_nowait()) + except FrameError: + decoder.reset() + continue + if packet: + self._track_reply(packet) + if packet[0] & 0xF3 == 0x51 and packet[1:] == bytes([command, block, 0]): + confirmed = True + if confirmed: + break + self.stats['cleanup'].append({'block': block, 'command': command, 'confirmed': confirmed}) + + async def _write(self, client, packet, pid): + if len(packet) <= 20: + await asyncio.wait_for(client.write_gatt_char(CRTP, packet, response=True), 3) + else: + for frame in fragment_up(packet, pid): + await asyncio.wait_for(client.write_gatt_char(UP, frame, response=True), 3) + + def send_packet(self, pk): + packet = bytes([pk.get_header()]) + bytes(pk.data) + allowed = bench_allowed(packet) + if packet[0] & 0xF3 == 0x51 and len(packet) >= 3: + command, block = packet[1:3] + if command in (2, 3, 4, 7, 8) and block not in self._owned_blocks: + allowed = False + if (command == 6 and len(self._owned_blocks | self._pending_blocks) >= 4 + and block not in self._pending_blocks): + allowed = False + if not allowed: + self.stats['blocked'] += 1 + if self.stats['blocked'] == 1: + logger.warning('BLE bench mode blocked a flight/control/persistent-write packet') + return # Do not deadlock cflib's send lock on its close-time setpoint. + if self._closed: + return + try: + if packet[0] & 0xF3 == 0x51 and packet[1] == 6: + self._pending_blocks.add(packet[2]) + self._tx.put_nowait(packet) + except queue.Full: + self._error = RuntimeError('BLE transmit queue overflow; closing link') + self._stop.set() + if self._callback: + threading.Thread(target=self._callback, args=(str(self._error),), daemon=True).start() + + def receive_packet(self, wait=0): + if self._closed and self._rx.empty(): + return None + try: + value = self._rx.get(block=wait != 0, timeout=None if wait < 0 or wait == 0 else wait) + return None if value is _END else value + except queue.Empty: + return None + + def close(self): + self._stop.set() + if self._thread and self._thread is not threading.current_thread(): + self._thread.join(timeout=15) + self._closed = True + + def get_name(self): + return 'ble' + + def get_status(self): + return 'Experimental BLE bench mode; flight and persistent writes blocked' + + def get_help(self): + return 'ble://?name=; corrected nRF required' + + def scan_interface(self, address=None): + async def scan(): + from bleak import BleakScanner + devices = await BleakScanner.discover(timeout=5, return_adv=True, scanning_mode='active') + results = [] + for device, advertisement in devices.values(): + name = advertisement.local_name or device.name or '' + if name.startswith('Crazyflie'): + try: + identifier = str(UUID(device.address)) + except ValueError: + continue + results.append(['ble://' + identifier + '?' + urlencode({'name': name}), '']) + return results + # CLI callers may already have an event loop; isolate scan as well. + result, error = [], [] + + def worker(): + try: + result.extend(asyncio.run(scan())) + except Exception as exc: + error.append(exc) + thread = threading.Thread(target=worker, name='CrazyflieBLEScan', daemon=True) + thread.start() + thread.join(timeout=10) + if thread.is_alive(): + raise TimeoutError('BLE scan did not terminate') + if error: + raise RuntimeError('BLE unavailable: check macOS Bluetooth permission and power: ' + str(error[0])) + return result diff --git a/cflib/crtp/blepolicy.py b/cflib/crtp/blepolicy.py new file mode 100644 index 000000000..9b3f604a0 --- /dev/null +++ b/cflib/crtp/blepolicy.py @@ -0,0 +1,41 @@ +"""Mandatory BLE bench packet policy, separate from wire framing. + +Copyright (C) 2026. Licensed under GPL-3.0-or-later, as part of cflib. +""" + + +def bench_allowed(packet): + """Allow read/query/log operations; flight and persistent writes stay blocked.""" + if not 1 <= len(packet) <= 31: + return False + h, data = packet[0] & 0xF3, packet[1:] + if packet == b'\xff': + return True + if h == 0xF0: # Echo, no bootloader/null payloads + return True + if h == 0xF1: # Firmware identification, not link-layer control + return data == b'\x00' + if h == 0xD1: + return data in (b'\x00', b'\x01', b'\x02') + if h in (0x20, 0x50): # Param/log TOC v1/v2 + return (data in (b'\x01', b'\x03') or + len(data) == 2 and data[0] == 0 or + len(data) == 3 and data[0] == 2) + if h == 0x21: + return len(data) in (1, 2) # Param value read + if h == 0x23: + return len(data) == 3 and data[0] in (2, 4, 6, 7, 8) + if h == 0x40: # Memory enumeration only + return data in (b'\x00', b'\x01') or len(data) == 2 and data[0] == 2 + if h == 0x51 and data: + if data[0] in (2, 4): # Delete/stop an explicitly selected log block + return len(data) == 2 + if data[0] == 3: + return len(data) == 3 and data[2] >= 10 # >=100 ms + if data[0] == 8: + return len(data) == 4 and int.from_bytes(data[2:], 'little') >= 100 + if data[0] in (6, 7): # TOC-backed logging only, never raw-memory ops + return (len(data) >= 5 and (len(data) - 2) % 3 == 0 and + all(1 <= data[i] & 15 <= 8 and 1 <= data[i] >> 4 <= 8 and + data[i+1:i+3] != b'\xff\xff' for i in range(2, len(data), 3))) + return False diff --git a/cflib/crtp/bletransport.py b/cflib/crtp/bletransport.py new file mode 100644 index 000000000..1bb7be700 --- /dev/null +++ b/cflib/crtp/bletransport.py @@ -0,0 +1,54 @@ +"""Crazyflie BLE CRTP framing, independent of an IO backend. + +Copyright (C) 2026. Licensed under GPL-3.0-or-later, as part of cflib. +""" + + +class FrameError(ValueError): + """A BLE fragment does not follow the documented CRTP wire format.""" + + +class DownDecoder: + """Reassemble one packet at a time; malformed input discards partial state.""" + + def __init__(self): + self.reset() + + def reset(self): + """Discard any incomplete packet at a connection or error boundary.""" + self.pid = None + self.expected = 0 + self.data = bytearray() + + def feed(self, value): + """Return a complete packet, None for a partial one, or raise FrameError.""" + try: + if not 2 <= len(value) <= 20: + raise FrameError('Invalid BLE notification size') + control = value[0] + pid = (control >> 5) & 3 + if control & 0x80: + if self.pid is not None: + raise FrameError('New start before previous packet completed') + self.pid, self.expected = pid, (control & 31) + 1 + elif self.pid is None or pid != self.pid or control & 31: + raise FrameError('Missing start, wrong PID, or invalid continuation') + self.data.extend(value[1:]) + if len(self.data) > self.expected: + raise FrameError('Notification exceeds declared packet length') + if len(self.data) == self.expected: + packet = bytes(self.data) + self.reset() + return packet + return None + except FrameError: + self.reset() + raise + + +def fragment_up(packet, pid): + """Encode a 1-31 byte CRTP packet into 20-byte ATT fragments using a 2-bit PID.""" + if not 1 <= len(packet) <= 31: + raise FrameError('CRTP packet must contain 1..31 bytes') + first = bytes([0x80 | ((pid & 3) << 5) | (len(packet) - 1)]) + packet[:19] + return [first] + ([bytes([(pid & 3) << 5]) + packet[19:]] if len(packet) > 19 else []) diff --git a/docs/functional-areas/ble.md b/docs/functional-areas/ble.md new file mode 100644 index 000000000..8cd902338 --- /dev/null +++ b/docs/functional-areas/ble.md @@ -0,0 +1,90 @@ +--- +title: Experimental Bluetooth Low Energy transport +page_id: ble +--- + +The optional BLE driver provides bench inspection and telemetry without a +Crazyradio. It has been hardware-tested on macOS 15.7.4 / Apple Silicon and a +Crazyflie 2.1. Other operating systems, multiple simultaneous devices, flight, +and radio-equivalent timing/range have not been validated. + +## Install and discover + +Install this branch with its optional backend: + +```sh +python -m pip install '.[ble]' +``` + +Enable the driver explicitly. To discover only BLE, use its scanner directly: + +```python +import cflib.crtp +from cflib.crtp.bledriver import BleDriver + +cflib.crtp.init_drivers(enable_ble_driver=True) +for uri, _description in BleDriver().scan_interface(): + print(uri) +``` + +The existing Radio, USB and network drivers remain available. A scan through +`cflib.crtp.scan_interfaces()` scans all registered drivers, including radio; +radio traffic can disable BLE in the Crazyflie until it is restarted. Close +other clients before connecting. macOS Bluetooth permission is required; +CoreBluetooth scanning here is active, not strictly passive. + +URIs have the form `ble://?name=`. +The UUID is local to macOS, not a portable MAC address. Discovery uses the +advertised name because the tested firmware does not advertise the proprietary +CRTP service UUID. Connection validates the selected UUID/name and GATT profile. +This avoids selecting the first device by name; it is not authentication. + +## Read-only bench session + +Use the discovered URI with ordinary `Crazyflie`, `SyncCrazyflie`, `LogConfig` +and parameter-read APIs. The connection initializes the log/parameter TOCs and +reads parameter values; uncached startup can take several minutes. On the tested +unit there were 525 logging items and 310 parameters. Three real sessions +completed, including 40 voltage samples and confirmed cleanup of owned blocks. +These observations are not performance guarantees for other hardware. + +The transport always blocks flight/setpoint ports, supervisor commands, +parameter writes, raw-memory accesses and bootloader payloads. It permits +identification, TOC/parameter reads, memory metadata and bounded TOC-based log +configuration. It avoids a global log reset and cleans up only blocks whose +creation succeeded in this session. Up to four owned blocks are allowed, with a +minimum logging period of 100 ms. There is no URI option to enable flight. + +The normal cflib close-time setpoint is also blocked before transmission. Packet +statistics count blocked requests separately from transmitted data. + +## Firmware compatibility and failure handling + +The [companion nRF CRTPUP fix](https://github.com/bitcraze/crazyflie2-nrf-firmware/pull/127) is required. Legacy nRF 2024.10 was observed losing +byte 19 of long DOWN packets. The driver never guesses a missing byte. It drains +startup messages, then checks exact echo data at 8, 20 and 31 bytes before cflib +initialization. Incompatible framing/data causes a failed connection. + +BLE IO runs in a dedicated event-loop thread, with bounded queues and operation +timeouts. UP/DOWN use the documented length-minus-one/PID framing. BLE request +retries have an 800 ms minimum to avoid premature retries with ATT writes and +null polling; other drivers retain their existing timeout. A health echo and +10-second receive watchdog detect a stalled connection. Disconnects, queue +errors and framing errors stop the session and notify cflib. + +This driver does not install firmware. Updating the application and bootloader +are separate operations. Tests use a fake backend and do not require Bleak or a +Bluetooth adapter: + +```sh +python -m unittest discover -s test -p test_ble_driver.py +``` + + +## Review dependencies + +The [standalone framing prerequisite](https://github.com/bitcraze/crazyflie-lib-python/pull/613) +contains only the wire codec and its tests. This driver branch carries the same +codec files until that prerequisite is incorporated upstream. The mandatory +bench packet policy is separate in `cflib.crtp.blepolicy`; the wire codec has no +policy or Bleak dependency. diff --git a/docs/index.md b/docs/index.md index fbb76ef50..e0e6f1baa 100644 --- a/docs/index.md +++ b/docs/index.md @@ -10,3 +10,7 @@ communicate with and control a Crazyflie quadcopter. For instance the [Crazyflie ## Contribute Everyone is encouraged to contribute to the CrazyFlie library by forking the Github repository and making a pull request or opening an issue. + +## Experimental transports + +- [Bluetooth Low Energy bench transport](functional-areas/ble.md) diff --git a/pyproject.toml b/pyproject.toml index 29d370615..6b9f225a6 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -51,6 +51,7 @@ Issues = "https://github.com/bitcraze/crazyflie-lib-python/issues" [project.optional-dependencies] dev = ["pre-commit"] +ble = ["bleak~=3.0.2"] qualisys = ["qtm-rt~=3.0.2"] motioncapture = ["motioncapture~=1.0a4"] diff --git a/test/crtp/test_bletransport.py b/test/crtp/test_bletransport.py new file mode 100644 index 000000000..8c9eadb65 --- /dev/null +++ b/test/crtp/test_bletransport.py @@ -0,0 +1,58 @@ +"""BLE codec tests: no Bleak import, platform backend or hardware.""" +import unittest + +from cflib.crtp.bletransport import DownDecoder +from cflib.crtp.bletransport import fragment_up +from cflib.crtp.bletransport import FrameError + + +class CodecTests(unittest.TestCase): + def test_all_lengths_and_pids_roundtrip(self): + for size in range(1, 32): + for pid in range(4): + packet = bytes(range(size)) + decoder = DownDecoder() + result = None + for frame in fragment_up(packet, pid): + result = decoder.feed(frame) + self.assertEqual(result, packet) + + def test_corruption_rejected(self): + decoder = DownDecoder() + decoder.feed(fragment_up(bytes(31), 1)[0]) + with self.assertRaises(FrameError): + decoder.feed(b'\x00\x01') + with self.assertRaises(FrameError): + decoder.feed(b'\x80\0\0') + + def test_rejects_invalid_packet_and_fragment_sizes(self): + for packet in (b'', bytes(32)): + with self.assertRaises(FrameError): + fragment_up(packet, 0) + for fragment in (b'', b'\x80', bytes(21)): + with self.assertRaises(FrameError): + DownDecoder().feed(fragment) + + def test_rejects_orphan_continuation_and_recovers(self): + decoder = DownDecoder() + with self.assertRaises(FrameError): + decoder.feed(b'\x00\x01') + packet = b'\xfcdata' + self.assertEqual(decoder.feed(fragment_up(packet, 2)[0]), packet) + + def test_new_start_during_partial_packet_rejects_and_resets(self): + decoder = DownDecoder() + first = fragment_up(bytes(range(31)), 1)[0] + self.assertIsNone(decoder.feed(first)) + with self.assertRaises(FrameError): + decoder.feed(first) + packet = b'\xfcrecovered' + self.assertEqual(decoder.feed(fragment_up(packet, 3)[0]), packet) + + def test_reset_discards_incomplete_packet(self): + decoder = DownDecoder() + fragments = fragment_up(bytes(range(31)), 0) + self.assertIsNone(decoder.feed(fragments[0])) + decoder.reset() + with self.assertRaises(FrameError): + decoder.feed(fragments[1]) diff --git a/test/test_ble_driver.py b/test/test_ble_driver.py new file mode 100644 index 000000000..63402e54f --- /dev/null +++ b/test/test_ble_driver.py @@ -0,0 +1,294 @@ +import asyncio +import struct +import sys +import threading +import unittest +from types import ModuleType +from types import SimpleNamespace as NS +from unittest.mock import patch + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crtp.bledriver import BleDriver +from cflib.crtp.bledriver import FirmwareCompatibilityError +from cflib.crtp.bledriver import parse_uri +from cflib.crtp.blepolicy import bench_allowed +from cflib.crtp.bletransport import DownDecoder +from cflib.crtp.bletransport import fragment_up +from cflib.crtp.crtpstack import CRTPPacket +from cflib.crtp.exceptions import WrongUriType + +IDENTIFIER = '00000000-0000-4000-8000-000000000001' +URI = 'ble://' + IDENTIFIER + '?name=Crazyflie-ABCDEF' + + +class FakePeripheral: + """A corrected firmware simulator; never touches CoreBluetooth.""" + legacy = False + create_status = 0 + instances = [] + + def __init__(self, device, **kwargs): + from cflib.crtp.bledriver import SERVICE, CRTP, UP, DOWN + self.is_connected = True + self.services = [NS(uuid=SERVICE, characteristics=[ + NS(uuid=CRTP, properties=['write']), + NS(uuid=UP, properties=['write']), NS(uuid=DOWN, properties=['notify'])])] + self.callback = None + self.decoder = DownDecoder() + self.sent = [] + self.blocks = set() + self.block_ops = {} + self.pid = 0 + self.cancel = kwargs.get('disconnected_callback') + self.instances.append(self) + + async def __aenter__(self): + return self + + async def __aexit__(self, *args): + self.is_connected = False + + async def start_notify(self, uuid, callback): + self.callback = callback + + async def stop_notify(self, uuid): + self.callback = None + + def respond(self, raw): + if self.legacy: + self.callback(None, bytes([0x80 | len(raw)]) + raw) + else: + for frame in fragment_up(raw, self.pid): + self.callback(None, frame) + self.pid = (self.pid + 1) & 3 + + async def write_gatt_char(self, uuid, data, response): + from cflib.crtp.bledriver import UP + raw = self.decoder.feed(data) if uuid == UP else bytes(data) + if raw is None: + return + self.sent.append(raw) + h, payload = raw[0] & 0xF3, raw[1:] + if h == 0xF0: + self.respond(raw) + elif raw == b'\xff': + return + elif h == 0xF1: + self.respond(bytes([raw[0]]) + b'Bitcraze Crazyflie' + bytes(12)) + elif h == 0xD1: + self.respond(bytes([raw[0], payload[0]]) + (b'\x08\0\0\0' if payload[0] == 0 else b'2024.10')) + elif h in (0x50, 0x20) and payload == b'\x03': + count = 3 if h == 0x50 else 1 + self.respond(bytes([raw[0], 3]) + struct.pack('= 3: + got_samples.set() + log.data_received_cb.add_callback(sample) + cf.log.add_config(log) + log.start() + self.assertTrue(got_samples.wait(3)) + self.assertAlmostEqual(samples[0], 3.8, places=5) + driver = cf.link + cf.close_link() + self.assertTrue(driver.stats['cleanup']) + self.assertTrue(all(entry['confirmed'] for entry in driver.stats['cleanup'])) + self.assertFalse(FakePeripheral.instances[-1].blocks) + self.assertFalse(driver._thread.is_alive()) + self.assertTrue(all(bench_allowed(p) for p in FakePeripheral.instances[-1].sent)) + finally: + if cf.link: + cf.close_link() + cflib.crtp.CLASSES[:] = saved + + def test_unexpected_disconnect_reports_and_stops(self): + driver = BleDriver() + error = threading.Event() + messages = [] + + def callback(message): + messages.append(message) + driver.close() + error.set() + driver.connect(URI, link_error_callback=callback) + FakePeripheral.instances[-1].cancel(None) + self.assertTrue(error.wait(3)) + driver.close() + self.assertIn('disconnected', messages[0]) + self.assertFalse(driver._thread.is_alive()) + + def test_refused_create_never_deletes_existing_block(self): + FakePeripheral.create_status = 17 + driver = BleDriver() + driver.connect(URI) + driver.send_packet(CRTPPacket(0x51, b'\x06\x01\x77\0\0')) + response = driver.receive_packet(wait=2) + self.assertEqual(response.data[-1], 17) + driver.close() + sent = FakePeripheral.instances[-1].sent + self.assertFalse(any(p[:2] in (b'\x5d\x04', b'\x5d\x02') for p in sent)) + + def test_opt_in_registration_keeps_radio_available(self): + saved = list(cflib.crtp.CLASSES) + try: + cflib.crtp.CLASSES.clear() + cflib.crtp.init_drivers() + self.assertNotIn(BleDriver, cflib.crtp.CLASSES) + self.assertTrue(any(c.__name__ == 'RadioDriver' for c in cflib.crtp.CLASSES)) + cflib.crtp.init_drivers(enable_ble_driver=True) + self.assertEqual(cflib.crtp.CLASSES.count(BleDriver), 1) + finally: + cflib.crtp.CLASSES[:] = saved + + def test_receive_forever_unblocks_on_close(self): + driver = BleDriver() + driver.connect(URI) + waiter = threading.Thread(target=lambda: driver.receive_packet(-1)) + waiter.start() + driver.close() + waiter.join(timeout=2) + self.assertFalse(waiter.is_alive())