Python Caller.call方法代码示例

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class PeriodicTimer:
    """Create a periodic timer that will periodicall call a callback"""
    def __init__(self, period, callback):
        self._callbacks = Caller()
        self._callbacks.add_callback(callback)
        self._started = False
        self._period = period
        self._timer = Timer(period, self._expired)
        self._timer.daemon = True

    def start(self):
        """Start the timer"""
        self._timer = Timer(self._period, self._expired)
        self._timer.daemon = True
        self._timer.start()
        self._started = True

    def stop(self):
        """Stop the timer"""
        self._timer.cancel()
        self._started = False

    def _expired(self):
        """Callback for the expired internal timer"""
        self._callbacks.call()
        if self._started:
            self.start() 

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Console:
    """
    Crazyflie console is used to receive characters printed using printf
    from the firmware.
    """

    def __init__(self, crazyflie):
        """
        Initialize the console and register it to receive data from the copter.
        """
        self.receivedChar = Caller()

        self.cf = crazyflie
        self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming)

    def incoming(self, packet):
        """
        Callback for data received from the copter.
        """
        # This might be done prettier ;-)
        console_text = packet.data.decode("UTF-8")

        self.receivedChar.call(console_text)

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]

#.........这里部分代码省略.........
        self.emergency_stop_updated = Caller()
        self.device_discovery = Caller()
        self.device_error = Caller()
        self.assisted_control_updated = Caller()
        self.alt1_updated = Caller()
        self.alt2_updated = Caller()

        # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
        self.limiting_updated = Caller()

    def _get_device_from_name(self, device_name):
        """Get the raw device from a name"""
        for d in readers.devices():
            if d.name == device_name:
                return d
        return None

    def set_hover_max_height(self, height):
        self._hover_max_height = height

    def set_alt_hold_available(self, available):
        """Set if altitude hold is available or not (depending on HW)"""
        self.has_pressure_sensor = available

    def _do_device_discovery(self):
        devs = self.available_devices()

        # This is done so that devs can easily get access
        # to limits without creating lots of extra code
        for d in devs:
            d.input = self

        if len(devs):
            self.device_discovery.call(devs)
            self._discovery_timer.stop()

    def available_mux(self):
        return self._mux

    def set_mux(self, name=None, mux=None):
        old_mux = self._selected_mux
        if name:
            for m in self._mux:
                if m.name == name:
                    self._selected_mux = m
        elif mux:
            self._selected_mux = mux

        old_mux.close()

        logger.info("Selected MUX: {}".format(self._selected_mux.name))

    def set_assisted_control(self, mode):
        self._assisted_control = mode

    def get_assisted_control(self):
        return self._assisted_control

    def available_devices(self):
        """List all available and approved input devices.
        This function will filter available devices by using the
        blacklist configuration and only return approved devices."""
        devs = readers.devices()
        devs += interfaces.devices()
        approved_devs = []

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Param():
    """
    Used to read and write parameter values in the Crazyflie.
    """

    def __init__(self, crazyflie):
        self.toc = Toc()

        self.cf = crazyflie
        self.param_update_callbacks = {}
        self.group_update_callbacks = {}
        self.all_update_callback = Caller()
        self.param_updater = None

        self.param_updater = _ParamUpdater(self.cf, self._param_updated)
        self.param_updater.start()

        self.cf.disconnected.add_callback(self._disconnected)

        self.all_updated = Caller()
        self.is_updated = False

        self.values = {}

    def request_update_of_all_params(self):
        """Request an update of all the parameters in the TOC"""
        for group in self.toc.toc:
            for name in self.toc.toc[group]:
                complete_name = '%s.%s' % (group, name)
                self.request_param_update(complete_name)

    def _check_if_all_updated(self):
        """Check if all parameters from the TOC has at least been fetched
        once"""
        for g in self.toc.toc:
            if g not in self.values:
                return False
            for n in self.toc.toc[g]:
                if n not in self.values[g]:
                    return False

        return True

    def _param_updated(self, pk):
        """Callback with data for an updated parameter"""
        var_id = pk.data[0]
        element = self.toc.get_element_by_id(var_id)
        if element:
            s = struct.unpack(element.pytype, pk.data[1:])[0]
            s = s.__str__()
            complete_name = '%s.%s' % (element.group, element.name)

            # Save the value for synchronous access
            if element.group not in self.values:
                self.values[element.group] = {}
            self.values[element.group][element.name] = s

            logger.debug('Updated parameter [%s]' % complete_name)
            if complete_name in self.param_update_callbacks:
                self.param_update_callbacks[complete_name].call(
                    complete_name, s)
            if element.group in self.group_update_callbacks:
                self.group_update_callbacks[element.group].call(
                    complete_name, s)
            self.all_update_callback.call(complete_name, s)

            # Once all the parameters are updated call the
            # callback for "everything updated" (after all the param
            # updated callbacks)
            if self._check_if_all_updated() and not self.is_updated:
                self.is_updated = True
                self.all_updated.call()
        else:
            logger.debug('Variable id [%d] not found in TOC', var_id)

    def remove_update_callback(self, group, name=None, cb=None):
        """Remove the supplied callback for a group or a group.name"""
        if not cb:
            return

        if not name:
            if group in self.group_update_callbacks:
                self.group_update_callbacks[group].remove_callback(cb)
        else:
            paramname = '{}.{}'.format(group, name)
            if paramname in self.param_update_callbacks:
                self.param_update_callbacks[paramname].remove_callback(cb)

    def add_update_callback(self, group=None, name=None, cb=None):
        """
        Add a callback for a specific parameter name. This callback will be
        executed when a new value is read from the Crazyflie.
        """
        if not group and not name:
            self.all_update_callback.add_callback(cb)
        elif not name:
            if group not in self.group_update_callbacks:
                self.group_update_callbacks[group] = Caller()
            self.group_update_callbacks[group].add_callback(cb)
        else:
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Crazyflie():
    """The Crazyflie class"""

    def __init__(self, link=None, ro_cache=None, rw_cache=None):
        """
        Create the objects from this module and register callbacks.

        ro_cache -- Path to read-only cache (string)
        rw_cache -- Path to read-write cache (string)
        """

        # Called on disconnect, no matter the reason
        self.disconnected = Caller()
        # Called on unintentional disconnect only
        self.connection_lost = Caller()
        # Called when the first packet in a new link is received
        self.link_established = Caller()
        # Called when the user requests a connection
        self.connection_requested = Caller()
        # Called when the link is established and the TOCs (that are not
        # cached) have been downloaded
        self.connected = Caller()
        # Called if establishing of the link fails (i.e times out)
        self.connection_failed = Caller()
        # Called for every packet received
        self.packet_received = Caller()
        # Called for every packet sent
        self.packet_sent = Caller()
        # Called when the link driver updates the link quality measurement
        self.link_quality_updated = Caller()

        self.state = State.DISCONNECTED

        self.link = link
        self._toc_cache = TocCache(ro_cache=ro_cache,
                                   rw_cache=rw_cache)

        self.incoming = _IncomingPacketHandler(self)
        self.incoming.setDaemon(True)
        self.incoming.start()

        self.commander = Commander(self)
        self.loc = Localization(self)
        self.extpos = Extpos(self)
        self.log = Log(self)
        self.console = Console(self)
        self.param = Param(self)
        self.mem = Memory(self)
        self.platform = PlatformService(self)

        self.link_uri = ''

        # Used for retry when no reply was sent back
        self.packet_received.add_callback(self._check_for_initial_packet_cb)
        self.packet_received.add_callback(self._check_for_answers)

        self._answer_patterns = {}

        self._send_lock = Lock()

        self.connected_ts = None

        # Connect callbacks to logger
        self.disconnected.add_callback(
            lambda uri: logger.info('Callback->Disconnected from [%s]', uri))
        self.disconnected.add_callback(self._disconnected)
        self.link_established.add_callback(
            lambda uri: logger.info('Callback->Connected to [%s]', uri))
        self.connection_lost.add_callback(
            lambda uri, errmsg: logger.info(
                'Callback->Connection lost to [%s]: %s', uri, errmsg))
        self.connection_failed.add_callback(
            lambda uri, errmsg: logger.info(
                'Callback->Connected failed to [%s]: %s', uri, errmsg))
        self.connection_requested.add_callback(
            lambda uri: logger.info(
                'Callback->Connection initialized[%s]', uri))
        self.connected.add_callback(
            lambda uri: logger.info(
                'Callback->Connection setup finished [%s]', uri))

    def _disconnected(self, link_uri):
        """ Callback when disconnected."""
        self.connected_ts = None

    def _start_connection_setup(self):
        """Start the connection setup by refreshing the TOCs"""
        logger.info('We are connected[%s], request connection setup',
                    self.link_uri)
        self.log.refresh_toc(self._log_toc_updated_cb, self._toc_cache)

    def _param_toc_updated_cb(self):
        """Called when the param TOC has been fully updated"""
        logger.info('Param TOC finished updating')
        self.connected_ts = datetime.datetime.now()
        self.connected.call(self.link_uri)
        # Trigger the update for all the parameters
        self.param.request_update_of_all_params()

    def _mems_updated_cb(self):
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Log():
    """Create log configuration"""

    # These codes can be decoded using os.stderror, but
    # some of the text messages will look very stange
    # in the UI, so they are redefined here
    _err_codes = {
            errno.ENOMEM: "No more memory available",
            errno.ENOEXEC: "Command not found",
            errno.ENOENT: "No such block id",
            errno.E2BIG: "Block too large"
            }

    def __init__(self, crazyflie=None):
        self.log_blocks = []
        # Called with newly created blocks
        self.block_added_cb = Caller()

        self.cf = crazyflie
        self.toc = None
        self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)

        self.toc_updated = Caller()
        self.state = IDLE
        self.fake_toc_crc = 0xDEADBEEF

    def add_config(self, logconf):
        """Add a log configuration to the logging framework.

        When doing this the contents of the log configuration will be validated
        and listeners for new log configurations will be notified. When
        validating the configuration the variables are checked against the TOC
        to see that they actually exist. If they don't then the configuration
        cannot be used. Since a valid TOC is required, a Crazyflie has to be
        connected when calling this method, otherwise it will fail."""

        if not self.cf.link:
            logger.error("Cannot add configs without being connected to a "
                         "Crazyflie!")
            return

        # If the log configuration contains variables that we added without
        # type (i.e we want the stored as type for fetching as well) then
        # resolve this now and add them to the block again.
        for name in logconf.default_fetch_as:
            var = self.toc.get_element_by_complete_name(name)
            if not var:
                logger.warning("%s not in TOC, this block cannot be"
                               " used!", name)
                logconf.valid = False
                return
            # Now that we know what type this variable has, add it to the log
            # config again with the correct type
            logconf.add_variable(name, var.ctype)

        # Now check that all the added variables are in the TOC and that
        # the total size constraint of a data packet with logging data is
        # not
        size = 0
        for var in logconf.variables:
            size += LogTocElement.get_size_from_id(var.fetch_as)
            # Check that we are able to find the variable in the TOC so
            # we can return error already now and not when the config is sent
            if var.is_toc_variable():
                if (self.toc.get_element_by_complete_name(
                        var.name) is None):
                    logger.warning("Log: %s not in TOC, this block cannot be"
                                   " used!", var.name)
                    logconf.valid = False
                    return

        if (size <= MAX_LOG_DATA_PACKET_SIZE and
                (logconf.period > 0 and logconf.period < 0xFF)):
            logconf.valid = True
            logconf.cf = self.cf
            self.log_blocks.append(logconf)
            self.block_added_cb.call(logconf)
        else:
            logconf.valid = False

    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()

    def _find_block(self, id):
        for block in self.log_blocks:
            if block.id == id:
                return block
        return None

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

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class LogConfig(object):
    """Representation of one log configuration that enables logging
    from the Crazyflie"""
    _config_id_counter = 1

    def __init__(self, name, period_in_ms):
        """Initialize the entry"""
        self.data_received_cb = Caller()
        self.error_cb = Caller()
        self.started_cb = Caller()
        self.added_cb = Caller()
        self.err_no = 0

        self.id = LogConfig._config_id_counter
        LogConfig._config_id_counter += 1 % 255
        self.cf = None
        self.period = period_in_ms / 10
        self.period_in_ms = period_in_ms
        self._added = False
        self._started = False
        self.valid = False
        self.variables = []
        self.default_fetch_as = []
        self.name = name

    def add_variable(self, name, fetch_as=None):
        """Add a new variable to the configuration.

        name - Complete name of the variable in the form group.name
        fetch_as - String representation of the type the variable should be
                   fetched as (i.e uint8_t, float, FP16, etc)

        If no fetch_as type is supplied, then the stored as type will be used
        (i.e the type of the fetched variable is the same as it's stored in the
        Crazyflie)."""
        if fetch_as:
            self.variables.append(LogVariable(name, fetch_as))
        else:
            # We cannot determine the default type until we have connected. So
            # save the name and we will add these once we are connected.
            self.default_fetch_as.append(name)

    def add_memory(self, name, fetch_as, stored_as, address):
        """Add a raw memory position to log.

        name - Arbitrary name of the variable
        fetch_as - String representation of the type of the data the memory
                   should be fetch as (i.e uint8_t, float, FP16)
        stored_as - String representation of the type the data is stored as
                    in the Crazyflie
        address - The address of the data
        """
        self.variables.append(LogVariable(name, fetch_as, LogVariable.MEM_TYPE,
                                          stored_as, address))

    def _set_added(self, added):
        self._added = added
        self.added_cb.call(added)

    def _get_added(self):
        return self._added

    def _set_started(self, started):
        self._started = started
        self.started_cb.call(started)

    def _get_started(self):
        return self._started

    added = property(_get_added, _set_added)
    started = property(_get_started, _set_started)

    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)
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Log():
    """Create log configuration"""

    # These codes can be decoded using os.stderror, but
    # some of the text messages will look very stange
    # in the UI, so they are redefined here
    _err_codes = {
            errno.ENOMEM: "No more memory available",
            errno.ENOEXEC: "Command not found",
            errno.ENOENT: "No such block id",
            errno.E2BIG: "Block too large"
            }

    def __init__(self, crazyflie=None):
        self.log_blocks = []
        # Called with newly created blocks
        self.block_added_cb = Caller()

        self.cf = crazyflie
        self.toc = None
        self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)

        self.toc_updated = Caller()
        self.state = IDLE
        self.fake_toc_crc = 0xDEADBEEF

    def create_log_packet(self, logconf):
        """Create a new log configuration"""
        size = 0
        period = logconf.getPeriod() / 10
        for var in logconf.getVariables():
            size += LogTocElement.get_size_from_id(var.getFetchAs())
            # Check that we are able to find the variable in the TOC so
            # we can return error already now and not when the config is sent
            if (var.isTocVariable()):
                if (self.toc.get_element_by_complete_name(
                        var.getName()) is None):
                    logger.warning("Log: %s not in TOC, this block cannot be"
                                   " used!", var.getName())
                    return None
        if (size <= MAX_LOG_DATA_PACKET_SIZE and period > 0 and period < 0xFF):
            block = LogEntry(self.cf, logconf)
            self.log_blocks.append(block)
            self.block_added_cb.call(block)
            return block
        else:
            return None

    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)

        self.log_blocks = []

        self.toc = Toc()
        toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
                                self.toc, refresh_done_callback, toc_cache)
        toc_fetcher.start()

    def _find_block(self, block_id):
        for block in self.log_blocks:
            if block.block_id == block_id:
                return block
        return None

    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 !!!")
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Memory:
    """Access memories on the Crazyflie"""

    # These codes can be decoded using os.stderror, but
    # some of the text messages will look very stange
    # in the UI, so they are redefined here
    _err_codes = {
        errno.ENOMEM: "No more memory available",
        errno.ENOEXEC: "Command not found",
        errno.ENOENT: "No such block id",
        errno.E2BIG: "Block too large",
        errno.EEXIST: "Block already exists",
    }

    def __init__(self, crazyflie=None):
        """Instantiate class and connect callbacks"""
        self.mems = []
        # Called when new memories have been added
        self.mem_added_cb = Caller()
        # Called when new data has been read
        self.mem_read_cb = Caller()

        self.mem_write_cb = Caller()

        self.cf = crazyflie
        self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb)

        self._refresh_callback = None
        self._fetch_id = 0
        self.nbr_of_mems = 0
        self._ow_mem_fetch_index = 0
        self._elem_data = ()
        self._read_requests = {}
        self._write_requests = {}
        self._ow_mems_left_to_update = []

        self._getting_count = False

    def _mem_update_done(self, mem):
        """Callback from each individual memory (only 1-wire) when reading of header/elements are done"""
        if mem.id in self._ow_mems_left_to_update:
            self._ow_mems_left_to_update.remove(mem.id)

        logger.info(mem)

        if len(self._ow_mems_left_to_update) == 0:
            if self._refresh_callback:
                self._refresh_callback()
                self._refresh_callback = None

    def get_mem(self, id):
        """Fetch the memory with the supplied id"""
        for m in self.mems:
            if m.id == id:
                return m

        return None

    def get_mems(self, type):
        """Fetch all the memories of the supplied type"""
        ret = ()
        for m in self.mems:
            if m.type == type:
                ret += (m,)

        return ret

    def write(self, memory, addr, data):
        """Write the specified data to the given memory at the given address"""
        if memory.id in self._write_requests:
            logger.warning("There is already a write operation ongoing for memory id {}".format(memory.id))
            return False

        wreq = _WriteRequest(memory, addr, data, self.cf)
        self._write_requests[memory.id] = wreq

        wreq.start()

        return True

    def read(self, memory, addr, length):
        """Read the specified amount of bytes from the given memory at the given address"""
        if memory.id in self._read_requests:
            logger.warning("There is already a read operation ongoing for memory id {}".format(memory.id))
            return False

        rreq = _ReadRequest(memory, addr, length, self.cf)
        self._read_requests[memory.id] = rreq

        rreq.start()

        return True

    def refresh(self, refresh_done_callback):
        """Start fetching all the detected memories"""
        self._refresh_callback = refresh_done_callback
        self._fetch_id = 0
        for m in self.mems:
            try:
                self.mem_read_cb.remove_callback(m.new_data)
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]

