Exemplo n.º 1
0
    def batch_read_flash(self, addr=0xFF, page=0x00):
        """Read back a flash page from the Crazyflie and return it"""
        page_size = self.targets[addr].page_size
        batch_index = []

        for i in range(0, int(math.ceil(page_size / 25.0))):
            batch_index.append(1)
        buff = bytearray(len(batch_index) * 25)
        while not sum(batch_index) == 0:
            for index in range(len(batch_index)):
                if batch_index[index] == 1:
                    pk = CRTPPacket()
                    pk.set_header(0xFF, 0xFF)
                    pk.data = struct.pack('<BBHH', addr, 0x1C, page,
                                          (index * 25))
                    self.link.send_packet(pk)

                    pk = self.link.receive_packet(0)
                    if pk and len(pk.data) >= 6:
                        temp_pk = struct.unpack('<BBHH', pk.data[0:6])

                        back_element = int(temp_pk[3])
                        if list(temp_pk[:2]) == [addr, 0x1C]:
                            buff[back_element:back_element + 25] = pk.data[6:]
                            batch_index[int(back_element / 25)] = 0
        return buff[0:page_size]
Exemplo n.º 2
0
    def bandwidth(self, uri, packet_size=4, count=500):
        link = cflib.crtp.get_link_driver(uri, link_error_callback=self.error_cb)
        # # wait until no more packets in queue
        # while True:
        #     pk = link.receive_packet(0.5)
        #     if not pk:
        #         break

        # enqueue packets
        start_time = time.time()
        for i in range(count):
            pk = CRTPPacket()
            pk.set_header(CRTPPort.LINKCTRL, 0)  # Echo channel
            pk.data = self.build_data(i, packet_size)
            link.send_packet(pk)

        # get the result
        for i in range(count):
            while True:
                pk_ack = link.receive_packet(-1)
                if pk_ack.port == CRTPPort.LINKCTRL and pk_ack.channel == 0:
                    break
            # make sure we actually received the expected value
            i_recv, = struct.unpack('<I', pk_ack.data[0:4])
            self.assertEqual(i, i_recv)
        end_time = time.time()
        link.close()
        result = count / (end_time - start_time)
        kbps = (count * packet_size) / 1024 / (end_time - start_time)
        print('Bandwith for {} (packet size {} B): {:.2f} packets/s ({:.2f} kB/s)'.format(
            uri, packet_size, result, kbps))
        return result
    def _update_info(self):
        """ Call the command getInfo and fill up the information received in
        the fields of the object
        """

        #Call getInfo ...
        pk = CRTPPacket()
        pk.set_header(0xFF, 0xFF)
        pk.data = (0xFF, 0x10)
        self.link.send_packet(pk)

        #Wait for the answer
        pk = self.link.receive_packet(2)

        if (pk and pk.header == 0xFF and
                struct.unpack("<BB", pk.data[0:2]) == (0xFF, 0x10)):
            tab = struct.unpack("BBHHHH", pk.data[0:10])
            cpuid = struct.unpack("B"*12, pk.data[10:22])
            if len(pk.data)>22:
                self.protocol_version = pk.data[22]
            self.page_size = tab[2]
            self.buffer_pages = tab[3]
            self.flash_pages = tab[4]
            self.start_page = tab[5]
            self.cpuid = "%02X" % cpuid[0]
            for i in cpuid[1:]:
                self.cpuid += ":%02X" % i

            return True

        return False
Exemplo n.º 4
0
 def _request_param_update(self, varid):
     """Send a request to the Crazyflie for an update of a param value"""
     logger.debug("Requesting update for varid %d", varid)
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     pk.data = struct.pack('<B', varid)
     self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 5
0
    def start(self):
        """Start the logging for this entry"""
        if (self.cf.link is not None):
            if (self._added is False):
                logger.debug("First time block is started, add block")
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_CREATE_BLOCK, self.id)
                for var in self.variables:
                    if (var.is_toc_variable() is False):  # Memory location
                        logger.debug("Logging to raw memory %d, 0x%04X",
                                     var.get_storage_and_fetch_byte(), var.address)
                        pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<I', var.address)
                    else:  # Item in TOC
                        logger.debug("Adding %s with id=%d and type=0x%02X",
                                     var.name,
                                     self.cf.log._toc.get_element_id(
                                     var.name), var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<B', self.cf.log._toc.
                                               get_element_id(var.name))
                logger.debug("Adding log block id {}".format(self.id))
                self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))

            else:
                logger.debug("Block already registered, starting logging"
                             " for id=%d", self.id)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_START_LOGGING, self.id, self.period)
                self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id))
