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