Python crazyradio.Crazyradio类代码示例

    def get_status(self):
        cradio = Crazyradio()
        ver = cradio.version
        cradio.close()
        cradio = None

        return "Crazyradio version {}".format(ver)

    def scan_interface(self):
        """ Scan interface for Crazyflies """
        if self.cradio is None:
            try:
                self.cradio = Crazyradio()
            except Exception:
                return []
        else:
            raise Exception("Cannot scann for links while the link is open!")

        # FIXME: implements serial number in the Crazyradio driver!
        serial = "N/A"

        logger.info("v%s dongle with serial %s found", self.cradio.version,
                    serial)
        found = []

        self.cradio.set_arc(1)

        self.cradio.set_data_rate(self.cradio.DR_250KPS)
        found += map(lambda c: ["radio://0/{}/250K".format(c), ""],
                     self._scan_radio_channels())
        self.cradio.set_data_rate(self.cradio.DR_1MPS)
        found += map(lambda c: ["radio://0/{}/1M".format(c), ""],
                     self._scan_radio_channels())
        self.cradio.set_data_rate(self.cradio.DR_2MPS)
        found += map(lambda c: ["radio://0/{}/2M".format(c), ""],
                     self._scan_radio_channels())

        self.cradio.close()
        self.cradio = None

        return found

    def __init__(self, radio_id):
        threading.Thread.__init__(self)
        self.cradio = Crazyradio(devid=radio_id)
        self._num_profiles = 0

        self.rx_queues = []
        self._num_profiles = 0
        self.tx_queue = queue.Queue()
        self.sp = False

    def get_status(self):
        if self.cradio is None:
            try:
                self.cradio = Crazyradio()
            except USBError as e:
                return "Cannot open Crazyradio. Permission problem?"\
                       " ({})".format(str(e))
            except Exception as e:
                return str(e)

        return "Crazyradio version {}".format(self.cradio.version)

    def __init__(self, radio_id):
        threading.Thread.__init__(self)
        self.cradio = Crazyradio(devid=radio_id)
        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning("Radio version <0.4 will be obsoleted soon!")

        self._num_profiles = 0
        self.tx_queue = Queue.Queue()

        self.sp = False

    def scan_interface(self, address):
        """ Scan interface for Crazyflies """
        if self.cradio is None:
            try:
                self.cradio = Crazyradio()
            except Exception:
                return []
        else:
            raise Exception("Cannot scann for links while the link is open!")

        # FIXME: implements serial number in the Crazyradio driver!
        serial = "N/A"

        logger.info("v%s dongle with serial %s found", self.cradio.version,
                    serial)
        found = []

        if address is not None:
            addr = "{:X}".format(address)
            new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
            self.cradio.set_address(new_addr)

        self.cradio.set_arc(1)

        self.cradio.set_data_rate(self.cradio.DR_250KPS)

        if address is None or address == 0xE7E7E7E7E7:
            found += [["radio://0/{}/250K".format(c), ""]
                      for c in self._scan_radio_channels()]
            self.cradio.set_data_rate(self.cradio.DR_1MPS)
            found += [["radio://0/{}/1M".format(c), ""]
                      for c in self._scan_radio_channels()]
            self.cradio.set_data_rate(self.cradio.DR_2MPS)
            found += [["radio://0/{}/2M".format(c), ""]
                      for c in self._scan_radio_channels()]
        else:
            found += [["radio://0/{}/250K/{:X}".format(c, address), ""]
                      for c in self._scan_radio_channels()]
            self.cradio.set_data_rate(self.cradio.DR_1MPS)
            found += [["radio://0/{}/1M/{:X}".format(c, address), ""]
                      for c in self._scan_radio_channels()]
            self.cradio.set_data_rate(self.cradio.DR_2MPS)
            found += [["radio://0/{}/2M/{:X}".format(c, address), ""]
                      for c in self._scan_radio_channels()]

        self.cradio.close()
        self.cradio = None

        return found

    def get_status(self):
        if self.cradio is None:
            try:
                self.cradio = Crazyradio()
            except USBError as e:
                return 'Cannot open Crazyradio. Permission problem?' \
                       ' ({})'.format(str(e))
            except Exception as e:
                return str(e)

        ver = self.cradio.version
        self.cradio.close()
        self.cradio = None

        return 'Crazyradio version {}'.format(ver)

    def scan_interface(self):
        """ Scan interface for Crazyflies """
        found = []
        cont = 0
        device_num = 0
        busses = usb.busses()
        for bus in busses:
            for device in bus.devices:
                if device.idVendor == 0x1915:
                    if device.idProduct == 0x7777:
                        device_num += 1

        logger.info("Numero di antenne trovate: %d", device_num)

        while cont < device_num:
            if self.cradio is None:
                try:
                    self.cradio = Crazyradio(devid=cont)
                except Exception:
                    logger.info("L'antenna n: %d e' momentaneamente non disponibile.", cont)
                    cont += 1
                    self.cradio = None
                    continue
            else:
                raise Exception("Cannot scann for links while the link is open!")

            self.cradio.set_arc(1)

            self.cradio.set_data_rate(self.cradio.DR_250KPS)
            found += map(lambda c: [("radio://"+unicode(cont)+"/{}/250K").format(c), ""], self._scan_radio_channels())
            self.cradio.set_data_rate(self.cradio.DR_1MPS)
            found += map(lambda c: ["radio://"+unicode(cont)+"/{}/1M".format(c), ""], self._scan_radio_channels())
            self.cradio.set_data_rate(self.cradio.DR_2MPS)
            found += map(lambda c: ["radio://"+unicode(cont)+"/{}/2M".format(c), ""], self._scan_radio_channels())

            cont += 1
            self.cradio.close()
            self.cradio = None

        return found

    def connect(self, uri, link_quality_callback, link_error_callback):
        """
        Connect the link driver to a specified URI of the format:
        radio://<dongle nbr>/<radio channel>/[250K,1M,2M]

        The callback for linkQuality can be called at any moment from the
        driver to report back the link quality in percentage. The
        callback from linkError will be called when a error occues with
        an error message.
        """

        # check if the URI is a radio URI
        if not re.search("^radio://", uri):
            raise WrongUriType("Not a radio URI")

        # Open the USB dongle
        if not re.search("^radio://([0-9]+)((/([0-9]+))(/(250K|1M|2M))?)?$",
                         uri):
            raise WrongUriType('Wrong radio URI format!')

        uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
                             "(/(250K|1M|2M))?)?$",
                             uri)

        self.uri = uri

        channel = 2
        if uri_data.group(4):
            channel = int(uri_data.group(4))

        datarate = Crazyradio.DR_2MPS
        if uri_data.group(6) == "250K":
            datarate = Crazyradio.DR_250KPS
        if uri_data.group(6) == "1M":
            datarate = Crazyradio.DR_1MPS
        if uri_data.group(6) == "2M":
            datarate = Crazyradio.DR_2MPS

        if self.cradio is None:
            self.cradio = Crazyradio(devid=int(uri_data.group(1)))
        else:
            raise Exception("Link already open!")

        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning("Radio version <0.4 will be obsoleted soon!")

        self.cradio.set_channel(channel)

        self.cradio.set_data_rate(datarate)

        # Prepare the inter-thread communication queue
        self.in_queue = Queue.Queue()
        # Limited size out queue to avoid "ReadBack" effect
        self.out_queue = Queue.Queue(50)

        # Launch the comm thread
        self._thread = _RadioDriverThread(self.cradio, self.in_queue,
                                          self.out_queue,
                                          link_quality_callback,
                                          link_error_callback)
        self._thread.start()

        self.link_error_callback = link_error_callback

