Bridge connection to Crazyflie

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

class CrazyflieConnection:

    def __init__(self, uri, receive_callback):

        self.uri = uri

        self.receive_callback = receive_callback

        self._platform_information_packet = None

        self._cf = Crazyflie()

        self._cf.connection_failed.add_callback(self._connection_failed)

        self._cf.connection_lost.add_callback(self._connection_lost)

        print('Connecting to %s' % uri)

        logger.info('Connecting to {}'.format(uri))

        self._cf.open_link(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)

        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')

        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:

    try:

        bridge.run()

    except KeyboardInterrupt:

        print('\n')

        bridge.stop()