PRRT Link working now

salt/drone/scripts/bridge.py

@@ -24,7 +24,6 @@ class CrazyflieConnection:
     def __init__(self, uri, receive_callback):
         self.uri = uri
         self.receive_callback = receive_callback
-        # self._platform_information_packet = None
 
         self._cf = Crazyflie()
 
@@ -51,7 +50,6 @@ class CrazyflieConnection:
             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
@@ -68,11 +66,6 @@ class CrazyflieConnection:
         # Variable used to keep main loop occupied until disconnect
         self.is_connected = False
 
-    # def _get_initial_packet(self, pk):
-    #     if pk.port == CRTPPort.LINKCTRL and pk.channel == 1:
-    #         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)
@@ -80,15 +73,12 @@ 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)
+        self._cf.packet_received.callbacks = [self.receive_callback]
+        self._cf.incoming.cb = []
 
     def send(self, pk):
-        print('Serial send:', pk)
         self._cf.send_packet(pk)
 
-    # def get_platform_information_packet(self):
-    #     return self._platform_information_packet
-
     def _connection_failed(self, 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))
@@ -104,6 +94,8 @@ class CrazyflieConnection:
     def close(self):
         logger.info('Closing Crazyflie Connection')
         self._cf.close_link()
+        self._cf.packet_received.remove_callback(self.receive_callback)
+        self.receive_callback = None
 
 
 class ClientConnection:
@@ -129,15 +121,12 @@ class ClientConnection:
 
     def send(self, pk):
         pk_bytes = bytearray([pk.get_header()]) + pk.data
-        print('PRRT try sending:', pk_bytes)
         self._prrt_socket.send_sync(pk_bytes)
-        print('PRRT completed sending')
 
     def receive(self):
         pk_bytes, _ = self._prrt_socket.receive_asap()
         if len(pk_bytes) > 0:
             pk = CRTPPacket(pk_bytes[0], pk_bytes[1:])
-            print('PRRT receive:', pk)
             return pk
         else:
             return None
@@ -154,32 +143,25 @@ class Bridge:
 
         self._client_connection = ClientConnection(client_uri)
         self._crazyflie_connection = CrazyflieConnection(crazyflie_uri, self._client_connection.send)
+        self.stop_running = False
 
     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:
+        while not self.stop_running and self._crazyflie_connection.is_connected:
             pk = self._client_connection.receive()
             if pk:
                 if pk.port == CRTPPort.LINKCTRL and pk.channel == 3:
                     continue
-                # elif pk.port == CRTPPort.LINKCTRL and pk.channel == 1:
-                #     print('platform information request')
-                #     platform_information = self._crazyflie_connection.get_platform_information_packet()
-                #     if platform_information is not None:
-                #         print('platform information reply')
-                #         self._client_connection.send(platform_information)
-                #         print('platform information reply send')
-                #         continue
                 self._crazyflie_connection.send(pk)
-                print('Bridge send finished')
         self.stop()
 
     def stop(self):
-        self._client_connection.close()
+        self.stop_running = True
         self._crazyflie_connection.close()
+        self._client_connection.close()
 
 
 if __name__ == '__main__':