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
logging.basicConfig(level=logging.DEBUG)
logger = logging.getLogger(__name__)
MTU = 32
DEFAULT_TARGET_DELAY = 200
......@@ -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:
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % uri)
self._cf.open_link(uri)
logger.info('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.connection_requested.call(uri)
self._cf.state = State.INITIALIZED
self._cf.link_uri = uri
try:
self._cf.link = cflib.crtp.get_link_driver(
uri, self._cf._link_quality_cb, self._cf._link_error_cb)
if not self._cf.link:
message = 'No driver found or malformed URI: {}'.format(uri)
logger.warning(message)
self._cf.connection_failed.call(uri, message)
else:
# Add a callback so we can check that any data is coming back from the copter
self._cf.packet_received.add_callback(self._cf._check_for_initial_packet_cb)
self._cf.packet_received.add_callback(self._get_initial_packet)
self._cf.platform.fetch_platform_informations(self._fetched_platform_information)
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 self._cf.link:
self._cf.link.close()
self._cf.link = None
self._cf.connection_failed.call(uri, 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
self._cf.packet_received.remove_callback(self._get_initial_packet)
def _fetched_platform_information(self):
self._cf.connected_ts = datetime.datetime.now()
self._cf.connected.call(self.uri)
def _connected(self, link_uri):
logger.info('Connected to {}'.format(link_uri))
self._cf.packet_received.add_callback(self.receive_callback)
def send(self, pk):
self._cf.send_packet(pk)
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, 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)"""
logger.info('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))
logger.info('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)
logger.info('Disconnected from {}'.format(link_uri))
self.is_connected = False
def close(self):
print('Closing Crazyflie Connection')
logger.info('Closing Crazyflie Connection')
self._cf.close_link()
class ClientConnection:
def __init__(self, uri):
self.uri = uri
uri_match = re.search(r'^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 = uri_match.group(1)
port = int(uri_match.group(2))
target_delay_us = DEFAULT_TARGET_DELAY
if uri_match.group(3):
target_delay_us = int(uri_match.group(3))
print('Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))
logger.info('Open PRRT Link to {}:{} with target delay {}'.format(address, port, target_delay_us))
self._prrt_socket = prrt.PrrtSocket(("0.0.0.0", PRRT_LOCAL_PORT), maximum_payload_size=MTU,
target_delay=target_delay_us)
......@@ -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()
logger.info('Closing Client Connection')
self._prrt_socket = None
logger.info('Disconnected from {}'.format(self.uri))
class Bridge:
......@@ -137,5 +163,6 @@ if __name__ == '__main__':
try:
bridge.run()
except KeyboardInterrupt:
print('\n')
bridge.stop()
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