def _request_protocol_version(self):
# Sending a sink request to detect if the connected Espdrone
# supports protocol versioning
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, LINKSERVICE_SOURCE)
pk.data = (0,)
self._ed.send_packet(pk)
def _write_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Espdrone
"""
# Figure out the length of the next request
new_len = len(self._data)
if new_len > _WriteRequest.MAX_DATA_LENGTH:
new_len = _WriteRequest.MAX_DATA_LENGTH
logger.debug('Writing new chunk of {}bytes at 0x{:X}'.format(
new_len, self._current_addr))
data = self._data[:new_len]
self._data = self._data[new_len:]
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_WRITE)
pk.data = struct.pack('<BI', self.mem.id, self._current_addr)
# Create a tuple used for matching the reply using id and address
reply = struct.unpack('<BBBBB', pk.data)
self._sent_reply = reply
# Add the data
pk.data += struct.pack('B' * len(data), *data)
self._sent_packet = pk
self.ed.send_packet(pk, expected_reply=reply, timeout=1)
self._addr_add = len(data)
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 read_flash(self, addr=0xFF, page=0x00):
"""Read back a flash page from the Espdrone and return it"""
buff = bytearray()
page_size = self.targets[addr].page_size
for i in range(0, int(math.ceil(page_size / 25.0))):
pk = None
retry_counter = 5
while ((not pk or pk.header != 0xFF
or struct.unpack('<BB', pk.data[0:2]) != (addr, 0x1C))
and retry_counter >= 0):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('<BBHH', addr, 0x1C, page, (i * 25))
self.link.send_packet(pk)
pk = self.link.receive_packet(1)
retry_counter -= 1
if (retry_counter < 0):
return None
else:
buff += pk.data[6:]
# For some reason we get one byte extra here...
return buff[0:page_size]
def write_flash(self, addr, page_buffer, target_page, page_count):
"""Initiate flashing of data in the buffer to flash."""
# print "Write page", flashPage
# print "Writing page [%d] and [%d] forward" % (flashPage, nPage)
pk = None
# Flushing downlink ...
pk = self.link.receive_packet(0)
while pk is not None:
pk = self.link.receive_packet(0)
retry_counter = 5
# print "Flasing to 0x{:X}".format(addr)
while ((not pk or pk.header != 0xFF
or struct.unpack('<BB', pk.data[0:2]) != (addr, 0x18))
and retry_counter >= 0):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('<BBHHH', addr, 0x18, page_buffer,
target_page, page_count)
self.link.send_packet(pk)
pk = self.link.receive_packet(1)
retry_counter -= 1
if retry_counter < 0:
self.error_code = -1
return False
self.error_code = pk.data[3]
return pk.data[2] == 1
def create(self):
"""Save the log configuration in the Espdrone"""
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
if self.useV2:
pk.data = (CMD_CREATE_BLOCK_V2, self.id)
else:
pk.data = (CMD_CREATE_BLOCK, self.id)
for var in self.variables:
if (var.is_toc_variable() is False): # Memory location
logger.debug('Logging to raw memory %d, 0x%04X',
var.get_storage_and_fetch_byte(), var.address)
pk.data.append(
struct.pack('<B', var.get_storage_and_fetch_byte()))
pk.data.append(struct.pack('<I', var.address))
else: # Item in TOC
logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
self.ed.log.toc.get_element_id(var.name),
var.get_storage_and_fetch_byte())
pk.data.append(var.get_storage_and_fetch_byte())
if self.useV2:
ident = self.ed.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.ed.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
if self.useV2:
self.ed.send_packet(pk,
expected_reply=(CMD_CREATE_BLOCK_V2, self.id))
else:
self.ed.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
def set_continous_wave(self, enabled):
"""
Enable/disable the client side X-mode. When enabled this recalculates
the setpoints before sending them to the Espdrone.
"""
pk = CRTPPacket()
pk.set_header(CRTPPort.PLATFORM, PLATFORM_COMMAND)
pk.data = (0, enabled)
self._ed.send_packet(pk)
def request_param_update(self, var_id):
"""Place a param update request on the queue"""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
if self._useV2:
pk.data = struct.pack('<H', var_id)
else:
pk.data = struct.pack('<B', var_id)
logger.debug('Requesting request to update param [%d]', var_id)
self.request_queue.put(pk)
def stop(self):
"""Stop the logging for this entry"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Stopping block, but no block registered')
else:
logger.debug('Sending stop logging for block id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_STOP_LOGGING, self.id))
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.ed.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, ))
def _request_toc_element(self, index):
"""Request information about a specific item in the TOC"""
logger.debug('Requesting index %d on port %d', index, self.port)
pk = CRTPPacket()
if self._useV2:
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff)
self.ed.send_packet(pk, expected_reply=(
CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff))
else:
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ELEMENT, index)
self.ed.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
def delete(self):
"""Delete this entry in the Espdrone"""
if (self.ed.link is not None):
if (self.id is None):
logger.warning('Delete block, but no block registered')
else:
logger.debug(
'LogEntry: Sending delete logging for block id=%d' %
self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.ed.send_packet(pk,
expected_reply=(CMD_DELETE_BLOCK, self.id))
def _crt_service_callback(self, pk):
if pk.channel == LINKSERVICE_SOURCE:
# If the sink contains a magic string, get the protocol version,
# otherwise -1
if pk.data[:18].decode('utf8').rstrip('\0') == 'Edlab Espdrone':
pk = CRTPPacket()
pk.set_header(CRTPPort.PLATFORM, VERSION_COMMAND)
pk.data = (VERSION_GET_PROTOCOL, )
self._ed.send_packet(pk)
else:
print(pk.data[:18].decode('utf8'))
self._protocolVersion = -1
logger.info('Procotol version: {}'.format(
self.get_protocol_version()))
self._callback()
def reset_to_bootloader1(self, cpu_id):
""" Reset to the bootloader
The parameter cpuid shall correspond to the device to reset.
Return true if the reset has been done and the contact with the
bootloader is established.
"""
# Send an echo request and wait for the answer
# Mainly aim to bypass a bug of the espdrone firmware that prevents
# reset before normal CRTP communication
pk = CRTPPacket()
pk.port = CRTPPort.LINKCTRL
pk.data = (1, 2, 3) + cpu_id
self.link.send_packet(pk)
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == CRTPPort.LINKCTRL:
break
# Send the reset to bootloader request
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (0xFF, 0xFE) + cpu_id
self.link.send_packet(pk)
# Wait to ack the reset ...
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if pk.port == 0xFF and tuple(pk.data) == (0xFF, 0xFE) + cpu_id:
pk.data = (0xFF, 0xF0) + cpu_id
self.link.send_packet(pk)
break
time.sleep(0.1)
self.link.close()
self.link = edlib.crtp.get_link_driver(self.clink_address)
# time.sleep(0.1)
return self._update_info()
def start(self):
"""Start the logging for this entry"""
if (self.ed.link is not None):
if (self._added is False):
self.create()
logger.debug('First time block is started, add block')
else:
logger.debug(
'Block already registered, starting logging'
' for id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.ed.send_packet(pk,
expected_reply=(CMD_START_LOGGING,
self.id))
def _request_new_chunk(self):
"""
Called to request a new chunk of data to be read from the Espdrone
"""
# Figure out the length of the next request
new_len = self._bytes_left
if new_len > _ReadRequest.MAX_DATA_LENGTH:
new_len = _ReadRequest.MAX_DATA_LENGTH
logger.debug('Requesting new chunk of {}bytes at 0x{:X}'.format(
new_len, self._current_addr))
# Request the data for the next address
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_READ)
pk.data = struct.pack('<BIB', self.mem.id, self._current_addr, new_len)
reply = struct.unpack('<BBBBB', pk.data[:-1])
self.ed.send_packet(pk, expected_reply=reply, timeout=1)
def start(self):
"""Initiate fetching of the TOC."""
self._useV2 = self.ed.platform.get_protocol_version() >= 4
logger.debug('[%d]: Using V2 protocol: %d', self.port, self._useV2)
logger.debug('[%d]: Start fetching...', self.port)
# Register callback in this class for the port
self.ed.add_port_callback(self.port, self._new_packet_cb)
# Request the TOC CRC
self.state = GET_TOC_INFO
pk = CRTPPacket()
pk.set_header(self.port, TOC_CHANNEL)
if self._useV2:
pk.data = (CMD_TOC_INFO_V2,)
self.ed.send_packet(pk, expected_reply=(CMD_TOC_INFO_V2,))
else:
pk.data = (CMD_TOC_INFO,)
self.ed.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
def reset_to_bootloader(self, target_id):
retry_counter = 5
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xFF)
self.link.send_packet(pk)
got_answer = False
while (not got_answer and retry_counter >= 0):
pk = self.link.receive_packet(1)
if pk and pk.header == 0xFF:
try:
data = struct.unpack('<BB', pk.data[0:2])
got_answer = data == (target_id, 0xFF)
except struct.error:
# Failed unpacking, retry
pass
if got_answer:
new_address = (0xb1, ) + struct.unpack('<BBBB', pk.data[2:6][::-1])
# The reset packet arrival cannot be checked.
# Send it more than one time to increase the chances it makes it.
for _ in range(10):
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xF0, 0x00)
self.link.send_packet(pk)
addr = int(binascii.hexlify(struct.pack('B' * 5, *new_address)),
16)
time.sleep(1)
self.link.close()
time.sleep(0.2)
self.link = edlib.crtp.get_link_driver(
'radio://0/0/2M/{:X}'.format(addr))
return True
else:
return False
def _update_mapping(self, target_id):
pk = CRTPPacket()
pk.set_header(0xff, 0xff)
pk.data = (target_id, 0x12)
self.link.send_packet(pk)
pk = self.link.receive_packet(2)
if (pk and pk.header == 0xFF
and struct.unpack('<BB', pk.data[0:2]) == (target_id, 0x12)):
m = pk.datat[2:]
if (len(m) % 2) != 0:
raise Exception('Malformed flash mapping packet')
self.mapping = []
page = 0
for i in range(int(len(m) / 2)):
for j in range(m[2 * i]):
self.mapping.append(page)
page += m[(2 * i) + 1]
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 _update_info(self, target_id):
""" Call the command getInfo and fill up the information received in
the fields of the object
"""
# Call getInfo ...
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0x10)
self.link.send_packet(pk)
# Wait for the answer
pk = self.link.receive_packet(2)
if (pk and pk.header == 0xFF
and struct.unpack('<BB', pk.data[0:2]) == (target_id, 0x10)):
tab = struct.unpack('BBHHHH', pk.data[0:10])
cpuid = struct.unpack('B' * 12, pk.data[10:22])
if target_id not in self.targets:
self.targets[target_id] = Target(target_id)
self.targets[target_id].addr = target_id
if len(pk.data) > 22:
self.targets[target_id].protocol_version = pk.datat[22]
self.protocol_version = pk.datat[22]
self.targets[target_id].page_size = tab[2]
self.targets[target_id].buffer_pages = tab[3]
self.targets[target_id].flash_pages = tab[4]
self.targets[target_id].start_page = tab[5]
self.targets[target_id].cpuid = '%02X' % cpuid[0]
for i in cpuid[1:]:
self.targets[target_id].cpuid += ':%02X' % i
if (self.protocol_version == 0x10
and target_id == TargetTypes.STM32):
self._update_mapping(target_id)
return True
return False
def upload_buffer(self, target_id, page, address, buff):
"""Upload data into a buffer on the Espdrone"""
# print len(buff)
count = 0
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('=BBHH', target_id, 0x14, page, address)
for i in range(0, len(buff)):
pk.data.append(buff[i])
count += 1
if count > 24:
self.link.send_packet(pk)
count = 0
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = struct.pack('=BBHH', target_id, 0x14, page,
i + address + 1)
self.link.send_packet(pk)
def reset_to_firmware(self, target_id):
""" Reset to firmware
The parameter cpuid shall correspond to the device to reset.
Return true if the reset has been done
"""
# The fake CPU ID is legacy from the Espdrone 1.0
# In order to reset the CPU id had to be sent, but this
# was removed before launching it. But the length check is
# still in the bootloader. So to work around this bug so
# some extra data needs to be sent.
fake_cpu_id = (1, 2, 4, 5, 6, 7, 8, 9, 10, 11, 12)
# Send the reset to bootloader request
pk = CRTPPacket()
pk.set_header(0xFF, 0xFF)
pk.data = (target_id, 0xFF) + fake_cpu_id
self.link.send_packet(pk)
# Wait to ack the reset ...
pk = None
while True:
pk = self.link.receive_packet(2)
if not pk:
return False
if (pk.header == 0xFF and struct.unpack(
'B' * len(pk.data), pk.data)[:2] == (target_id, 0xFF)):
# Difference in ed1 and ed2 (CPU ID)
if target_id == 0xFE:
pk.data = (target_id, 0xF0, 0x01)
else:
pk.data = (target_id, 0xF0) + fake_cpu_id
self.link.send_packet(pk)
break
time.sleep(0.1)
class CRTPPacketTest(unittest.TestCase):
def setUp(self):
self.callback_count = 0
self.sut = CRTPPacket()
def test_that_port_and_channle_is_encoded_in_header(self):
# Fixture
self.sut.set_header(2, 1)
# Test
actual = self.sut.get_header()
# Assert
expected = 0x2d
self.assertEqual(expected, actual)
def test_that_port_is_truncated_in_header(self):
# Fixture
port = 0xff
self.sut.set_header(port, 0)
# Test
actual = self.sut.get_header()
# Assert
expected = 0xfc
self.assertEqual(expected, actual)
def test_that_channel_is_truncated_in_header(self):
# Fixture
channel = 0xff
self.sut.set_header(0, channel)
# Test
actual = self.sut.get_header()
# Assert
expected = 0x0f
self.assertEqual(expected, actual)
def test_that_port_and_channel_is_encoded_in_header_when_set_separat(self):
# Fixture
self.sut.port = 2
self.sut.channel = 1
# Test
actual = self.sut.get_header()
# Assert
expected = 0x2d
self.assertEqual(expected, actual)
def test_that_default_header_is_set_when_constructed(self):
# Fixture
# Test
actual = self.sut.get_header()
# Assert
expected = 0x0c
self.assertEqual(expected, actual)
def test_that_header_is_set_when_constructed(self):
# Fixture
sut = CRTPPacket(header=0x21)
# Test
actual = sut.get_header()
# Assert
self.assertEqual(0x2d, actual)
self.assertEqual(2, sut.port)
self.assertEqual(1, sut.channel)
def forceDisconnecPressed(self):
if (self.helper.cf.link is not None):
p = CRTPPacket()
p.set_header(CRTPPort.DEBUGDRIVER, 0)
p.data = struct.pack('<B', 1) # Force disconnect
self.helper.cf.send_packet(p)
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()
def linkQualityChanged(self, value):
if (self.helper.cf.link is not None):
p = CRTPPacket()
p.set_header(CRTPPort.DEBUGDRIVER, 0)
p.data = struct.pack('<BB', 0, value) # Set link quality
self.helper.cf.send_packet(p)
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if (chan == CHAN_SETTINGS):
id = payload[0]
error_status = payload[1]
block = self._find_block(id)
if cmd == CMD_CREATE_BLOCK or cmd == CMD_CREATE_BLOCK_V2:
if (block is not None):
if error_status == 0 or error_status == errno.EEXIST:
if not block.added:
logger.debug('Have successfully added id=%d', id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id, block.period)
self.ed.send_packet(
pk, expected_reply=(CMD_START_LOGGING, id))
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when adding id=%d (%s)',
error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning('No LogEntry to assign block to !!!')
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully started logging for id=%d',
id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when starting id=%d (%s)',
error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(self, False)
# This is a temporary fix, we are adding a new issue
# for this. For some reason we get an error back after
# the block has been started and added. This will show
# an error in the UI, but everything is still working.
# block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully stopped logging for id=%d',
id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
# Accept deletion of a block that isn't added. This could
# happen due to timing (i.e add/start/delete in fast sequence)
if error_status == 0x00 or error_status == errno.ENOENT:
logger.info('Have successfully deleted id=%d', id)
if block:
block.started = False
block.added = False
if (cmd == CMD_RESET_LOGGING):
# Guard against multiple responses due to re-sending
if not self.toc:
logger.debug('Logging reset, continue with TOC download')
self.log_blocks = []
self.toc = Toc()
toc_fetcher = Toedetcher(self.ed, LogTocElement,
CRTPPort.LOGGING, self.toc,
self._refresh_callback,
self._toc_cache)
toc_fetcher.start()
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.data[0]
block = self._find_block(id)
timestamps = struct.unpack('<BBB', packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8
| timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning('Error no LogEntry to handle id=%d', id)
def request_echo_with_seq_nr(self, link, seq_nr):
pk = CRTPPacket()
pk.set_header(CRTPPort.LINKCTRL, self.ECHO)
pk.data = (seq_nr,)
link.send_packet(pk)