Exemplo n.º 6
0
    def _update_info(self):
        """ Call the command getInfo and fill up the information received in
        the fields of the object
        """

        #Call getInfo ...
        pk = CRTPPacket()
        pk.set_header(0xFF, 0xFF)
        pk.data = (0xFF, 0x10)
        self.link.send_packet(pk)

        #Wait for the answer
        pk = self.link.receive_packet(2)

        if (pk and pk.header == 0xFF and
                struct.unpack("<BB", pk.data[0:2]) == (0xFF, 0x10)):
            tab = struct.unpack("BBHHHH", pk.data[0:10])
            cpuid = struct.unpack("B" * 12, pk.data[10:22])
            if len(pk.data) > 22:
                self.protocol_version = pk.data[22]
            self.page_size = tab[2]
            self.buffer_pages = tab[3]
            self.flash_pages = tab[4]
            self.start_page = tab[5]
            self.cpuid = "%02X" % cpuid[0]
            for i in cpuid[1:]:
                self.cpuid += ":%02X" % i

            return True

        return False
Exemplo n.º 7
0
    def latency(self, uri, packet_size=4, count=500):
        link = cflib.crtp.get_link_driver(uri)
        # # wait until no more packets in queue
        # while True:
        #     pk = link.receive_packet(0.5)
        #     print(pk)
        #     if not pk or pk.header == 0xFF:
        #         break

        pk = CRTPPacket()
        pk.set_header(CRTPPort.LINKCTRL, 0)  # Echo channel

        latencies = []
        for i in range(count):
            pk.data = self.build_data(i, packet_size)

            start_time = time.time()
            link.send_packet(pk)
            while True:
                pk_ack = link.receive_packet(-1)
                if pk_ack.port == CRTPPort.LINKCTRL and pk_ack.channel == 0:
                    break
            end_time = time.time()

            # make sure we actually received the expected value
            i_recv, = struct.unpack('<I', pk_ack.data[0:4])
            self.assertEqual(i, i_recv)
            latencies.append((end_time - start_time) * 1000)
        link.close()
        result = np.min(latencies)
        print('Latency for {} (packet size {} B): {:.2f} ms'.format(uri, packet_size, result))
        return result
Exemplo n.º 8
0
    def set_value(self, complete_name, value):
        """
        Set the value for the supplied parameter.
        """
        element = self.toc.get_element_by_complete_name(complete_name)

        if not element:
            logger.warning("Cannot set value for [%s], it's not in the TOC!",
                           complete_name)
            raise KeyError('{} not in param TOC'.format(complete_name))
        elif element.access == ParamTocElement.RO_ACCESS:
            logger.debug('[%s] is read only, no trying to set value',
                         complete_name)
            raise AttributeError('{} is read-only!'.format(complete_name))
        else:
            varid = element.ident
            pk = CRTPPacket()
            pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
            if self._useV2:
                pk.data = struct.pack('<H', varid)
            else:
                pk.data = struct.pack('<B', varid)

            try:
                value_nr = eval(value)
            except TypeError:
                value_nr = value

            pk.data += struct.pack(element.pytype, value_nr)
            self.param_updater.request_param_setvalue(pk)
Exemplo n.º 9
0
 def create(self):
     """Save the log configuration in the Crazyflie"""
     pk = CRTPPacket()
     pk.set_header(5, CHAN_SETTINGS)
     if self.useV2:
         pk.data = (CMD_CREATE_BLOCK_V2, self.id)
     else:
         pk.data = (CMD_CREATE_BLOCK, self.id)
     for var in self.variables:
         if (var.is_toc_variable() is False):  # Memory location
             logger.debug('Logging to raw memory %d, 0x%04X',
                          var.get_storage_and_fetch_byte(), var.address)
             pk.data.append(struct.pack('<B',
                                        var.get_storage_and_fetch_byte()))
             pk.data.append(struct.pack('<I', var.address))
         else:  # Item in TOC
             logger.debug('Adding %s with id=%d and type=0x%02X',
                          var.name,
                          self.cf.log.toc.get_element_id(
                              var.name), var.get_storage_and_fetch_byte())
             pk.data.append(var.get_storage_and_fetch_byte())
             if self.useV2:
                 ident = self.cf.log.toc.get_element_id(var.name)
                 pk.data.append(ident & 0x0ff)
                 pk.data.append((ident >> 8) & 0x0ff)
             else:
                 pk.data.append(self.cf.log.toc.get_element_id(var.name))
     logger.debug('Adding log block id {}'.format(self.id))
     if self.useV2:
         self.cf.send_packet(pk, expected_reply=(
             CMD_CREATE_BLOCK_V2, self.id))
     else:
         self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
Exemplo n.º 10
0
    def upload_buffer(self, target_id, page, address, buff):
        """Upload data into a buffer on the Crazyflie"""
        #print len(buff)
        count = 0
        pk = CRTPPacket()
        pk.set_header(0xFF, 0xFF)
        pk.data = struct.pack("=BBHH", target_id, 0x14, page, address)

        for i in range(0, len(buff)):
            #print "[0x%02X]" %  ord(buff[i]),
            pk.data += buff[i]

            count += 1

            if count > 24:
                self.link.send_packet(pk)
                count = 0
                pk = CRTPPacket()
                pk.set_header(0xFF, 0xFF)
                pk.data = struct.pack("=BBHH", target_id, 0x14, page,
                                      i + address + 1)

                #sys.stdout.write("+")
                #sys.stdout.flush()

        self.link.send_packet(pk)
