Add Bridge Script

salt/drone/scripts/bridge.py

+#!/usr/bin/env python
+
+import logging
+import sys
+import re
+import time
+
+import prrt
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crtp.crtpstack import CRTPPacket
+
+logging.basicConfig(level=logging.DEBUG)
+
+MTU = 32
+DEFAULT_TARGET_DELAY = 200
+PRRT_LOCAL_PORT = 5000
+
+
+class CrazyflieConnection:
+    def __init__(self, uri, receive_callback):
+        self.receive_callback = receive_callback
+
+        self._cf = Crazyflie()
+
+        self._cf.connected.add_callback(self._connected)
+        self._cf.disconnected.add_callback(self._disconnected)
+        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)
+
+        # 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)
+
+        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))
+        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))
+
+    def _disconnected(self, link_uri):
+        """Callback when the Crazyflie is disconnected (called in all cases)"""
+        print('Disconnected from %s' % link_uri)
+        self.is_connected = False
+
+    def close(self):
+        print('Closing Crazyflie Connection')
+        self._cf.close_link()
+
+
+class ClientConnection:
+    def __init__(self, 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))
+
+        self._prrt_socket = prrt.PrrtSocket(("0.0.0.0", PRRT_LOCAL_PORT), maximum_payload_size=MTU,
+                                            target_delay=target_delay_us)
+#        self.prrt_socket.coding_configuration = prrt.PrrtCodingConfiguration(1, 1, [0])
+        self._prrt_socket.connect((address, port))
+
+    def send(self, pk):
+        print('send to client')
+
+    def receive(self):
+        print('receive from client')
+        pk = None
+        return pk
+
+    def close(self):
+        print('Closing Client Connection')
+#        self._prrt_socket.close()
+
+
+class Bridge:
+    def __init__(self, crazyflie_uri, client_uri):
+        cflib.crtp.init_drivers(enable_debug_driver=False)
+
+        self._client_connection = ClientConnection(client_uri)
+        self._crazyflie_connection = CrazyflieConnection(crazyflie_uri, self._client_connection.send)
+
+    def run(self):
+        while self._crazyflie_connection.is_connected:
+            pk = self._client_connection.receive()
+            if pk:
+                self._crazyflie_connection.send(pk)
+            time.sleep(2)
+        self.stop()
+
+    def stop(self):
+        self._client_connection.close()
+        self._crazyflie_connection.close()
+
+
+if __name__ == '__main__':
+    serial_uri = 'serial://pi'
+    prrt_uri = sys.argv[1]
+
+    bridge = Bridge(serial_uri, prrt_uri)
+
+    try:
+        bridge.run()
+    except KeyboardInterrupt:
+        bridge.stop()
+

salt/drone/scripts/prrt_simple_flightscript.py

@@ -11,7 +11,7 @@ import cflib.crtp
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 from cflib.positioning.motion_commander import MotionCommander
 
-URI = 'prrt://10.8.0.32:6000/250'
+URI = 'prrt://10.8.0.208:5000/250'
 
 # Only output errors from the logging framework
 logging.basicConfig(level=logging.ERROR)