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()
class PeriodicTimer: """Create a periodic timer that will periodically call a callback""" def __init__(self, period, callback): self._callbacks = Caller() self._callbacks.add_callback(callback) self._started = False self._period = period self._thread = None def start(self): """Start the timer""" self._thread = _PeriodicTimerThread(self._period, self._callbacks) self._thread.setDaemon(True) self._thread.start() def stop(self): """Stop the timer""" if self._thread: self._thread.stop() self._thread = None
class PeriodicTimer: """Create a periodic timer that will periodically call a callback""" def __init__(self, period, callback): self._callbacks = Caller() self._callbacks.add_callback(callback) self._started = False self._period = period self._thread = None def start(self): """Start the timer""" self._thread = _PeriodicTimerThread(self._period, self._callbacks) self._thread.setDaemon(True) self._thread.start() def stop(self): """Stop the timer""" if self._thread: self._thread.stop() self._thread = None
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater( self.cf, self._useV2, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self._initialized = Event() 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""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: 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 # Once all the parameters are updated call the # callback for "everything updated" if self._check_if_all_updated() and not self.is_updated: self.is_updated = True self._initialized.set() self.all_updated.call() 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) 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: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ def refresh_done(): extended_elements = list() for group in self.toc.toc: for element in self.toc.toc[group].values(): if element.is_extended(): extended_elements.append(element) if len(extended_elements) > 0: extended_type_fetcher = _ExtendedTypeFetcher(self.cf, self.toc) extended_type_fetcher.start() extended_type_fetcher.set_callback(refresh_done_callback) extended_type_fetcher.request_extended_types(extended_elements) else: refresh_done_callback() self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False self._initialized.clear() # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value_raw(self, complete_name, type, value): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) # This gives us the type for the struct.pack pytype = ParamTocElement.types[type][1][1] pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f'<B{len_array}sB{pytype}', 0, char_array, type, value) # We will not get an update callback when using raw (MISC_CHANNEL) # so just send. self.cf.send_packet(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) def get_value(self, complete_name, timeout=60): """ Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') [group, name] = complete_name.split('.') return self.values[group][name] 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 persistent_clear(self, complete_name, callback=None): """ Clear the current value of the specified persistent parameter from 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_CLEAR: 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_CLEAR, element.ident) self.param_updater.send_param_misc(pk) 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) def persistent_get_state(self, complete_name, callback): """ Get the state of the specified persistent parameter. The state will be returned in the supplied callback. The state is represented as a namedtuple with members: `is_stored`, `default_value` and `stored_value`. The state is `None` if the parameter is not persistent or if something goes wrong. | Member | Description | | ----------------- | ----------------------------------------------- | | `is_stored` | `True` if the value is stored to eeprom | | `default_value` | The default value supplied by the firmware | | `stored_value` | Value stored in eeprom, None if `not is_stored` | @param complete_name The 'group.name' name of the parameter to store @param callback Callback, takes `complete_name` and PersistentParamState namedtuple as arg """ 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_GET_STATE: if pk.data[3] == errno.ENOENT: callback(complete_name, None) self.cf.remove_port_callback(CRTPPort.PARAM, new_packet_cb) return is_stored = pk.data[3] == 1 if not is_stored: default_value, = struct.unpack(element.pytype, pk.data[4:]) else: # Remove little-endian indicator ('<') just_type = element.pytype[1:] default_value, stored_value = struct.unpack(f'<{just_type * 2}', pk.data[4:]) callback(complete_name, PersistentParamState( is_stored, default_value, stored_value if is_stored else None ) ) 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_PERSISTENT_GET_STATE, element.ident) self.param_updater.send_param_misc(pk)
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) m.disconnect() except Exception as e: logger.info("Error when removing memory after update: {}".format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info("Requesting number of memories") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR,) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) # logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload))) if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = ord(payload[0]) logger.info("{} memories found".format(self.nbr_of_mems)) # Start requesting information about the memories, if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info("Requesting first id") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory (the I2C). logger.error("-------->Got good count, but no info on mem!") self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = ord(payload[0]) # Type - 1 byte mem_type = ord(payload[1]) # Size 4 bytes (as addr) mem_size = struct.unpack("I", payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack("B" * 8, payload[6:14]) mem_addr = "" for m in mem_addr_raw: mem_addr += "{:02X}".format(m) if not self.get_mem(mem_id): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info("Requesting information about memory {}".format(self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info("Done getting all the memories, start reading the OWs") ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise we are done for ow_mem in self.get_mems(MemoryElement.TYPE_1W): ow_mem.update(self._mem_update_done) if len(self.get_mems(MemoryElement.TYPE_1W)) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status)) # Find the read request if id in self._write_requests: wreq = self._write_requests[id] if status == 0: if wreq.write_done(addr): self._write_requests.pop(id, None) self.mem_write_cb.call(wreq.mem, wreq.addr) else: wreq.resend() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) data = struct.unpack("B" * len(payload[5:]), payload[5:]) logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info("READING: We are still interested in request for mem {}".format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: rreq.resend()
class Memory(): """Access memories on the Crazyflie""" # 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): """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.cf.disconnected.add_callback(self._disconnected) 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._read_requests_lock = Lock() self._write_requests = {} self._write_requests_lock = Lock() 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 ow_search(self, vid=0xBC, pid=None, name=None): """Search for specific memory id/name and return it""" for m in self.get_mems(MemoryElement.TYPE_1W): if pid and m.pid == pid or name and m.name == name: return m return None def write(self, memory, addr, data, flush_queue=False): """Write the specified data to the given memory at the given address""" wreq = _WriteRequest(memory, addr, data, self.cf) if memory.id not in self._write_requests: self._write_requests[memory.id] = [] # Workaround until we secure the uplink and change messages for # mems to non-blocking self._write_requests_lock.acquire() if flush_queue: self._write_requests[memory.id] = self._write_requests[ memory.id][:1] self._write_requests[memory.id].insert(len(self._write_requests), wreq) if len(self._write_requests[memory.id]) == 1: wreq.start() self._write_requests_lock.release() 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) m.disconnect() except Exception as e: logger.info( 'Error when removing memory after update: {}'.format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info('Requesting number of memories') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR,) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _disconnected(self, uri): """The link to the Crazyflie has been broken. Reset state""" for m in self.mems: try: m.disconnect() except Exception as e: logger.info( 'Error when resetting after disconnect: {}'.format(e)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = payload[0] logger.info('{} memories found'.format(self.nbr_of_mems)) # Start requesting information about the memories, # if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info('Requesting first id') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=( CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory # (the I2C). logger.error( '-------->Got good count, but no info on mem!') self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = payload[0] # Type - 1 byte mem_type = payload[1] # Size 4 bytes (as addr) mem_size = struct.unpack('I', payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) mem_addr = '' for m in mem_addr_raw: mem_addr += '{:02X}'.format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_DRIVER_LED: mem = LEDDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_LOCO: mem = LocoMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info( 'Requesting information about memory {}'.format( self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=( CMD_INFO_DETAILS, self._fetch_id)) else: logger.info( 'Done getting all the memories, start reading the OWs') ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise # we are done for ow_mem in ows: ow_mem.update(self._mem_update_done) if len(ows) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) logger.info( 'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format( id, addr, status)) # Find the read request if id in self._write_requests: self._write_requests_lock.acquire() wreq = self._write_requests[id][0] if status == 0: if wreq.write_done(addr): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) self.mem_write_cb.call(wreq.mem, wreq.addr) # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: self._write_requests[id][0].start() else: logger.info('Status {}: write resending...'.format(status)) wreq.resend() self._write_requests_lock.release() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) data = struct.unpack('B' * len(payload[5:]), payload[5:]) logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, ' 'data={}'.format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info( 'READING: We are still interested in request for ' 'mem {}'.format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: logger.info('Status {}: resending...'.format(status)) rreq.resend()
class Memory(): """Access memories on the Crazyflie""" # 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): """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._read_requests_lock = Lock() self._write_requests = {} self._write_requests_lock = Lock() 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 ow_search(self, vid=0xBC, pid=None, name=None): """Search for specific memory id/name and return it""" for m in self.get_mems(MemoryElement.TYPE_1W): if pid and m.pid == pid or name and m.name == name: return m return None def write(self, memory, addr, data, flush_queue=False): """Write the specified data to the given memory at the given address""" wreq = _WriteRequest(memory, addr, data, self.cf) if memory.id not in self._write_requests: self._write_requests[memory.id] = [] # Workaround until we secure the uplink and change messages for # mems to non-blocking self._write_requests_lock.acquire() if flush_queue: self._write_requests[memory.id] = self._write_requests[ memory.id][:1] self._write_requests[memory.id].insert(len(self._write_requests), wreq) if len(self._write_requests[memory.id]) == 1: wreq.start() self._write_requests_lock.release() 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) m.disconnect() except Exception as e: logger.info( 'Error when removing memory after update: {}'.format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info('Requesting number of memories') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR, ) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR, )) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.data[0] payload = packet.data[1:] if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = payload[0] logger.info('{} memories found'.format(self.nbr_of_mems)) # Start requesting information about the memories, # if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info('Requesting first id') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory # (the I2C). logger.error( '-------->Got good count, but no info on mem!') self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = payload[0] # Type - 1 byte mem_type = payload[1] # Size 4 bytes (as addr) mem_size = struct.unpack('I', payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack('B' * 8, payload[6:14]) mem_addr = '' for m in mem_addr_raw: mem_addr += '{:02X}'.format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_DRIVER_LED: mem = LEDDriverMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) # logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info( 'Requesting information about memory {}'.format( self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info( 'Done getting all the memories, start reading the OWs') ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise # we are done for ow_mem in ows: ow_mem.update(self._mem_update_done) if len(ows) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) logger.info('WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format( id, addr, status)) # Find the read request if id in self._write_requests: self._write_requests_lock.acquire() wreq = self._write_requests[id][0] if status == 0: if wreq.write_done(addr): # self._write_requests.pop(id, None) # Remove the first item self._write_requests[id].pop(0) self.mem_write_cb.call(wreq.mem, wreq.addr) # Get a new one to start (if there are any) if len(self._write_requests[id]) > 0: self._write_requests[id][0].start() else: logger.info('Status {}: write resending...'.format(status)) wreq.resend() self._write_requests_lock.release() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack('<IB', payload[0:5]) data = struct.unpack('B' * len(payload[5:]), payload[5:]) logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, ' 'data={}'.format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info('READING: We are still interested in request for ' 'mem {}'.format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: logger.info('Status {}: resending...'.format(status)) rreq.resend()
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.high_level_commander = HighLevelCommander(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.platform.fetch_platform_informations(self._platform_info_fetched) def _platform_info_fetched(self): 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): """Called when the memories have been identified""" logger.info('Memories finished updating') self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache) def _log_toc_updated_cb(self): """Called when the log TOC has been fully updated""" logger.info('Log TOC finished updating') self.mem.refresh(self._mems_updated_cb) def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED or self.state == State.SETUP_FINISHED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection (download log/parameter TOC). """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver(link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) self._start_connection_setup() except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self._answer_patterns = {} self.disconnected.call(self.link_uri) """Check if the communication link is open or not.""" def is_connected(self): return self.connected_ts is not None def add_port_callback(self, port, cb): """Add a callback to cb on port""" self.incoming.add_port_callback(port, cb) def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) def _no_answer_do_retry(self, pk, pattern): """Resend packets that we have not gotten answers to""" logger.info('Resending for pattern %s', pattern) # Set the timer to None before trying to send again self.send_packet(pk, expected_reply=pattern, resend=True) def _check_for_answers(self, pk): """ Callback called for every packet received to check if we are waiting for an answer on this port. If so, then cancel the retry timer. """ longest_match = () if len(self._answer_patterns) > 0: data = (pk.header, ) + tuple(pk.data) for p in list(self._answer_patterns.keys()): logger.debug('Looking for pattern match on %s vs %s', p, data) if len(p) <= len(data): if p == data[0:len(p)]: match = data[0:len(p)] if len(match) >= len(longest_match): logger.debug('Found new longest match %s', match) longest_match = match if len(longest_match) > 0: self._answer_patterns[longest_match].cancel() del self._answer_patterns[longest_match] def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ Send a packet through the link interface. pk -- Packet to send expect_answer -- True if a packet from the Crazyflie is expected to be sent back, otherwise false """ # print("Send packet in port: %x"%(pk.port)) self._send_lock.acquire() if self.link is not None: if len(expected_reply ) > 0 and not resend and self.link.needs_resending: pattern = (pk.header, ) + expected_reply logger.debug( 'Sending packet and expecting the %s pattern back', pattern) new_timer = Timer( timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() elif resend: # Check if we have gotten an answer, if not try again pattern = expected_reply if pattern in self._answer_patterns: logger.debug('We want to resend and the pattern is there') if self._answer_patterns[pattern]: new_timer = Timer( timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() else: logger.debug('Resend requested, but no pattern found: %s', self._answer_patterns) self.link.send_packet(pk) self.packet_sent.call(pk) self._send_lock.release()
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: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) 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) pk.data = struct.pack('<B', varid) pk.data += struct.pack(element.pytype, eval(value)) self.param_updater.request_param_setvalue(pk)
class Crazyflie(): """The Crazyflie class""" def __init__(self, link=None): """ Create the objects from this module and register callbacks. """ # 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 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 # Thread handling incoming packets self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) self.incoming.start() # Thread handling outgoing packets self.outgoing = _OutgoingPacketHandler(self) self.outgoing.setDaemon(True) self.outgoing.start() self.commander = Commander(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._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 _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver(link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) logger.info('Connection setup finished') self.connected_ts = datetime.datetime.now() self.connected.call(self.link_uri) except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self.disconnected.call(self.link_uri) def is_connected(self): """Check if the communication link is open or not.""" return self.connected_ts is not None def send_packet(self, pk): """ Send a packet through the link interface. pk -- Packet to send """ self._send_lock.acquire() if self.link is not None: self.link.send_packet(pk) # self.packet_sent.call(pk) self._send_lock.release()
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater( self.cf, self._useV2, self._param_updated, crazyflie.on_params_flushed) 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""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: 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: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) 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) # print("Id for parameter: ", complete_name, " = ", varid) self.param_updater.request_param_setvalue(pk)
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) m.disconnect() except Exception as e: logger.info("Error when removing memory after update: {}".format(e)) self.mems = [] self.nbr_of_mems = 0 self._getting_count = False logger.info("Requesting number of memories") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR, ) self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _new_packet_cb(self, packet): """Callback for newly arrived packets for the memory port""" chan = packet.channel cmd = packet.datal[0] payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:]) #logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload))) if chan == CHAN_INFO: if cmd == CMD_INFO_NBR: self.nbr_of_mems = ord(payload[0]) logger.info("{} memories found".format(self.nbr_of_mems)) # Start requesting information about the memories, if there are any... if self.nbr_of_mems > 0: if not self._getting_count: self._getting_count = True logger.info("Requesting first id") pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0)) else: self._refresh_callback() if cmd == CMD_INFO_DETAILS: # Did we get a good reply, otherwise try again: if len(payload) < 5: # Workaround for 1-wire bug when memory is detected # but updating the info crashes the communication with # the 1-wire. Fail by saying we only found 1 memory (the I2C). logger.error("-------->Got good count, but no info on mem!") self.nbr_of_mems = 1 if self._refresh_callback: self._refresh_callback() self._refresh_callback = None return # Create information about a new memory # Id - 1 byte mem_id = ord(payload[0]) # Type - 1 byte mem_type = ord(payload[1]) # Size 4 bytes (as addr) mem_size = struct.unpack("I", payload[2:6])[0] # Addr (only valid for 1-wire?) mem_addr_raw = struct.unpack("B"*8, payload[6:14]) mem_addr = "" for m in mem_addr_raw: mem_addr += "{:02X}".format(m) if (not self.get_mem(mem_id)): if mem_type == MemoryElement.TYPE_1W: mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) self._ow_mems_left_to_update.append(mem.id) elif mem_type == MemoryElement.TYPE_I2C: mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) else: mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.info(mem) self.mems.append(mem) self.mem_added_cb.call(mem) #logger.info(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.info("Requesting information about memory {}".format(self._fetch_id)) pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, self._fetch_id) self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id)) else: logger.info("Done getting all the memories, start reading the OWs") ows = self.get_mems(MemoryElement.TYPE_1W) # If there are any OW mems start reading them, otherwise we are done for ow_mem in self.get_mems(MemoryElement.TYPE_1W): ow_mem.update(self._mem_update_done) if len (self.get_mems(MemoryElement.TYPE_1W)) == 0: if self._refresh_callback: self._refresh_callback() self._refresh_callback = None if chan == CHAN_WRITE: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status)) # Find the read request if id in self._write_requests: wreq = self._write_requests[id] if status == 0: if wreq.write_done(addr): self._write_requests.pop(id, None) self.mem_write_cb.call(wreq.mem, wreq.addr) else: wreq.resend() if chan == CHAN_READ: id = cmd (addr, status) = struct.unpack("<IB", payload[0:5]) data = struct.unpack("B"*len(payload[5:]), payload[5:]) logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.info("READING: We are still interested in request for mem {}".format(id)) rreq = self._read_requests[id] if status == 0: if rreq.add_data(addr, payload[5:]): self._read_requests.pop(id, None) self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data) else: rreq.resend()
class CallerTest(unittest.TestCase): def setUp(self): self.callback_count = 0 self.sut = Caller() def test_that_callback_is_added(self): # Fixture # Test self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_callback_is_added_only_one_time(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_multiple_callbacks_are_added(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback2) # Assert self.sut.call() self.assertEqual(2, self.callback_count) def test_that_callback_is_removed(self): # Fixture self.sut.add_callback(self._callback) # Test self.sut.remove_callback(self._callback) # Assert self.sut.call() self.assertEqual(0, self.callback_count) def test_that_callback_is_called_with_arguments(self): # Fixture self.sut.add_callback(self._callback_with_args) # Test self.sut.call('The token') # Assert self.assertEqual('The token', self.callback_token) def _callback(self): self.callback_count += 1 def _callback2(self): self.callback_count += 1 def _callback_with_args(self, token): self.callback_token = token
class CallerTest(unittest.TestCase): def setUp(self): self.callback_count = 0 self.sut = Caller() def test_that_callback_is_added(self): # Fixture # Test self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_callback_is_added_only_one_time(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback) # Assert self.sut.call() self.assertEqual(1, self.callback_count) def test_that_multiple_callbacks_are_added(self): # Fixture # Test self.sut.add_callback(self._callback) self.sut.add_callback(self._callback2) # Assert self.sut.call() self.assertEqual(2, self.callback_count) def test_that_callback_is_removed(self): # Fixture self.sut.add_callback(self._callback) # Test self.sut.remove_callback(self._callback) # Assert self.sut.call() self.assertEqual(0, self.callback_count) def test_that_callback_is_called_with_arguments(self): # Fixture self.sut.add_callback(self._callback_with_args) # Test self.sut.call('The token') # Assert self.assertEqual('The token', self.callback_token) def _callback(self): self.callback_count += 1 def _callback2(self): self.callback_count += 1 def _callback_with_args(self, token): self.callback_token = token
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): """Called when the memories have been identified""" logger.info('Memories finished updating') self.param.refresh_toc(self._param_toc_updated_cb, self._toc_cache) def _log_toc_updated_cb(self): """Called when the log TOC has been fully updated""" logger.info('Log TOC finished updating') self.mem.refresh(self._mems_updated_cb) def _link_error_cb(self, errmsg): """Called from the link driver when there's an error""" logger.warning('Got link error callback [%s] in state [%s]', errmsg, self.state) if (self.link is not None): self.link.close() self.link = None if (self.state == State.INITIALIZED): self.connection_failed.call(self.link_uri, errmsg) if (self.state == State.CONNECTED or self.state == State.SETUP_FINISHED): self.disconnected.call(self.link_uri) self.connection_lost.call(self.link_uri, errmsg) self.state = State.DISCONNECTED def _link_quality_cb(self, percentage): """Called from link driver to report link quality""" self.link_quality_updated.call(percentage) def _check_for_initial_packet_cb(self, data): """ Called when first packet arrives from Crazyflie. This is used to determine if we are connected to something that is answering. """ self.state = State.CONNECTED self.link_established.call(self.link_uri) self.packet_received.remove_callback(self._check_for_initial_packet_cb) def open_link(self, link_uri): """ Open the communication link to a copter at the given URI and setup the connection (download log/parameter TOC). """ self.connection_requested.call(link_uri) self.state = State.INITIALIZED self.link_uri = link_uri try: self.link = cflib.crtp.get_link_driver( link_uri, self._link_quality_cb, self._link_error_cb) if not self.link: message = 'No driver found or malformed URI: {}' \ .format(link_uri) logger.warning(message) self.connection_failed.call(link_uri, message) else: # Add a callback so we can check that any data is coming # back from the copter self.packet_received.add_callback( self._check_for_initial_packet_cb) self._start_connection_setup() except Exception as ex: # pylint: disable=W0703 # We want to catch every possible exception here and show # it in the user interface import traceback logger.error("Couldn't load link driver: %s\n\n%s", ex, traceback.format_exc()) exception_text = "Couldn't load link driver: %s\n\n%s" % ( ex, traceback.format_exc()) if self.link: self.link.close() self.link = None self.connection_failed.call(link_uri, exception_text) def close_link(self): """Close the communication link.""" logger.info('Closing link') if (self.link is not None): self.commander.send_setpoint(0, 0, 0, 0) if (self.link is not None): self.link.close() self.link = None self._answer_patterns = {} self.disconnected.call(self.link_uri) """Check if the communication link is open or not.""" def is_connected(self): return self.connected_ts is not None def add_port_callback(self, port, cb): """Add a callback to cb on port""" self.incoming.add_port_callback(port, cb) def remove_port_callback(self, port, cb): """Remove the callback cb on port""" self.incoming.remove_port_callback(port, cb) def _no_answer_do_retry(self, pk, pattern): """Resend packets that we have not gotten answers to""" logger.info('Resending for pattern %s', pattern) # Set the timer to None before trying to send again self.send_packet(pk, expected_reply=pattern, resend=True) def _check_for_answers(self, pk): """ Callback called for every packet received to check if we are waiting for an answer on this port. If so, then cancel the retry timer. """ longest_match = () if len(self._answer_patterns) > 0: data = (pk.header,) + tuple(pk.data) for p in list(self._answer_patterns.keys()): logger.debug('Looking for pattern match on %s vs %s', p, data) if len(p) <= len(data): if p == data[0:len(p)]: match = data[0:len(p)] if len(match) >= len(longest_match): logger.debug('Found new longest match %s', match) longest_match = match if len(longest_match) > 0: self._answer_patterns[longest_match].cancel() del self._answer_patterns[longest_match] def send_packet(self, pk, expected_reply=(), resend=False, timeout=0.2): """ Send a packet through the link interface. pk -- Packet to send expect_answer -- True if a packet from the Crazyflie is expected to be sent back, otherwise false """ self._send_lock.acquire() if self.link is not None: if len(expected_reply) > 0 and not resend and \ self.link.needs_resending: pattern = (pk.header,) + expected_reply logger.debug( 'Sending packet and expecting the %s pattern back', pattern) new_timer = Timer(timeout, lambda: self._no_answer_do_retry(pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() elif resend: # Check if we have gotten an answer, if not try again pattern = expected_reply if pattern in self._answer_patterns: logger.debug('We want to resend and the pattern is there') if self._answer_patterns[pattern]: new_timer = Timer(timeout, lambda: self._no_answer_do_retry( pk, pattern)) self._answer_patterns[pattern] = new_timer new_timer.start() else: logger.debug('Resend requested, but no pattern found: %s', self._answer_patterns) self.link.send_packet(pk) self.packet_sent.call(pk) self._send_lock.release()
class Param(): """ Used to read and write parameter values in the Crazyflie. """ toc = Toc() def __init__(self, crazyflie): 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.param_updater.close) self.all_updated = Caller() self._have_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 not g in self.values: return False for n in self.toc.toc[g]: if not n in self.values[g]: return False return True def _param_updated(self, pk): """Callback with data for an updated parameter""" var_id = pk.datal[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 not element.group in self.values: self.values[element.group] = {} self.values[element.group][element.name] = s # This will only be called once if self._check_if_all_updated() and not self._have_updated: self._have_updated = True self.all_updated.call() 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) 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 not group in self.group_update_callbacks: self.group_update_callbacks[group] = Caller() self.group_update_callbacks[group].add_callback(cb) else: paramname = "{}.{}".format(group, name) if not paramname in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self.toc = Toc() toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self._have_updated = False def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) 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) pk.data = struct.pack('<B', varid) pk.data += struct.pack(element.pytype, eval(value)) self.param_updater.request_param_setvalue(pk)
class Param(): """ Used to read and write parameter values in the Crazyflie. """ def __init__(self, crazyflie): self.toc = Toc() self.cf = crazyflie self._useV2 = False self.param_update_callbacks = {} self.group_update_callbacks = {} self.all_update_callback = Caller() self.param_updater = None self.param_updater = _ParamUpdater(self.cf, self._useV2, self._param_updated) self.param_updater.start() self.cf.disconnected.add_callback(self._disconnected) self.all_updated = Caller() self.is_updated = False self._initialized = Event() 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""" if self._useV2: var_id = struct.unpack('<H', pk.data[:2])[0] else: var_id = pk.data[0] element = self.toc.get_element_by_id(var_id) if element: if self._useV2: s = struct.unpack(element.pytype, pk.data[2:])[0] else: 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._initialized.set() 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: paramname = '{}.{}'.format(group, name) if paramname not in self.param_update_callbacks: self.param_update_callbacks[paramname] = Caller() self.param_update_callbacks[paramname].add_callback(cb) def refresh_toc(self, refresh_done_callback, toc_cache): """ Initiate a refresh of the parameter TOC. """ self._useV2 = self.cf.platform.get_protocol_version() >= 4 toc_fetcher = TocFetcher(self.cf, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Crazyflie API""" self.param_updater.close() self.is_updated = False self._initialized.clear() # Clear all values from the previous Crazyflie self.toc = Toc() self.values = {} def request_param_update(self, complete_name): """ Request an update of the value for the supplied parameter. """ self.param_updater.request_param_update( self.toc.get_element_id(complete_name)) def set_value_raw(self, complete_name, type, value): """ Set a parameter value using the complete name and the type. Does not need to have received the TOC. """ char_array = bytes(complete_name.replace('.', '\0') + '\0', 'utf-8') len_array = len(char_array) # This gives us the type for the struct.pack pytype = ParamTocElement.types[type][1][1] pk = CRTPPacket() pk.set_header(CRTPPort.PARAM, MISC_CHANNEL) pk.data = struct.pack(f'<B{len_array}sB{pytype}', 0, char_array, type, value) # We will not get an update callback when using raw (MISC_CHANNEL) # so just send. self.cf.send_packet(pk) 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) def get_value(self, complete_name, timeout=60): """ Read a value for the supplied parameter. This can block for a period of time if the parameter values have not been fetched yet. """ if not self._initialized.wait(timeout=60): raise Exception('Connection timed out') [group, name] = complete_name.split('.') return self.values[group][name]