Exemplo n.º 11
0
 def request_param_update(self, var_id):
     """Place a param update request on the queue"""
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     pk.data = struct.pack('<B', var_id)
     logger.debug('Requesting request to update param [%d]', var_id)
     self.request_queue.put(pk)
Exemplo n.º 12
0
    def start(self):
        if (self.cf.link is not None):
            if (self.blockCreated is False):
                logger.debug("First time block is started, add block")
                self.blockCreated = True
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_CREATE_BLOCK, self.blockId)
                for v in self.logconf.getVariables():
                    if (v.isTocVariable() is False):  # Memory location
                        logger.debug("Logging to raw memory %d, 0x%04X",
                                     v.getStoredFetchAs(), v.getAddress())
                        pk.data += struct.pack('<B', v.getStoredFetchAs())
                        pk.data += struct.pack('<I', v.getAddress())
                    else:  # Item in TOC
                        logger.debug("Adding %s with id=%d and type=0x%02X",
                                     v.getName(),
                                     self.cf.log.toc.get_element_id(
                                     v.getName()), v.getStoredFetchAs())
                        pk.data += struct.pack('<B', v.getStoredFetchAs())
                        pk.data += struct.pack('<B', self.cf.log.toc.
                                               get_element_id(v.getName()))
                logger.debug("Adding log block id {}".format(self.blockId))
                self.cf.send_packet(pk)

            else:
                logger.debug("Block already registered, starting logging"
                             " for %d", self.blockId)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_START_LOGGING, self.blockId, self.period)
                self.cf.send_packet(pk)
    def get_default_value(self, complete_name, callback):
        """
        Get the default value of the specified parameter.
        The supplied callback will be called with the name of the parameter
        as well as the default value. None if there is an error.

        @param complete_name The 'group.name' name of the parameter to store
        @param callback The callback should take `complete_name` and default value as argument
        """
        element = self.toc.get_element_by_complete_name(complete_name)

        def new_packet_cb(pk):
            if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_GET_DEFAULT_VALUE:
                if pk.data[3] == errno.ENOENT:
                    callback(complete_name, None)
                    self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)
                    return

                default_value, = struct.unpack(element.pytype, pk.data[3:])
                callback(complete_name, default_value)
                self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)

        self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb)

        pk = CRTPPacket()
        pk.set_header(CRTPPort.PARAM, MISC_CHANNEL)
        pk.data = struct.pack('<BH', MISC_GET_DEFAULT_VALUE, element.ident)
        self.param_updater.send_param_misc(pk)
    def set_value(self, complete_name, value):
        """
        Set the value for the supplied parameter.
        """
        if not self._initialized.wait(timeout=60):
            raise Exception('Connection timed out')

        element = self.toc.get_element_by_complete_name(complete_name)

        if not element:
            logger.warning("Cannot set value for [%s], it's not in the TOC!",
                           complete_name)
            raise KeyError('{} not in param TOC'.format(complete_name))
        elif element.access == ParamTocElement.RO_ACCESS:
            logger.debug('[%s] is read only, no trying to set value',
                         complete_name)
            raise AttributeError('{} is read-only!'.format(complete_name))
        else:
            varid = element.ident
            pk = CRTPPacket()
            pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
            if self._useV2:
                pk.data = struct.pack('<H', varid)
            else:
                pk.data = struct.pack('<B', varid)

            try:
                value_nr = eval(value)
            except TypeError:
                value_nr = value

            pk.data += struct.pack(element.pytype, value_nr)
            self.param_updater.request_param_setvalue(pk)
Exemplo n.º 15
0
 def _request_param_update(self, varid):
     """Send a request to the Crazyflie for an update of a param value"""
     logger.debug("Requesting update for varid %d", varid)
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     pk.data = struct.pack('<B', varid)
     self.cf.send_packet(pk, expect_answer=True)
    def persistent_store(self, complete_name, callback=None):
        """
        Store the current value of the specified persistent parameter to
        eeprom. The supplied callback will be called with `True` as an
        argument on success, and with `False` as an argument on failure.

        @param complete_name The 'group.name' name of the parameter to store
        @param callback Optional callback should take `complete_name` and boolean status as arguments
        """
        element = self.toc.get_element_by_complete_name(complete_name)
        if not element.is_persistent():
            raise AttributeError(f"Param '{complete_name}' is not persistent")

        def new_packet_cb(pk):
            if pk.channel == MISC_CHANNEL and pk.data[0] == MISC_PERSISTENT_STORE:
                callback(complete_name, pk.data[3] == 0)
                self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb)

        if callback is not None:
            self.cf.add_port_callback(CRTPPort.PARAM, new_packet_cb)

        pk = CRTPPacket()
        pk.set_header(CRTPPort.PARAM, MISC_CHANNEL)
        pk.data = struct.pack('<BH', MISC_PERSISTENT_STORE, element.ident)
        self.param_updater.send_param_misc(pk)
