Commit 43be2f87 authored by Marlene Böhmer's avatar Marlene Böhmer

Bridge connection to Crazyflie

parent c120bbed
......@@ -3,15 +3,16 @@
import logging
import sys
import re
import datetime
import time
import prrt
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie import Crazyflie, State
from cflib.crtp.crtpstack import CRTPPacket
logger = logging.getLogger(__name__)
MTU = 32
......@@ -20,7 +21,9 @@ PRRT_LOCAL_PORT = 5000
class CrazyflieConnection:
def __init__(self, uri, receive_callback):
self.uri = uri
self.receive_callback = receive_callback
self._platform_information_packet = None
self._cf = Crazyflie()
......@@ -29,66 +32,88 @@ class CrazyflieConnection:
print('Connecting to %s' % uri)
self._cf.open_link(uri)'Connecting to {}'.format(uri))
# Manually open link of _cf without starting connection setup.
# This prevents sending messages from the library at this bridging point.
self._cf.state = State.INITIALIZED
self._cf.link_uri = uri
try: = cflib.crtp.get_link_driver(
uri, self._cf._link_quality_cb, self._cf._link_error_cb)
if not
message = 'No driver found or malformed URI: {}'.format(uri)
logger.warning(message), message)
# Add a callback so we can check that any data is coming back from the copter
except Exception as ex: # pylint: disable=W0703
# We want to catch every possible exception here and show it in the user interface
import traceback
logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc())
exception_text = "Couldn't load link driver: %s\n\n%s" % (ex, traceback.format_exc())
if = None, exception_text)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Crazyflie API when a Crazyflie
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
def _get_initial_packet(self, pk):
self._platform_information_packet = pk
def _fetched_platform_information(self):
self._cf.connected_ts =
def _connected(self, link_uri):'Connected to {}'.format(link_uri))
def send(self, pk):
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp,, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Crazyflie
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
"""Callback when connection initial connection fails (i.e no Crazyflie at the speficied address)"""'Connection to {} failed: {}'.format(link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Crazyflie moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))'Connection to {} lost: {}'.format(link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)'Disconnected from {}'.format(link_uri))
self.is_connected = False
def close(self):
print('Closing Crazyflie Connection')'Closing Crazyflie Connection')
class ClientConnection:
def __init__(self, uri):
self.uri = uri
uri_match ='^prrt://((?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})\.(?:[\d]{1,3})):([\d]{1,5})'
r'(?:/([\d]{1,9}))?$', uri)
if not uri_match:
raise Exception('Invalid PRRT URI')
address =
port = int(
target_delay_us = DEFAULT_TARGET_DELAY
target_delay_us = int(
print('Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))'Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))
self._prrt_socket = prrt.PrrtSocket(("", PRRT_LOCAL_PORT), maximum_payload_size=MTU,
......@@ -96,7 +121,7 @@ class ClientConnection:
self._prrt_socket.connect((address, port))
def send(self, pk):
print('send to client')
print('send to client' + pk)
def receive(self):
print('receive from client')
......@@ -104,8 +129,9 @@ class ClientConnection:
return pk
def close(self):
print('Closing Client Connection')
# self._prrt_socket.close()'Closing Client Connection')
self._prrt_socket = None'Disconnected from {}'.format(self.uri))
class Bridge:
......@@ -137,5 +163,6 @@ if __name__ == '__main__':
except KeyboardInterrupt:
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment