Skip to content

Commit

Permalink
Closes #89: Implement new log/param commands to handle 64k variables
Browse files Browse the repository at this point in the history
  • Loading branch information
@ataffanel
ataffanel committed Aug 20, 2018
1 parent 2841a27 commit 4468582
Show file tree
Hide file tree
Showing 3 changed files with 117 additions and 47 deletions.
50 changes: 37 additions & 13 deletions cflib/crazyflie/log.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,8 +79,10 @@
CHAN_LOGDATA = 2

# Commands used when accessing the Table of Contents
CMD_TOC_ELEMENT = 0
CMD_TOC_INFO = 1
CMD_TOC_ELEMENT = 0 # original version: up to 255 entries
CMD_TOC_INFO = 1 # original version: up to 255 entries
CMD_GET_ITEM_V2 = 2 # version 2: up to 16k entries
CMD_GET_INFO_V2 = 3 # version 2: up to 16k entries

# Commands used when accessing the Log configurations
CMD_CREATE_BLOCK = 0
Expand All @@ -89,6 +91,8 @@
CMD_START_LOGGING = 3
CMD_STOP_LOGGING = 4
CMD_RESET_LOGGING = 5
CMD_CREATE_BLOCK_V2 = 6
CMD_APPEND_BLOCK_V2 = 7

# Possible states when receiving TOC
IDLE = 'IDLE'
Expand Down Expand Up @@ -150,8 +154,11 @@ def __init__(self, name, period_in_ms):
self.added_cb = Caller()
self.err_no = 0

# These 3 variables are set by the log subsystem when the bock is added
self.id = 0
self.cf = None
self.useV2 = False

self.period = int(period_in_ms / 10)
self.period_in_ms = period_in_ms
self._added = False
Expand Down Expand Up @@ -214,7 +221,10 @@ def create(self):
"""Save the log configuration in the Crazyflie"""
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.id)
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',
Expand All @@ -228,9 +238,18 @@ def create(self):
self.cf.log.toc.get_element_id(
var.name), var.get_storage_and_fetch_byte())
pk.data.append(var.get_storage_and_fetch_byte())
pk.data.append(self.cf.log.toc.get_element_id(var.name))
if self.useV2:
ident = self.cf.log.toc.get_element_id(var.name)
pk.data.append(ident & 0x0ff)
pk.data.append((ident >> 8) & 0x0ff)
else:
pk.data.append(self.cf.log.toc.get_element_id(var.name))
logger.debug('Adding log block id {}'.format(self.id))
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
if self.useV2:
self.cf.send_packet(pk, expected_reply=(
CMD_CREATE_BLOCK_V2, self.id))
else:
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))

def start(self):
"""Start the logging for this entry"""
Expand Down Expand Up @@ -337,21 +356,21 @@ def get_unpack_string_from_id(ident):
raise KeyError(
'Type [%d] not found in LogTocElement.types!' % ident)

def __init__(self, data=None):
def __init__(self, ident=0, data=None):
"""TocElement creator. Data is the binary payload of the element."""

self.ident = ident

if (data):
naming = data[2:]
naming = data[1:]
zt = bytearray((0, ))
self.group = naming[:naming.find(zt)].decode('ISO-8859-1')
self.name = naming[naming.find(zt) + 1:-1].decode('ISO-8859-1')

self.ident = data[0]

self.ctype = LogTocElement.get_cstring_from_id(data[1])
self.pytype = LogTocElement.get_unpack_string_from_id(data[1])
self.ctype = LogTocElement.get_cstring_from_id(data[0])
self.pytype = LogTocElement.get_unpack_string_from_id(data[0])

self.access = data[1] & 0x10
self.access = data[0] & 0x10


class Log():
Expand Down Expand Up @@ -386,6 +405,8 @@ def __init__(self, crazyflie=None):

self._config_id_counter = 1

self._useV2 = False

def add_config(self, logconf):
"""Add a log configuration to the logging framework.
Expand Down Expand Up @@ -436,6 +457,7 @@ def add_config(self, logconf):
logconf.valid = True
logconf.cf = self.cf
logconf.id = self._config_id_counter
logconf.useV2 = self._useV2
self._config_id_counter = (self._config_id_counter + 1) % 255
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
Expand All @@ -448,6 +470,8 @@ def add_config(self, logconf):
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""

self._useV2 = self.cf.platform.get_protocol_version() >= 4

self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
Expand All @@ -473,7 +497,7 @@ def _new_packet_cb(self, packet):
id = payload[0]
error_status = payload[1]
block = self._find_block(id)
if (cmd == CMD_CREATE_BLOCK):
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:
Expand Down
64 changes: 42 additions & 22 deletions cflib/crazyflie/param.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,9 @@
READ_CHANNEL = 1
WRITE_CHANNEL = 2

# TOC access command
TOC_RESET = 0
TOC_GETNEXT = 1
TOC_GETCRC32 = 2
# One element entry in the TOC


# One element entry in the TOC
class ParamTocElement:
"""An element in the Log TOC."""

Expand All @@ -88,10 +84,11 @@ class ParamTocElement:
0x06: ('float', '<f'),
0x07: ('double', '<d')}

def __init__(self, data=None):
def __init__(self, ident=0, data=None):
"""TocElement creator. Data is the binary payload of the element."""
self.ident = ident
if (data):
strs = struct.unpack('s' * len(data[2:]), data[2:])
strs = struct.unpack('s' * len(data[1:]), data[1:])
if sys.version_info < (3,):
strs = ('{}' * len(strs)).format(*strs).split('\0')
else:
Expand All @@ -102,12 +99,7 @@ def __init__(self, data=None):
self.group = strs[0]
self.name = strs[1]

if type(data[0]) == str:
self.ident = ord(data[0])
else:
self.ident = data[0]

metadata = data[1]
metadata = data[0]
if type(metadata) == str:
metadata = ord(metadata)

Expand All @@ -133,12 +125,14 @@ def __init__(self, crazyflie):
self.toc = Toc()

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

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

self.cf.disconnected.add_callback(self._disconnected)
Expand Down Expand Up @@ -169,10 +163,16 @@ def _check_if_all_updated(self):

def _param_updated(self, pk):
"""Callback with data for an updated parameter"""
var_id = pk.data[0]
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:
s = struct.unpack(element.pytype, pk.data[1:])[0]
if self._useV2:
s = struct.unpack(element.pytype, pk.data[2:])[0]
else:
s = struct.unpack(element.pytype, pk.data[2:])[0]
s = s.__str__()
complete_name = '%s.%s' % (element.group, element.name)

Expand Down Expand Up @@ -233,6 +233,7 @@ def refresh_toc(self, refresh_done_callback, toc_cache):
"""
Initiate a refresh of the parameter TOC.
"""
self._useV2 = self.cf.platform.get_protocol_version() >= 4
toc_fetcher = TocFetcher(self.cf, ParamTocElement,
CRTPPort.PARAM, self.toc,
refresh_done_callback, toc_cache)
Expand Down Expand Up @@ -271,7 +272,10 @@ def set_value(self, complete_name, value):
varid = element.ident
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, WRITE_CHANNEL)
pk.data = struct.pack('<B', varid)
if self._useV2:
pk.data = struct.pack('<H', varid)
else:
pk.data = struct.pack('<B', varid)
pk.data += struct.pack(element.pytype, eval(value))
self.param_updater.request_param_setvalue(pk)

Expand All @@ -280,12 +284,13 @@ class _ParamUpdater(Thread):
"""This thread will update params through a queue to make sure that we
get back values"""

def __init__(self, cf, updated_callback):
def __init__(self, cf, useV2, updated_callback):
"""Initialize the thread"""
Thread.__init__(self)
self.setDaemon(True)
self.wait_lock = Lock()
self.cf = cf
self._useV2 = useV2
self.updated_callback = updated_callback
self.request_queue = Queue()
self.cf.add_port_callback(CRTPPort.PARAM, self._new_packet_cb)
Expand All @@ -311,7 +316,12 @@ def request_param_setvalue(self, pk):
def _new_packet_cb(self, pk):
"""Callback for newly arrived packets"""
if pk.channel == READ_CHANNEL or pk.channel == WRITE_CHANNEL:
var_id = pk.data[0]
if self._useV2:
var_id = struct.unpack('<H', pk.data[:2])[0]
if pk.channel == READ_CHANNEL:
pk.data = pk.data[:2] + pk.data[3:]
else:
var_id = pk.data[0]
if (pk.channel != TOC_CHANNEL and self._req_param == var_id and
pk is not None):
self.updated_callback(pk)
Expand All @@ -323,9 +333,13 @@ def _new_packet_cb(self, pk):

def request_param_update(self, var_id):
"""Place a param update request on the queue"""
self._useV2 = self.cf.platform.get_protocol_version() >= 4
pk = CRTPPacket()
pk.set_header(CRTPPort.PARAM, READ_CHANNEL)
pk.data = struct.pack('<B', var_id)
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)

Expand All @@ -334,7 +348,13 @@ def run(self):
pk = self.request_queue.get() # Wait for request update
self.wait_lock.acquire()
if self.cf.link:
self._req_param = pk.data[0]
self.cf.send_packet(pk, expected_reply=(tuple(pk.data[0:2])))
if self._useV2:
self._req_param = struct.unpack('<H', pk.data[:2])[0]
self.cf.send_packet(
pk, expected_reply=(tuple(pk.data[:2])))
else:
self._req_param = pk.data[0]
self.cf.send_packet(
pk, expected_reply=(tuple(pk.data[:1])))
else:
self.wait_lock.release()
50 changes: 38 additions & 12 deletions cflib/crazyflie/toc.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,10 @@
TOC_CHANNEL = 0

# Commands used when accessing the Table of Contents
CMD_TOC_ELEMENT = 0
CMD_TOC_INFO = 1
CMD_TOC_ELEMENT = 0 # original version: up to 255 entries
CMD_TOC_INFO = 1 # original version: up to 255 entries
CMD_TOC_ITEM_V2 = 2 # version 2: up to 16k entries
CMD_TOC_INFO_V2 = 3 # version 2: up to 16k entries

# Possible states when receiving TOC
IDLE = 'IDLE'
Expand Down Expand Up @@ -121,9 +123,14 @@ def __init__(self, crazyflie, element_class, port, toc_holder,
self._toc_cache = toc_cache
self.finished_callback = finished_callback
self.element_class = element_class
self._useV2 = False

def start(self):
"""Initiate fetching of the TOC."""
self._useV2 = self.cf.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.cf.add_port_callback(self.port, self._new_packet_cb)
Expand All @@ -132,8 +139,12 @@ def start(self):
self.state = GET_TOC_INFO
pk = CRTPPacket()
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_INFO,)
self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO,))
if self._useV2:
pk.data = (CMD_TOC_INFO_V2,)
self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO_V2,))
else:
pk.data = (CMD_TOC_INFO,)
self.cf.send_packet(pk, expected_reply=(CMD_TOC_INFO,))

def _toc_fetch_finished(self):
"""Callback for when the TOC fetching is finished"""
Expand All @@ -149,7 +160,12 @@ def _new_packet_cb(self, packet):
payload = packet.data[1:]

if (self.state == GET_TOC_INFO):
[self.nbr_of_items, self._crc] = struct.unpack('<BI', payload[:5])
if self._useV2:
[self.nbr_of_items, self._crc] = struct.unpack(
'<HI', payload[:6])
else:
[self.nbr_of_items, self._crc] = struct.unpack(
'<BI', payload[:5])
logger.debug('[%d]: Got TOC CRC, %d items and crc=0x%08X',
self.port, self.nbr_of_items, self._crc)

Expand All @@ -166,11 +182,15 @@ def _new_packet_cb(self, packet):
elif (self.state == GET_TOC_ELEMENT):
# Always add new element, but only request new if it's not the
# last one.
if self.requested_index != payload[0]:
if self._useV2:
ident = struct.unpack('<H', payload[:2])[0]
else:
ident = payload[0]

if ident != self.requested_index:
return
self.toc.add_element(self.element_class(payload))
logger.debug('Added element [%s]',
self.element_class(payload).ident)
self.toc.add_element(self.element_class(ident, payload[2:]))
logger.debug('Added element [%s]', ident)
if (self.requested_index < (self.nbr_of_items - 1)):
logger.debug('[%d]: More variables, requesting index %d',
self.port, self.requested_index + 1)
Expand All @@ -184,6 +204,12 @@ 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()
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ELEMENT, index)
self.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))
if self._useV2:
pk.set_header(self.port, TOC_CHANNEL)
pk.data = (CMD_TOC_ITEM_V2, index & 0x0ff, (index >> 8) & 0x0ff)
self.cf.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.cf.send_packet(pk, expected_reply=(CMD_TOC_ELEMENT, index))

0 comments on commit 4468582

Please sign in to comment.