Exemplo n.º 17
0
    def _write_new_chunk(self):
        """
        Called to request a new chunk of data to be read from the Crazyflie
        """
        # Figure out the length of the next request
        new_len = len(self._data)
        if new_len > _WriteRequest.MAX_DATA_LENGTH:
            new_len = _WriteRequest.MAX_DATA_LENGTH

        logger.info('Writing new chunk of {}bytes at 0x{:X}'.format(
            new_len, self._current_addr))

        data = self._data[:new_len]
        self._data = self._data[new_len:]

        pk = CRTPPacket()
        pk.set_header(CRTPPort.MEM, CHAN_WRITE)
        pk.data = struct.pack('<BI', self.mem.id, self._current_addr)
        # Create a tuple used for matching the reply using id and address
        reply = struct.unpack('<BBBBB', pk.data)
        self._sent_reply = reply
        # Add the data
        pk.data += struct.pack('B' * len(data), *data)
        self._sent_packet = pk
        self.cf.send_packet(pk, expected_reply=reply, timeout=1)

        self._addr_add = len(data)
Exemplo n.º 18
0
    def _write_new_chunk(self):
        """
        Called to request a new chunk of data to be read from the Crazyflie
        """
        # Figure out the length of the next request
        new_len = len(self._data)
        if new_len > _WriteRequest.MAX_DATA_LENGTH:
            new_len = _WriteRequest.MAX_DATA_LENGTH

        logger.info('Writing new chunk of {}bytes at 0x{:X}'.format(
            new_len, self._current_addr))

        data = self._data[:new_len]
        self._data = self._data[new_len:]

        pk = CRTPPacket()
        pk.set_header(CRTPPort.MEM, CHAN_WRITE)
        pk.data = struct.pack('<BI', self.mem.id, self._current_addr)
        # Create a tuple used for matching the reply using id and address
        reply = struct.unpack('<BBBBB', pk.data)
        self._sent_reply = reply
        # Add the data
        pk.data += struct.pack('B' * len(data), *data)
        self._sent_packet = pk
        self.cf.send_packet(pk, expected_reply=reply, timeout=1)

        self._addr_add = len(data)
Exemplo n.º 19
0
 def _request_protocol_version(self):
     # Sending a sink request to detect if the connected Crazyflie
     # supports protocol versioning
     pk = CRTPPacket()
     pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE)
     pk.data = (0,)
     self._cf.send_packet(pk)
Exemplo n.º 20
0
    def reset_to_firmware(self, target_id):
        """ Reset to firmware
        The parameter cpuid shall correspond to the device to reset.

        Return true if the reset has been done
        """
        fake_cpu_id = (1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12)
        #Send the reset to bootloader request
        pk = CRTPPacket()
        pk.set_header(0xFF, 0xFF)
        pk.data = (target_id, 0xFF) + fake_cpu_id
        self.link.send_packet(pk)

        #Wait to ack the reset ...
        pk = None
        while True:
            pk = self.link.receive_packet(2)
            if not pk:
                return False

            if (pk.header == 0xFF and
                struct.unpack("B" * len(pk.data),
                              pk.data)[:2] == (target_id, 0xFF)):
                pk.data = (target_id, 0xF0, 0x01)
                self.link.send_packet(pk)
                break

        time.sleep(0.1)
Exemplo n.º 21
0
    def read_flash(self, addr=0xFF, page=0x00):
        """Read back a flash page from the Crazyflie and return it"""
        buff = bytearray()

        page_size = self.targets[addr].page_size

        for i in range(0, int(math.ceil(page_size / 25.0))):
            pk = None
            retry_counter = 5
            while ((not pk or pk.header != 0xFF or
                    struct.unpack('<BB', pk.data[0:2]) != (addr, 0x1C)) and
                    retry_counter >= 0):
                pk = CRTPPacket()
                pk.set_header(0xFF, 0xFF)
                pk.data = struct.pack('<BBHH', addr, 0x1C, page, (i * 25))
                self.link.send_packet(pk)

                pk = self.link.receive_packet(1)
                retry_counter -= 1
            if (retry_counter < 0):
                return None
            else:
                buff += pk.data[6:]

        # For some reason we get one byte extra here...
        return buff[0:page_size]
Exemplo n.º 22
0
    def start(self):
        """Start the logging for this entry"""
        if (self.cf.link is not None):
            if (self.block_created is False):
                logger.debug("First time block is started, add block")
                self.block_created = True
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_CREATE_BLOCK, self.block_id)
                for var in self.logconf.getVariables():
                    if (var.isTocVariable() is False):  # Memory location
                        logger.debug("Logging to raw memory %d, 0x%04X",
                                     var.getStoredFetchAs(), var.getAddress())
                        pk.data += struct.pack('<B', var.getStoredFetchAs())
                        pk.data += struct.pack('<I', var.getAddress())
                    else:  # Item in TOC
                        logger.debug("Adding %s with id=%d and type=0x%02X",
                                     var.getName(),
                                     self.cf.log.toc.get_element_id(
                                     var.getName()), var.getStoredFetchAs())
                        pk.data += struct.pack('<B', var.getStoredFetchAs())
                        pk.data += struct.pack('<B', self.cf.log.toc.
                                               get_element_id(var.getName()))
                logger.debug("Adding log block id {}".format(self.block_id))
                self.cf.send_packet(pk)

            else:
                logger.debug("Block already registered, starting logging"
                             " for %d", self.block_id)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_START_LOGGING, self.block_id, self.period)
                self.cf.send_packet(pk)
Exemplo n.º 23
0
    def write_flash(self, addr, page_buffer, target_page, page_count):
        """Initate flashing of data in the buffer to flash."""
        #print "Write page", flashPage
        #print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
        pk = None
        retry_counter = 5
        #print "Flasing to 0x{:X}".format(addr)
        while ((not pk or pk.header != 0xFF or
                struct.unpack("<BB", pk.data[0:2]) != (addr, 0x18))
                and retry_counter >= 0):
            pk = CRTPPacket()
            pk.set_header(0xFF, 0xFF)
            pk.data = struct.pack("<BBHHH", addr, 0x18, page_buffer,
                                  target_page, page_count)
            self.link.send_packet(pk)
            pk = self.link.receive_packet(1)
            retry_counter -= 1

        if retry_counter < 0:
            self.error_code = -1
            return False

        self.error_code = ord(pk.data[3])

        return ord(pk.data[2]) == 1
Exemplo n.º 24
0
    def reset_to_bootloader(self, target_id):
        retry_counter = 5
        pk = CRTPPacket()
        pk.set_header(0xFF, 0xFF)
        pk.data = (target_id, 0xFF)
        self.link.send_packet(pk)

        pk = self.link.receive_packet(1)

        while ((not pk or pk.header != 0xFF or
                struct.unpack("<BB", pk.data[0:2]) != (target_id, 0xFF)) and
                retry_counter >= 0 ):

            pk = self.link.receive_packet(1)
            retry_counter -= 1

        if pk:
            new_address = (0xb1, ) + struct.unpack("<BBBB", pk.data[2:6][::-1])

            pk = CRTPPacket()
            pk.set_header(0xFF, 0xFF)
            pk.data = (target_id, 0xF0, 0x00)
            self.link.send_packet(pk)

            addr = int(struct.pack("B"*5, *new_address).encode('hex'), 16)

            time.sleep(0.2)
            self.link.close()
            time.sleep(0.2)
            self.link = cflib.crtp.get_link_driver("radio://0/0/2M/{}".format(addr))

            return True
        else:
            return False
Exemplo n.º 25
0
    def read_flash(self, addr=0xFF, page=0x00):
        """Read back a flash page from the Crazyflie and return it"""
        buff = bytearray()

        page_size = self.targets[addr].page_size

        for i in range(0, int(math.ceil(page_size / 25.0))):
            pk = None
            retry_counter = 5
            while ((not pk or pk.header != 0xFF
                    or struct.unpack('<BB', pk.data[0:2]) != (addr, 0x1C))
                   and retry_counter >= 0):
                pk = CRTPPacket()
                pk.set_header(0xFF, 0xFF)
                pk.data = struct.pack('<BBHH', addr, 0x1C, page, (i * 25))
                self.link.send_packet(pk)

                pk = self.link.receive_packet(1)
                retry_counter -= 1
            if (retry_counter < 0):
                return None
            else:
                buff += pk.data[6:]

        # For some reason we get one byte extra here...
        return buff[0:page_size]
Exemplo n.º 26
0
    def _ramp_motors(self):
        thrust_mult = 1
        thrust_step = 100
        thrust_base = 3000
        thrust = thrust_base
        pitch = 0
        roll = 0
        yawrate = 0

        # Unlock startup thrust protection
        pk = CRTPPacket()
        pk.set_header(0xF, 2)  # unlock drone
        pk.data = '0'
        self._cf.send_packet(pk)

        while thrust >= thrust_base:
            self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
            time.sleep(0.1)
            if thrust >= thrust_base + 1000:
                thrust_mult = -1
            thrust += thrust_step * thrust_mult

        # Make sure that the last packet leaves before the link is closed
        # since the message queue is not flushed before closing
        self._cf.commander.send_setpoint(0, 0, 0, 0)
        time.sleep(0.2)
        self._cf.close_link()
Exemplo n.º 27
0
 def request_param_update(self, var_id):
     """Place a param update request on the queue"""
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     pk.data = struct.pack('<B', var_id)
     logger.debug("Requesting request to update param [%d]", var_id)
     self.request_queue.put(pk)
Exemplo n.º 28
0
    def start(self):
        """Start the logging for this entry"""
        if (self.cf.link is not None):
            if (self._added is False):
                logger.debug("First time block is started, add block")
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_CREATE_BLOCK, self.id)
                for var in self.variables:
                    if (var.is_toc_variable() is False):  # Memory location
                        logger.debug("Logging to raw memory %d, 0x%04X",
                                     var.get_storage_and_fetch_byte(), var.address)
                        pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<I', var.address)
                    else:  # Item in TOC
                        logger.debug("Adding %s with id=%d and type=0x%02X",
                                     var.name,
                                     self.cf.log.toc.get_element_id(
                                     var.name), var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
                        pk.data += struct.pack('<B', self.cf.log.toc.
                                               get_element_id(var.name))
                logger.debug("Adding log block id {}".format(self.id))
                self.cf.send_packet(pk, expect_answer=True)

            else:
                logger.debug("Block already registered, starting logging"
                             " for %d", self.id)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_START_LOGGING, self.id, self.period)
                self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 29
