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