#.........这里部分代码省略.........
        self.device_discovery = Caller()
        self.device_error = Caller()
        self.althold_updated = Caller()
        self.alt1_updated = Caller()
        self.alt2_updated = Caller()

        # Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
        self.limiting_updated = Caller()

    def _get_device_from_name(self, device_name):
        """Get the raw device from a name"""
        for d in readers.devices():
            if d.name == device_name:
                return d
        return None

    def set_alt_hold_available(self, available):
        """Set if altitude hold is available or not (depending on HW)"""
        self.has_pressure_sensor = available

    def enable_alt_hold(self, althold):
        """Enable or disable altitude hold"""
        self._old_alt_hold = althold

    def _do_device_discovery(self):
        devs = self.available_devices()

        # This is done so that devs can easily get access
        # to limits without creating lots of extra code
        for d in devs:
            d.input = self

        if len(devs):
            self.device_discovery.call(devs)
            self._discovery_timer.stop()

    def available_mux(self):
        return self._mux

    def set_mux(self, name=None, mux=None):
        old_mux = self._selected_mux
        if name:
            for m in self._mux:
                if m.name == name:
                    self._selected_mux = m
        elif mux:
            self._selected_mux = mux

        old_mux.close()

        logger.info("Selected MUX: {}".format(self._selected_mux.name))

    def available_devices(self):
        """List all available and approved input devices.
        This function will filter available devices by using the
        blacklist configuration and only return approved devices."""
        devs = readers.devices()
        devs += interfaces.devices()
        approved_devs = []

        for dev in devs:
            if ((not self._dev_blacklist) or
                    (self._dev_blacklist and
                     not self._dev_blacklist.match(dev.name))):
                dev.input = self
                approved_devs.append(dev)

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class JoystickReader:
    """
    Thread that will read input from devices/joysticks and send control-set
    ponts to the Crazyflie
    """
    inputConfig = []

    def __init__(self, do_device_discovery=True):
        # TODO: Should be OS dependant
        self.inputdevice = PyGameReader()

        self.maxRPAngle = 0
        self.thrustDownSlew = 0
        self.thrustSlewEnabled = False
        self.slewEnableLimit = 0
        self.maxYawRate = 0
        self.detectAxis = False
        self.emergencyStop = False

        self.oldThrust = 0

        self._trim_roll = Config().get("trim_roll")
        self._trim_pitch = Config().get("trim_pitch")

        # TODO: The polling interval should be set from config file
        self.readTimer = PeriodicTimer(0.01, self.readInput)

        if do_device_discovery:
            self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery)
            self._discovery_timer.start()

        self._available_devices = {}

        # Check if user config exists, otherwise copy files
        if not os.path.isdir(ConfigManager().configs_dir):
            logger.info("No user config found, copying dist files")
            os.makedirs(ConfigManager().configs_dir)
            for f in glob.glob(sys.path[0] +
                               "/cfclient/configs/input/[A-Za-z]*.json"):
                shutil.copy2(f, ConfigManager().configs_dir)

        ConfigManager().get_list_of_configs()

        self.input_updated = Caller()
        self.rp_trim_updated = Caller()
        self.emergency_stop_updated = Caller()
        self.device_discovery = Caller()
        self.device_error = Caller()

    def _do_device_discovery(self):
        devs = self.getAvailableDevices()

        if len(devs):
            self.device_discovery.call(devs)
            self._discovery_timer.stop()

    def getAvailableDevices(self):
        """List all available input devices."""
        devs = self.inputdevice.getAvailableDevices()

        for d in devs:
            self._available_devices[d["name"]] = d["id"]

        return devs

    def enableRawReading(self, deviceId):
        """
        Enable raw reading of the input device with id deviceId. This is used
        to get raw values for setting up of input devices. Values are read
        without using a mapping.
        """
        self.inputdevice.enableRawReading(deviceId)

    def disableRawReading(self):
        """Disable raw reading of input device."""
        self.inputdevice.disableRawReading()

    def readRawValues(self):
        """ Read raw values from the input device."""
        return self.inputdevice.readRawValues()

    def start_input(self, device_name, config_name):
        """
        Start reading input from the device with name device_name using config
        config_name
        """
        try:
            device_id = self._available_devices[device_name]
            self.inputdevice.start_input(
                                    device_id,
                                    ConfigManager().get_config(config_name))
            self.readTimer.start()
        except Exception:
            self.device_error.call(
                     "Error while opening/initializing  input device\n\n%s" %
                     (traceback.format_exc()))

    def stop_input(self):
        """Stop reading from the input device."""
        self.readTimer.stop()
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Log():
    """Create log configuration"""

    # These codes can be decoded using os.stderror, but
    # some of the text messages will look very strange
    # in the UI, so they are redefined here
    _err_codes = {
        errno.ENOMEM: 'No more memory available',
        errno.ENOEXEC: 'Command not found',
        errno.ENOENT: 'No such block id',
        errno.E2BIG: 'Block too large',
        errno.EEXIST: 'Block already exists'
    }

    def __init__(self, crazyflie=None):
        self.log_blocks = []
        # Called with newly created blocks
        self.block_added_cb = Caller()

        self.cf = crazyflie
        self.toc = None
        self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)

        self.toc_updated = Caller()
        self.state = IDLE
        self.fake_toc_crc = 0xDEADBEEF

        self._refresh_callback = None
        self._toc_cache = None

        self._config_id_counter = 1

    def add_config(self, logconf):
        """Add a log configuration to the logging framework.

        When doing this the contents of the log configuration will be validated
        and listeners for new log configurations will be notified. When
        validating the configuration the variables are checked against the TOC
        to see that they actually exist. If they don't then the configuration
        cannot be used. Since a valid TOC is required, a Crazyflie has to be
        connected when calling this method, otherwise it will fail."""

        if not self.cf.link:
            logger.error('Cannot add configs without being connected to a '
                         'Crazyflie!')
            return

        # If the log configuration contains variables that we added without
        # type (i.e we want the stored as type for fetching as well) then
        # resolve this now and add them to the block again.
        for name in logconf.default_fetch_as:
            var = self.toc.get_element_by_complete_name(name)
            if not var:
                logger.warning(
                    '%s not in TOC, this block cannot be used!', name)
                logconf.valid = False
                raise KeyError('Variable {} not in TOC'.format(name))
            # Now that we know what type this variable has, add it to the log
            # config again with the correct type
            logconf.add_variable(name, var.ctype)

        # Now check that all the added variables are in the TOC and that
        # the total size constraint of a data packet with logging data is
        # not
        size = 0
        for var in logconf.variables:
            size += LogTocElement.get_size_from_id(var.fetch_as)
            # Check that we are able to find the variable in the TOC so
            # we can return error already now and not when the config is sent
            if var.is_toc_variable():
                if (self.toc.get_element_by_complete_name(var.name) is None):
                    logger.warning(
                        'Log: %s not in TOC, this block cannot be used!',
                        var.name)
                    logconf.valid = False
                    raise KeyError('Variable {} not in TOC'.format(var.name))

        if (size <= MAX_LOG_DATA_PACKET_SIZE and
                (logconf.period > 0 and logconf.period < 0xFF)):
            logconf.valid = True
            logconf.cf = self.cf
            logconf.id = self._config_id_counter
            self._config_id_counter = (self._config_id_counter + 1) % 255
            self.log_blocks.append(logconf)
            self.block_added_cb.call(logconf)
        else:
            logconf.valid = False
            raise AttributeError(
                'The log configuration is too large or has an invalid '
                'parameter')

    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)
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class LogEntry(object):
    """Representation of one log configuration that enables logging
    from the Crazyflie"""
    block_idCounter = 1

    def __init__(self, crazyflie, logconf):
        """Initialize the entry"""
        self.data_received = Caller()
        self.error = Caller()
        self.started_cb = Caller()
        self.added_cb = Caller()
        self.err_no = 0

        self.logconf = logconf
        self.block_id = LogEntry.block_idCounter
        LogEntry.block_idCounter += 1 % 255
        self.cf = crazyflie
        self.period = logconf.getPeriod() / 10
        self.period_in_ms = logconf.getPeriod()
        self._added = False
        self._started = False

    def _set_added(self, added):
        self._added = added
        self.added_cb.call(added)

    def _get_added(self):
        return self._added

    def _set_started(self, started):
        self._started = started
        self.started_cb.call(started)

    def _get_started(self):
        return self._started

    added = property(_get_added, _set_added)
    started = property(_get_started, _set_started)

    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.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)

    def stop(self):
        """Stop the logging for this entry"""
        if (self.cf.link is not None):
            if (self.block_id is None):
                logger.warning("Stopping block, but no block registered")
            else:
                logger.debug("Sending stop logging for block %d", self.block_id)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_STOP_LOGGING, self.block_id)
                self.cf.send_packet(pk)

    def close(self):
        """Delete this entry in the Crazyflie"""
        if (self.cf.link is not None):
            if (self.block_id is None):
                logger.warning("Delete block, but no block registered")
            else:
                logger.debug("LogEntry: Sending delete logging for block %d"
                             % self.block_id)
                pk = CRTPPacket()
                pk.set_header(5, CHAN_SETTINGS)
                pk.data = (CMD_DELETE_BLOCK, self.block_id)
                self.cf.send_packet(pk)
                self.block_id = None  # Wait until we get confirmation of delete

    def unpack_log_data(self, log_data, timestamp):
        """Unpack received logging data so it represent real values according
#.........这里部分代码省略.........

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class Localization():
    """
    Handle localization-related data communication with the Crazyflie
    """

    # Implemented channels
    POSITION_CH = 0
    GENERIC_CH = 1

    # Location message types for generig channel
    RANGE_STREAM_REPORT = 0x00
    RANGE_STREAM_REPORT_FP16 = 0x01
    LPS_SHORT_LPP_PACKET = 0x02

    def __init__(self, crazyflie=None):
        """
        Initialize the Extpos object.
        """
        self._cf = crazyflie

        self.receivedLocationPacket = Caller()
        self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)

    def _incoming(self, packet):
        """
        Callback for data received from the copter.
        """
        if len(packet.data) < 1:
            logger.warning('Localization packet received with incorrect' +
                           'length (length is {})'.format(len(packet.data)))
            return

        pk_type = struct.unpack('<B', packet.data[:1])[0]
        data = packet.data[1:]

        # Decoding the known packet types
        # TODO: more generic decoding scheme?
        decoded_data = None
        if pk_type == self.RANGE_STREAM_REPORT:
            if len(data) % 5 != 0:
                logger.error('Wrong range stream report data lenght')
                return
            decoded_data = {}
            raw_data = data
            for i in range(int(len(data) / 5)):
                anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
                decoded_data[anchor_id] = distance
                raw_data = raw_data[5:]

        pk = LocalizationPacket(pk_type, data, decoded_data)
        self.receivedLocationPacket.call(pk)

    def send_extpos(self, pos):
        """
        Send the current Crazyflie X, Y, Z position. This is going to be
        forwarded to the Crazyflie's position estimator.
        """

        pk = CRTPPacket()
        pk.port = CRTPPort.LOCALIZATION
        pk.channel = self.POSITION_CH
        pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
        self._cf.send_packet(pk)

    def send_short_lpp_packet(self, dest_id, data):
        """
        Send ultra-wide-band LPP packet to dest_id
        """

        pk = CRTPPacket()
        pk.port = CRTPPort.LOCALIZATION
        pk.channel = self.GENERIC_CH
        pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
        self._cf.send_packet(pk)

# 需要导入模块: from cflib.utils.callbacks import Caller [as 别名]
# 或者: from cflib.utils.callbacks.Caller import call [as 别名]
class LogEntry:

    blockIdCounter = 1

    def __init__(self, crazyflie, logconf):
        self.dataReceived = Caller()
        self.error = Caller()

        self.logconf = logconf
        self.blockId = LogEntry.blockIdCounter
        LogEntry.blockIdCounter += 1
        self.cf = crazyflie
        self.period = logconf.getPeriod() / 10
        self.blockCreated = False

    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 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)

    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

    def unpack_log_data(self, logData):
        retData = {}
        dataIndex = 0
        for v in self.logconf.getVariables():
            size = LogTocElement.get_size_from_id(v.getFetchAs())
            name = v.getName()
            unpackstring = LogTocElement.get_unpack_string_from_id(
                v.getFetchAs())
            value = struct.unpack(unpackstring,
                                  logData[dataIndex:dataIndex + size])[0]
            dataIndex += size
            retData[name] = value
        self.dataReceived.call(retData)