0
 def _request_toc_element(self, index):
     """Request information about a specific item in the TOC"""
     logger.debug("Requesting index %d on port %d", index, self.port)
     pk = CRTPPacket()
     pk.set_header(self.port, TOC_CHANNEL)
     pk.data = (CMD_TOC_ELEMENT, index)
     self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
Exemplo n.º 30
0
 def _request_toc_element(self, index):
     """Request information about a specific item in the TOC"""
     logger.debug("Requesting index %d on port %d", index, self.port)
     pk = CRTPPacket()
     pk.set_header(self.port, TOC_CHANNEL)
     pk.data = (CMD_TOC_ELEMENT, index)
     self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 31
0
 def create(self):
     """Save the log configuration in the Crazyflie"""
     pk = CRTPPacket()
     pk.set_header(5, CHAN_SETTINGS)
     if self.useV2:
         pk.data = (CMD_CREATE_BLOCK_V2, self.id)
     else:
         pk.data = (CMD_CREATE_BLOCK, self.id)
     for var in self.variables:
         if (var.is_toc_variable() is False):  # Memory location
             logger.debug('Logging to raw memory %d, 0x%04X',
                          var.get_storage_and_fetch_byte(), var.address)
             pk.data.append(
                 struct.pack('<B', var.get_storage_and_fetch_byte()))
             pk.data.append(struct.pack('<I', var.address))
         else:  # Item in TOC
             logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
                          self.cf.log.toc.get_element_id(var.name),
                          var.get_storage_and_fetch_byte())
             pk.data.append(var.get_storage_and_fetch_byte())
             if self.useV2:
                 ident = self.cf.log.toc.get_element_id(var.name)
                 pk.data.append(ident & 0x0ff)
                 pk.data.append((ident >> 8) & 0x0ff)
             else:
                 pk.data.append(self.cf.log.toc.get_element_id(var.name))
     logger.debug('Adding log block id {}'.format(self.id))
     if self.useV2:
         self.cf.send_packet(pk,
                             expected_reply=(CMD_CREATE_BLOCK_V2, self.id))
     else:
         self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
    def _new_packet_cb(self, packet):
        """Callback for newly arrived packets with TOC information"""
        chan = packet.channel
        cmd = packet.datal[0]
        payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])

        if (chan == CHAN_SETTINGS):
            block_id = ord(payload[0])
            error_status = ord(payload[1])
            block = self._find_block(block_id)
            if (cmd == CMD_CREATE_BLOCK):
                if (block is not None):
                    if (error_status == 0):  # No error
                        logger.debug("Have successfully added block_id=%d",
                                     block_id)

                        pk = CRTPPacket()
                        pk.set_header(5, CHAN_SETTINGS)
                        pk.data = (CMD_START_LOGGING, block_id,
                                   block.period)
                        self.cf.send_packet(pk)
                        block.added = True
                    else:
                        msg = self._err_codes[error_status]
                        logger.warning("Error %d when adding block_id=%d (%s)"
                                       , error_status, block_id, msg)
                        block.err_no = error_status
                        block.added_cb.call(False)
                        block.error.call(block.logconf, msg)

                else:
                    logger.warning("No LogEntry to assign block to !!!")
            if (cmd == CMD_START_LOGGING):
                if (error_status == 0x00):
                    logger.info("Have successfully logging for block=%d",
                                block_id)
                    if block:
                        block.started = True

                else:
                    msg = self._err_codes[error_status]
                    logger.warning("Error %d when starting block_id=%d (%s)"
                                   , error_status, new_block_id, msg)
                    if block:
                        block.err_no = error_status
                        block.started_cb.call(False)
                        block.error.call(block.logconf, msg)

        if (chan == CHAN_LOGDATA):
            chan = packet.channel
            block_id = packet.datal[0]
            block = self._find_block(block_id)
            timestamps = struct.unpack("<BBB", packet.data[1:4])
            timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
            logdata = packet.data[4:]
            if (block is not None):
                block.unpack_log_data(logdata, timestamp)
            else:
                logger.warning("Error no LogEntry to handle block=%d", block_id)
Exemplo n.º 33
0
    def request_extended_types(self, elements):
        self._count = len(elements)
        for element in elements:
            pk = CRTPPacket()
            pk.set_header(CRTPPort.PARAM, MISC_CHANNEL)

            pk.data = struct.pack('<BH', MISC_GET_EXTENDED_TYPE, element.ident)
            self.request_queue.put(pk)
 def set_continous_wave(self, enabled):
     """
     Enable/disable the client side X-mode. When enabled this recalculates
     the setpoints before sending them to the Crazyflie.
     """
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
     pk.data = (0, enabled)
     self._cf.send_packet(pk)
