Prrt send and receive - Segmentation fault

salt/drone/scripts/bridge.py

@@ -10,8 +10,9 @@ import prrt
 
 import cflib.crtp
 from cflib.crazyflie import Crazyflie, State
-from cflib.crtp.crtpstack import CRTPPacket
+from cflib.crtp.crtpstack import CRTPPacket, CRTPPort
 
+logging.basicConfig(stream=sys.stdout, level=logging.INFO)
 logger = logging.getLogger(__name__)
 
 MTU = 32
@@ -65,7 +66,7 @@ class CrazyflieConnection:
             self._cf.connection_failed.call(uri, exception_text)
 
         # Variable used to keep main loop occupied until disconnect
-        self.is_connected = True
+        self.is_connected = False
 
     def _get_initial_packet(self, pk):
         self._platform_information_packet = pk
@@ -77,9 +78,11 @@ class CrazyflieConnection:
 
     def _connected(self, link_uri):
         logger.info('Connected to {}'.format(link_uri))
+        self.is_connected = True
         self._cf.packet_received.add_callback(self.receive_callback)
 
     def send(self, pk):
+        print('Send ' + str(pk))
         self._cf.send_packet(pk)
 
     def _connection_failed(self, link_uri, msg):
@@ -121,12 +124,20 @@ class ClientConnection:
         self._prrt_socket.connect((address, port))
 
     def send(self, pk):
-        print('send to client' + pk)
+        pk_bytes = bytearray([pk.get_header()]) + pk.data
+        self._prrt_socket.send(pk_bytes)
 
     def receive(self):
-        print('receive from client')
-        pk = None
-        return pk
+        pk_bytes, _ = self._prrt_socket.receive_asap()
+        if len(pk_bytes) > 0:
+            pk = CRTPPacket(pk_bytes[0], pk_bytes[1:])
+            print('Received: ' + str(pk) + ' Data Length: ' + str(len(pk.data)))
+            if pk.port == CRTPPort.LINKCTRL and pk.channel == 3:
+                return None
+            else:
+                return pk
+        else:
+            return None
 
     def close(self):
         logger.info('Closing Client Connection')
@@ -141,12 +152,15 @@ class Bridge:
         self._client_connection = ClientConnection(client_uri)
         self._crazyflie_connection = CrazyflieConnection(crazyflie_uri, self._client_connection.send)
 
+    def wait_for_crazyflie_connection(self):
+        while not self._crazyflie_connection.is_connected:
+            time.sleep(1)
+
     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):
@@ -161,6 +175,7 @@ if __name__ == '__main__':
     bridge = Bridge(serial_uri, prrt_uri)
 
     try:
+        bridge.wait_for_crazyflie_connection()
         bridge.run()
     except KeyboardInterrupt:
         print('\n')

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.208:5000/250'
+URI = 'prrt://10.8.0.208:5000'
 
 # Only output errors from the logging framework
 logging.basicConfig(level=logging.ERROR)