Bridge connection to Crazyflie

salt/drone/scripts/bridge.py

@@ -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()