Exemplo n.º 35
0
 def set_continous_wave(self, enabled):
     """
     Enable/disable the client side X-mode. When enabled this recalculates
     the setpoints before sending them to the Crazyflie.
     """
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PLATFORM, 0)
     pk.data = (0, enabled)
     self._cf.send_packet(pk)
Exemplo n.º 36
0
 def stop(self):
     if (self.cf.link is not None):
         if (self.blockId is None):
             logger.warning("Stopping block, but no block registered")
         else:
             logger.debug("Sending stop logging for block %d", self.blockId)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_STOP_LOGGING, self.blockId)
             self.cf.send_packet(pk)
Exemplo n.º 37
0
 def stop(self):
     if (self.cf.link is not None):
         if (self.blockId is None):
             logger.warning("Stopping block, but no block registered")
         else:
             logger.debug("Sending stop logging for block %d", self.blockId)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_STOP_LOGGING, self.blockId)
             self.cf.send_packet(pk)
Exemplo n.º 38
0
    def refresh_toc(self, refreshDoneCallback, toc_cache):
        pk = CRTPPacket()
        pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
        pk.data = (CMD_RESET_LOGGING, )
        self.cf.send_packet(pk)

        self.toc = Toc()
        tocFetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
                                self.toc, refreshDoneCallback, toc_cache)
        tocFetcher.start()
Exemplo n.º 39
0
    def refresh_toc(self, refreshDoneCallback, toc_cache):
        pk = CRTPPacket()
        pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
        pk.data = (CMD_RESET_LOGGING, )
        self.cf.send_packet(pk)

        self.toc = Toc()
        tocFetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
                                self.toc, refreshDoneCallback, toc_cache)
        tocFetcher.start()
Exemplo n.º 40
0
 def reset_logging(scf):
     """Reset onboard logging for this Crazyflie"""
     print('Resetting onboard log settings for: ' + scf.cf.link_uri)
     CHAN_SETTINGS = 1
     CMD_RESET_LOGGING = 5
     if scf.cf.link is not None:
         pk = CRTPPacket()
         pk.set_header(5, CHAN_SETTINGS)
         pk.data = (CMD_RESET_LOGGING, )
         scf.cf.send_packet(pk)
Exemplo n.º 41
0
 def stop(self):
     """Stop the logging for this entry"""
     if (self.cf.link is not None):
         if (self.id is None):
             logger.warning("Stopping block, but no block registered")
         else:
             logger.debug("Sending stop logging for block %d", self.id)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_STOP_LOGGING, self.id)
             self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 42
0
 def request_param_update(self, var_id):
     """Place a param update request on the queue"""
     self._useV2 = self.cf.platform.get_protocol_version() >= 4
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     if self._useV2:
         pk.data = struct.pack('<H', var_id)
     else:
         pk.data = struct.pack('<B', var_id)
     logger.debug('Requesting request to update param [%d]', var_id)
     self.request_queue.put(pk)
Exemplo n.º 43
0
    def refresh_toc(self, refresh_done_callback, toc_cache):
        """Start refreshing the table of loggale variables"""

        self._toc_cache = toc_cache
        self._refresh_callback = refresh_done_callback
        self.toc = None

        pk = CRTPPacket()
        pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
        pk.data = (CMD_RESET_LOGGING, )
        self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING,))
 def request_param_update(self, var_id):
     """Place a param update request on the queue"""
     self._useV2 = self.cf.platform.get_protocol_version() >= 4
     pk = CRTPPacket()
     pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
     if self._useV2:
         pk.data = struct.pack('<H', var_id)
     else:
         pk.data = struct.pack('<B', var_id)
     logger.debug('Requesting request to update param [%d]', var_id)
     self.request_queue.put(pk)
Exemplo n.º 45
0
 def stop(self):
     """Stop the logging for this entry"""
     if (self.cf.link is not None):
         if (self.id is None):
             logger.warning("Stopping block, but no block registered")
         else:
             logger.debug("Sending stop logging for block %d", self.id)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_STOP_LOGGING, self.id)
             self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 46
0
    def refresh_toc(self, refresh_done_callback, toc_cache):
        """Start refreshing the table of loggale variables"""

        self._toc_cache = toc_cache
        self._refresh_callback = refresh_done_callback
        self.toc = None

        pk = CRTPPacket()
        pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
        pk.data = (CMD_RESET_LOGGING, )
        self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, ))
Exemplo n.º 47
0
 def delete(self):
     """Delete this entry in the Crazyflie"""
     if (self.cf.link is not None):
         if (self.id is None):
             logger.warning("Delete block, but no block registered")
         else:
             logger.debug("LogEntry: Sending delete logging for block %d"
                          % self.id)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_DELETE_BLOCK, self.id)
             self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 48
0
 def close(self):
     if (self.cf.link is not None):
         if (self.blockId is None):
             logger.warning("Delete block, but no block registered")
         else:
             logger.debug("LogEntry: Sending delete logging for block %d"
                          % self.blockId)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_DELETE_BLOCK, self.blockId)
             self.cf.send_packet(pk)
             self.blockId = None  # Wait until we get confirmation of delete