class RadioDriver(CRTPDriver):
    """ Crazyradio link driver """
    def __init__(self):
        """ Create the link driver """
        CRTPDriver.__init__(self)
        self.cradio = None
        self.uri = ""
        self.link_error_callback = None
        self.link_quality_callback = None
        self.in_queue = None
        self.out_queue = None
        self._thread = None

    def connect(self, uri, link_quality_callback, link_error_callback):
        """
        Connect the link driver to a specified URI of the format:
        radio://<dongle nbr>/<radio channel>/[250K,1M,2M]

        The callback for linkQuality can be called at any moment from the
        driver to report back the link quality in percentage. The
        callback from linkError will be called when a error occues with
        an error message.
        """

        # check if the URI is a radio URI
        if not re.search("^radio://", uri):
            raise WrongUriType("Not a radio URI")

        # Open the USB dongle
        if not re.search("^radio://([0-9]+)((/([0-9]+))(/(250K|1M|2M))?)?$",
                         uri):
            raise WrongUriType('Wrong radio URI format!')

        uri_data = re.search("^radio://([0-9]+)((/([0-9]+))"
                             "(/(250K|1M|2M))?)?$",
                             uri)

        self.uri = uri

        channel = 2
        if uri_data.group(4):
            channel = int(uri_data.group(4))

        datarate = Crazyradio.DR_2MPS
        if uri_data.group(6) == "250K":
            datarate = Crazyradio.DR_250KPS
        if uri_data.group(6) == "1M":
            datarate = Crazyradio.DR_1MPS
        if uri_data.group(6) == "2M":
            datarate = Crazyradio.DR_2MPS

        if self.cradio is None:
            self.cradio = Crazyradio(devid=int(uri_data.group(1)))
        else:
            raise Exception("Link already open!")

        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning("Radio version <0.4 will be obsoleted soon!")

        self.cradio.set_channel(channel)

        self.cradio.set_data_rate(datarate)

        # Prepare the inter-thread communication queue
        self.in_queue = Queue.Queue()
        # Limited size out queue to avoid "ReadBack" effect
        self.out_queue = Queue.Queue(50)

        # Launch the comm thread
        self._thread = _RadioDriverThread(self.cradio, self.in_queue,
                                          self.out_queue,
                                          link_quality_callback,
                                          link_error_callback)
        self._thread.start()

        self.link_error_callback = link_error_callback

    def receive_packet(self, time=0):
        """
        Receive a packet though the link. This call is blocking but will
        timeout and return None if a timeout is supplied.
        """
        if time == 0:
            try:
                return self.in_queue.get(False)
            except Queue.Empty:
                return None
        elif time < 0:
            try:
                return self.in_queue.get(True)
            except Queue.Empty:
                return None
        else:
            try:
                return self.in_queue.get(True, time)
            except Queue.Empty:
                return None

