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""" if self._thread: logger.warning("Timer already started, not restarting") return 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 Memory(): """Access memories on the Espdrone""" # 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, espdrone=None): """Instantiate class and connect callbacks""" # 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.ed = espdrone self.ed.add_port_callback(CRTPPort.MEM, self._new_packet_cb) self.ed.disconnected.add_callback(self._disconnected) self._write_requests_lock = Lock() self._clear_state() def _clear_state(self): self.mems = [] 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.debug(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.ed) 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.ed) 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.debug('Requesting number of memories') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_NBR,) self.ed.send_packet(pk, expected_reply=(CMD_INFO_NBR,)) def _disconnected(self, uri): """The link to the Espdrone has been broken. Reset state""" self._clear_state() 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.debug('Requesting first id') pk = CRTPPacket() pk.set_header(CRTPPort.MEM, CHAN_INFO) pk.data = (CMD_INFO_DETAILS, 0) self.ed.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.debug(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.debug(mem) self.mem_read_cb.add_callback(mem.new_data) elif mem_type == MemoryElement.TYPE_TRAJ: mem = TrajectoryMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_LOCO2: mem = LocoMemory2(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) elif mem_type == MemoryElement.TYPE_LH: mem = LighthouseMemory(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(mem) self.mem_read_cb.add_callback(mem.new_data) self.mem_write_cb.add_callback(mem.write_done) elif mem_type == MemoryElement.TYPE_MEMORY_TESTER: mem = MemoryTester(id=mem_id, type=mem_type, size=mem_size, mem_handler=self) logger.debug(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.debug(mem) self.mems.append(mem) self.mem_added_cb.call(mem) self._fetch_id = mem_id + 1 if self.nbr_of_mems - 1 >= self._fetch_id: logger.debug( '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.ed.send_packet(pk, expected_reply=( CMD_INFO_DETAILS, self._fetch_id)) else: logger.debug( '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.debug( '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.debug( '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.debug('READ: Mem={}, addr=0x{:X}, status=0x{}, ' 'data={}'.format(id, addr, status, data)) # Find the read request if id in self._read_requests: logger.debug( '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.debug('Status {}: resending...'.format(status)) rreq.resend()
class Espdrone(): """The Espdrone class""" def __init__(self, name=None, 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.name = name self._toc_cache = TocCache(ro_cache=ro_cache, rw_cache=rw_cache) self.incoming = _IncomingPacketHandler(self) self.incoming.setDaemon(True) self.incoming.start() self.camera = Camera(self) 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 Espdrone. 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 = edlib.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) raise ConnectionError() 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.1): """ Send a packet through the link interface. pk -- Packet to send expect_answer -- True if a packet from the Espdrone 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 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 Param(): """ Used to read and write parameter values in the Espdrone. """ def __init__(self, espdrone): self.toc = Toc() self.ed = espdrone 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.ed, self._useV2, self._param_updated) self.param_updater.start() self.ed.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 Espdrone. """ 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.ed.platform.get_protocol_version() >= 4 toc_fetcher = Toedetcher(self.ed, ParamTocElement, CRTPPort.PARAM, self.toc, refresh_done_callback, toc_cache) toc_fetcher.start() def _disconnected(self, uri): """Disconnected callback from Espdrone API""" self.param_updater.close() self.is_updated = False # Clear all values from the previous Espdrone 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) self.param_updater.request_param_setvalue(pk)