Exemplo n.º 49
0
    def start(self):
        """Initiate fetching of the TOC."""
        logger.debug("[%d]: Start fetching...", self.port)
        # Register callback in this class for the port
        self.cf.add_port_callback(self.port, self._new_packet_cb)

        # Request the TOC CRC
        self.state = GET_TOC_INFO
        pk = CRTPPacket()
        pk.set_header(self.port, TOC_CHANNEL)
        pk.data = (CMD_TOC_INFO, )
        self.cf.send_packet(pk, expect_answer=True)
Exemplo n.º 50
0
    def start(self):
        """Initiate fetching of the TOC."""
        logger.debug("[%d]: Start fetching...", self.port)
        # Register callback in this class for the port
        self.cf.add_port_callback(self.port, self._new_packet_cb)

        # Request the TOC CRC
        self.state = GET_TOC_INFO
        pk = CRTPPacket()
        pk.set_header(self.port, TOC_CHANNEL)
        pk.data = (CMD_TOC_INFO, )
        self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO, ))
Exemplo n.º 51
0
    def refresh_toc(self, refresh_done_callback, toc_cache):
        """Start refreshing the table of loggale variables"""
        pk = CRTPPacket()
        pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
        pk.data = (CMD_RESET_LOGGING, )
        self.cf.send_packet(pk, expect_answer=True)

        self.log_blocks = []

        self.toc = Toc()
        toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
                                self.toc, refresh_done_callback, toc_cache)
        toc_fetcher.start()
Exemplo n.º 52
0
 def start(self):
     """Start the logging for this entry"""
     if (self.cf.link is not None):
         if (self._added is False):
             self.create()
             logger.debug("First time block is started, add block")
         else:
             logger.debug("Block already registered, starting logging"
                          " for id=%d", self.id)
             pk = CRTPPacket()
             pk.set_header(5, CHAN_SETTINGS)
             pk.data = (CMD_START_LOGGING, self.id, self.period)
             self.cf.send_packet(pk, expected_reply=(CMD_START_LOGGING, self.id))
Exemplo n.º 53
0
 def _request_toc_element(self, index):
     """Request information about a specific item in the TOC"""
     logger.debug('Requesting index %d on port %d', index, self.port)
     pk = CRTPPacket()
     if self._useV2:
         pk.set_header(self.port, TOC_CHANNEL)
         pk.data = (CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff)
         self.cf.send_packet(pk, expected_reply=(
             CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff))
     else:
         pk.set_header(self.port, TOC_CHANNEL)
         pk.data = (CMD_TOC_ELEMENT, index)
         self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
 def _crt_service_callback(self, pk):
     if pk.channel == LINKSERVICE_SOURCE:
         # If the sink contains a magic string, get the protocol version,
         # otherwise -1
         if pk.data[:18].decode('utf8') == 'Bitcraze Crazyflie':
             pk = CRTPPacket()
             pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND)
             pk.data = (VERSION_GET_PROTOCOL, )
             self._cf.send_packet(pk)
         else:
             self._protocolVersion = -1
             logger.info('Procotol version: {}'.format(
                 self.get_protocol_version()))
             self._callback()
Exemplo n.º 55
0
    def _new_packet_cb(self, packet):
        """Callback for newly arrived packets with TOC information"""
        chan = packet.channel
        cmd = packet.datal[0]
        payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])

        if (chan == CHAN_SETTINGS):
            new_block_id = ord(payload[0])
            error_status = ord(payload[1])
            if (cmd == CMD_CREATE_BLOCK):
                block = None
                for blocks in self.log_blocks:
                    if (blocks.block_id == new_block_id):
                        block = blocks
                if (block is not None):
                    if (error_status == 0):  # No error
                        logger.debug("Have successfully added block_id=%d",
                                     new_block_id)

                        pk = CRTPPacket()
                        pk.set_header(5, CHAN_SETTINGS)
                        pk.data = (CMD_START_LOGGING, new_block_id,
                                   block.period)
                        self.cf.send_packet(pk)
                    else:
                        logger.warning("Error when adding block_id=%d, should"
                                       " tell listenders...", new_block_id)

                else:
                    logger.warning("No LogEntry to assign block to !!!")
            if (cmd == CMD_START_LOGGING):
                if (error_status == 0x00):
                    logger.info("Have successfully logging for block=%d",
                                new_block_id)
                else:
                    logger.warning("Error=%d when starting logging for "
                                   "block=%d", error_status, new_block_id)
        if (chan == CHAN_LOGDATA):
            chan = packet.channel
            block_id = ord(packet.data[0])
            # timestamp = packet.data[0:4] # Not currently used
            logdata = packet.data[4:]
            block = None
            for blocks in self.log_blocks:
                if (blocks.block_id == block_id):
                    block = blocks
            if (block is not None):
                block.unpack_log_data(logdata)
            else:
                logger.warning("Error no LogEntry to handle block=%d", block_id)