Commit 550cf702 authored by Marlene Böhmer's avatar Marlene Böhmer
Browse files

PRRT Link working now

parent 7f892829
Loading
Loading
Loading
Loading
+1103 −0

File added.

Preview size limit exceeded, changes collapsed.

+5523 −0

File added.

Preview size limit exceeded, changes collapsed.

+12576 −0

File added.

Preview size limit exceeded, changes collapsed.

+7571 −0

File added.

Preview size limit exceeded, changes collapsed.

+8 −26
Original line number Diff line number Diff line
@@ -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__':