#.........这里部分代码省略.........

class _RadioTransferThread(threading.Thread):
    """ Thread that handles transfer for a single crazyradio hardware
    Can handles transfers form more than one radio profile (ie. link to a copter)
    """

    def __init__(self, radio_id):
        threading.Thread.__init__(self)
        self.cradio = Crazyradio(devid=radio_id)
        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning("Radio version <0.4 will be obsoleted soon!")

        self._num_profiles = 0
        self.tx_queue = Queue.Queue()

        self.sp = False

    def add_profile(self):
        self._num_profiles += 1
        rx_queue = Queue.Queue()
        return rx_queue

    def remove_profile(self, handle):
        # we don't need to to anything, the python garbage collector will take care of it
        self._num_profiles -= 1

    def num_profiles(self):
        return self._num_profiles

    def send_packet(self, profile, data):
        self.tx_queue.put([profile, data])
        return profile.handle.get()

    def stop(self):
        self.sp = True
        self.tx_queue.put([None, None])
        self.join()

    def run(self):
        #Simply service transfers requests
        while not self.sp:
            tx = self.tx_queue.get()
            if self.sp:
                break
            ack = self._send_packet(tx[0], tx[1])
            tx[0].handle.put(ack)

        # Close the USB dongle
        try:
            if self.cradio:
                self.cradio.close()
                print("Closed radio")
        except:
            # If we pull out the dongle we will not make this call
            pass
        self.cradio = None

    def _send_packet(self, profile, data):
        """
        Send packet making sure the radio is configured for the
        right transfers profile
        """
        assert isinstance(profile, _RadioProfile)
        if self.cradio.channel != profile.channel:
            self.cradio.set_channel(profile.channel)
        if self.cradio.data_rate != profile.rate:
            self.cradio.set_data_rate(profile.rate)
        if self.cradio.address != profile.address:
            self.cradio.set_address(profile.address)
        return self.cradio.send_packet(data)

    def scan_interface(self, address):
        """ Scan interface for Crazyflies """
        # This will cause an exception if not successful
        cradio = Crazyradio()

        # FIXME: implements serial number in the Crazyradio driver!
        serial = "N/A"

        logger.info("v%s dongle with serial %s found", cradio.version,
                    serial)
        found = []

        if address != None:
            addr = "{:X}".format(address)
            new_addr = struct.unpack("<BBBBB", binascii.unhexlify(addr))
            cradio.set_address(new_addr)

        cradio.set_arc(1)

        cradio.set_data_rate(cradio.DR_250KPS)

        if address == None or address == 0xE7E7E7E7E7:
            found += map(lambda c: ["radio://0/{}/250K".format(c), ""],
                         self._scan_radio_channels(cradio))
            cradio.set_data_rate(cradio.DR_1MPS)
            found += map(lambda c: ["radio://0/{}/1M".format(c), ""],
                         self._scan_radio_channels(cradio))
            cradio.set_data_rate(cradio.DR_2MPS)
            found += map(lambda c: ["radio://0/{}/2M".format(c), ""],
                         self._scan_radio_channels(cradio))
        else:
            found += map(lambda c: ["radio://0/{}/250K/{:X}".format(c, address), ""],
                         self._scan_radio_channels(cradio))
            cradio.set_data_rate(cradio.DR_1MPS)
            found += map(lambda c: ["radio://0/{}/1M/{:X}".format(c, address), ""],
                         self._scan_radio_channels(cradio))
            cradio.set_data_rate(cradio.DR_2MPS)
            found += map(lambda c: ["radio://0/{}/2M/{:X}".format(c, address), ""],
                         self._scan_radio_channels(cradio))

        cradio.close()
        cradio = None

        return found

    def connect(self, uri, link_quality_callback, link_error_callback):
        """
        Connect the link driver to a specified URI of the format:
        radio://<dongle nbr>/<radio channel>/[250K,1M,2M]

        The callback for linkQuality can be called at any moment from the
        driver to report back the link quality in percentage. The
        callback from linkError will be called when a error occurs with
        an error message.
        """

        # check if the URI is a radio URI
        if not re.search('^radio://', uri):
            raise WrongUriType('Not a radio URI')

        # Open the USB dongle
        if not re.search('^radio://([0-9]+)((/([0-9]+))'
                         '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri):
            raise WrongUriType('Wrong radio URI format!')

        uri_data = re.search('^radio://([0-9]+)((/([0-9]+))'
                             '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri)

        self.uri = uri

        channel = 2
        if uri_data.group(4):
            channel = int(uri_data.group(4))

        datarate = Crazyradio.DR_2MPS
        if uri_data.group(7) == '250K':
            datarate = Crazyradio.DR_250KPS
        if uri_data.group(7) == '1M':
            datarate = Crazyradio.DR_1MPS
        if uri_data.group(7) == '2M':
            datarate = Crazyradio.DR_2MPS

        if self.cradio is None:
            self.cradio = Crazyradio(devid=int(uri_data.group(1)))
        else:
            raise Exception('Link already open!')

        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning('Radio version <0.4 will be obsoleted soon!')

        self.cradio.set_channel(channel)

        self.cradio.set_data_rate(datarate)

        if uri_data.group(9):
            addr = str(uri_data.group(9))
            new_addr = struct.unpack('<BBBBB', binascii.unhexlify(addr))
            self.cradio.set_address(new_addr)

        # Prepare the inter-thread communication queue
        self.in_queue = queue.Queue()
        # Limited size out queue to avoid "ReadBack" effect
        self.out_queue = queue.Queue(1)

        # Launch the comm thread
        self._thread = _RadioDriverThread(self.cradio, self.in_queue,
                                          self.out_queue,
                                          link_quality_callback,
                                          link_error_callback,
                                          self)
        self._thread.start()

        self.link_error_callback = link_error_callback

class RadioDriver(CRTPDriver):
    """ Crazyradio link driver """

    def __init__(self):
        """ Create the link driver """
        CRTPDriver.__init__(self)
        self.cradio = None
        self.uri = ''
        self.link_error_callback = None
        self.link_quality_callback = None
        self.in_queue = None
        self.out_queue = None
        self._thread = None
        self.needs_resending = True

    def connect(self, uri, link_quality_callback, link_error_callback):
        """
        Connect the link driver to a specified URI of the format:
        radio://<dongle nbr>/<radio channel>/[250K,1M,2M]

        The callback for linkQuality can be called at any moment from the
        driver to report back the link quality in percentage. The
        callback from linkError will be called when a error occurs with
        an error message.
        """

        # check if the URI is a radio URI
        if not re.search('^radio://', uri):
            raise WrongUriType('Not a radio URI')

        # Open the USB dongle
        if not re.search('^radio://([0-9]+)((/([0-9]+))'
                         '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri):
            raise WrongUriType('Wrong radio URI format!')

        uri_data = re.search('^radio://([0-9]+)((/([0-9]+))'
                             '((/(250K|1M|2M))?(/([A-F0-9]+))?)?)?$', uri)

        self.uri = uri

        channel = 2
        if uri_data.group(4):
            channel = int(uri_data.group(4))

        datarate = Crazyradio.DR_2MPS
        if uri_data.group(7) == '250K':
            datarate = Crazyradio.DR_250KPS
        if uri_data.group(7) == '1M':
            datarate = Crazyradio.DR_1MPS
        if uri_data.group(7) == '2M':
            datarate = Crazyradio.DR_2MPS

        if self.cradio is None:
            self.cradio = Crazyradio(devid=int(uri_data.group(1)))
        else:
            raise Exception('Link already open!')

        if self.cradio.version >= 0.4:
            self.cradio.set_arc(10)
        else:
            logger.warning('Radio version <0.4 will be obsoleted soon!')

        self.cradio.set_channel(channel)

        self.cradio.set_data_rate(datarate)

        if uri_data.group(9):
            addr = str(uri_data.group(9))
            new_addr = struct.unpack('<BBBBB', binascii.unhexlify(addr))
            self.cradio.set_address(new_addr)

        # Prepare the inter-thread communication queue
        self.in_queue = queue.Queue()
        # Limited size out queue to avoid "ReadBack" effect
        self.out_queue = queue.Queue(1)

        # Launch the comm thread
        self._thread = _RadioDriverThread(self.cradio, self.in_queue,
                                          self.out_queue,
                                          link_quality_callback,
                                          link_error_callback,
                                          self)
        self._thread.start()

        self.link_error_callback = link_error_callback

    def receive_packet(self, time=0):
        """
        Receive a packet though the link. This call is blocking but will
        timeout and return None if a timeout is supplied.
        """
        if time == 0:
            try:
                return self.in_queue.get(False)
            except queue.Empty:
                return None
        elif time < 0:
            try:
                return self.in_queue.get(True)
            except queue.Empty:
#.........这里部分代码省略.........