class Console:
"""
Crazyflie console is used to receive characters printed using printf
from the firmware.
"""
def __init__(self, crazyflie):
"""
Initialize the console and register it to receive data from the copter.
"""
self.receivedChar = Caller()
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming)
#print("Console init finished")
def incoming(self, packet):
"""
Callback for data received from the copter.
"""
#print("Console incoming")
# This might be done prettier ;-)
console_text = packet.data.decode('UTF-8')
# leonana: add print for console information
#print(console_text, end='', flush=True)
self.receivedChar.call(console_text)
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 PluginHelper():
"""Used for passing objects to tabs and toolboxes"""
def __init__(self):
self.cf = None
self.menu = None
self.logConfigReader = None
self.referenceHeight = 0.400
self.hover_input_updated = Caller()
self.useReferenceHeight = False
self.inputType = 0
self.inputTimer = 0.000
self.sinewaveFrequency = 1.0 # 1s
self.mainUI = None
self.plotTab = None
self.pose_logger = None
self.connectivity_manager = None
self.init_time = time.time()
def send_hover_setpoint(self, vy, vx, yawrate, height):
if self.useReferenceHeight:
if (self.inputType == 0): # Step Input
self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight)
if (self.inputType == 1): # Sine Wave
self.hover_input_updated.call(vy, vx, yawrate, self.sine_wave_generator())
if (self.inputType == 2): # Ramp
self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight)
print(self.sine_wave_generator())
else:
self.hover_input_updated.call(vy, vx, yawrate, height)
def sine_wave_generator(self):
output = 0.25 * sin(2.00 * pi * self.sinewaveFrequency * (time.time() - self.init_time)) + 0.5
# output = output + self.inputTimer + 0.001
return output
class PluginHelper():
"""Used for passing objects to tabs and toolboxes"""
def __init__(self):
self.cf = None
self.menu = None
self.logConfigReader = None
self.referenceHeight = 0.400
self.hover_input_updated = Caller()
self.useReferenceHeight = False
self.inputType = 0
self.inputTimer = 0.000
self.sinewaveFrequency = 1.0 # 1s
def send_hover_setpoint(self, vy, vx, yawrate, height):
if self.useReferenceHeight:
if (self.inputType == 0): # Step Input
self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight)
if (self.inputType == 1): # Sine Wave
self.hover_input_updated.call(vy, vx, yawrate, self.sine_wave_generator())
if (self.inputType == 2): # Ramp
self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight)
#else:
#self.hover_input_updated.call(vy, vx, yawrate, self.referenceHeight)
else:
self.hover_input_updated.call(vy, vx, yawrate, height)
def sine_wave_generator(self):
output = 0.2 * sin(2.00 * pi * self.sinewaveFrequency * self.inputTimer)
output = output + self.referenceHeight
self.inputTimer = self.inputTimer + 0.001
return output
class Appchannel:
def __init__(self, crazyflie):
self._cf = crazyflie
self.packet_received = Caller()
self._cf.add_port_callback(CRTPPort.PLATFORM, self._incoming)
def send_packet(self, data):
packet = CRTPPacket()
packet.port = CRTPPort.PLATFORM
packet.channel = cflib.crazyflie.platformservice.APP_CHANNEL
packet.data = data
self._cf.send_packet(packet)
def _incoming(self, packet: CRTPPacket):
if packet.channel == cflib.crazyflie.platformservice.APP_CHANNEL:
self.packet_received.call(packet.data)
class Console:
"""
Crazyflie console is used to receive characters printed using printf
from the firmware.
"""
def __init__(self, crazyflie):
"""
Initialize the console and register it to receive data from the copter.
"""
self.receivedChar = Caller()
"""
This member variable is used to setup a callback that will be called
when text is received from the CONSOLE port of CRTP (0).
Example:
```python
[...]
def log_console(self, text):
self.log_file.write(text)
[...]
self.cf.console.receivedChar.add_callback(self.log_console)
```
"""
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.CONSOLE, self._incoming)
def _incoming(self, packet):
"""
Callback for data received from the copter.
"""
# This might be done prettier ;-)
console_text = packet.data.decode('UTF-8')
self.receivedChar.call(console_text)
class Console:
"""
Crazyflie console is used to receive characters printed using printf
from the firmware.
"""
def __init__(self, crazyflie):
"""
Initialize the console and register it to receive data from the copter.
"""
self.receivedChar = Caller()
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming)
def incoming(self, packet):
"""
Callback for data received from the copter.
"""
# This might be done prettier ;-)
console_text = packet.data.decode("UTF-8")
self.receivedChar.call(console_text)
class Log():
"""Create log configuration"""
# 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):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
self._refresh_callback = None
self._toc_cache = None
def add_config(self, logconf):
"""Add a log configuration to the logging framework.
When doing this the contents of the log configuration will be validated
and listeners for new log configurations will be notified. When
validating the configuration the variables are checked against the TOC
to see that they actually exist. If they don't then the configuration
cannot be used. Since a valid TOC is required, a Crazyflie has to be
connected when calling this method, otherwise it will fail."""
if not self.cf.link:
logger.error('Cannot add configs without being connected to a '
'Crazyflie!')
return
# If the log configuration contains variables that we added without
# type (i.e we want the stored as type for fetching as well) then
# resolve this now and add them to the block again.
for name in logconf.default_fetch_as:
var = self.toc.get_element_by_complete_name(name)
if not var:
logger.warning('%s not in TOC, this block cannot be used!',
name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(name))
# Now that we know what type this variable has, add it to the log
# config again with the correct type
logconf.add_variable(name, var.ctype)
# Now check that all the added variables are in the TOC and that
# the total size constraint of a data packet with logging data is
# not
size = 0
for var in logconf.variables:
size += LogTocElement.get_size_from_id(var.fetch_as)
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if var.is_toc_variable():
if (self.toc.get_element_by_complete_name(var.name) is None):
logger.warning(
'Log: %s not in TOC, this block cannot be used!',
var.name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(var.name))
if (size <= MAX_LOG_DATA_PACKET_SIZE
and (logconf.period > 0 and logconf.period < 0xFF)):
logconf.valid = True
logconf.cf = self.cf
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
else:
logconf.valid = False
raise AttributeError(
'The log configuration is too large or has an invalid '
'parameter')
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING, ))
def _find_block(self, id):
for block in self.log_blocks:
if block.id == id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if (chan == CHAN_SETTINGS):
id = payload[0]
error_status = payload[1]
block = self._find_block(id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if error_status == 0 or error_status == errno.EEXIST:
if not block.added:
logger.debug('Have successfully added id=%d', id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id, block.period)
self.cf.send_packet(
pk, expected_reply=(CMD_START_LOGGING, id))
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when adding id=%d (%s)',
error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning('No LogEntry to assign block to !!!')
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully started logging for id=%d',
id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when starting id=%d (%s)',
error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(self, False)
# This is a temporary fix, we are adding a new issue
# for this. For some reason we get an error back after
# the block has been started and added. This will show
# an error in the UI, but everything is still working.
# block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully stopped logging for id=%d',
id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
# Accept deletion of a block that isn't added. This could
# happen due to timing (i.e add/start/delete in fast sequence)
if error_status == 0x00 or error_status == errno.ENOENT:
logger.info('Have successfully deleted id=%d', id)
if block:
block.started = False
block.added = False
if (cmd == CMD_RESET_LOGGING):
# Guard against multiple responses due to re-sending
if not self.toc:
logger.debug('Logging reset, continue with TOC download')
self.log_blocks = []
self.toc = Toc()
toc_fetcher = TocFetcher(self.cf, LogTocElement,
CRTPPort.LOGGING, self.toc,
self._refresh_callback,
self._toc_cache)
toc_fetcher.start()
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.data[0]
block = self._find_block(id)
timestamps = struct.unpack('<BBB', packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8
| timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning('Error no LogEntry to handle id=%d', id)
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 JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
self._input_device = None
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self.max_rp_angle = 0
self.max_yaw_rate = 0
self._old_thrust = 0
self._old_raw_thrust = 0
self._old_alt_hold = False
self.springy_throttle = True
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._input_map = None
self._mux = [
NoMux(self),
TakeOverSelectiveMux(self),
TakeOverMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(cfclient.module_path +
"/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def enable_alt_hold(self, althold):
"""Enable or disable altitude hold"""
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons,
mapped_values] = self._input_device.read(include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._input_map = ConfigManager().get_config(input_map_name)
self._get_device_from_name(device_name).input_map = self._input_map
self._get_device_from_name(device_name).input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp, device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.althold:
try:
self.althold_updated.call(str(data.althold))
except Exception as e:
logger.warning(
"Exception while doing callback from input-device "
"for althold: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= 1
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += 1
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= 1
if data.toggled.rollPos and data.rollPos:
self.trim_roll += 1
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll, self.trim_pitch)
# Thrust might be <0 here, make sure it's not otherwise we'll
# get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
# If we are using alt hold the data is not in a percentage
if not data.althold:
data.thrust = JoystickReader.p2t(data.thrust)
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch, data.yaw,
data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
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 LogEntry(object):
"""Representation of one log configuration that enables logging
from the Crazyflie"""
block_idCounter = 1
def __init__(self, crazyflie, logconf):
"""Initialize the entry"""
self.data_received = Caller()
self.error = Caller()
self.started_cb = Caller()
self.added_cb = Caller()
self.err_no = 0
self.logconf = logconf
self.block_id = LogEntry.block_idCounter
LogEntry.block_idCounter += 1 % 255
self.cf = crazyflie
self.period = logconf.getPeriod() / 10
self.period_in_ms = logconf.getPeriod()
self._added = False
self._started = False
def _set_added(self, added):
self._added = added
self.added_cb.call(added)
def _get_added(self):
return self._added
def _set_started(self, started):
self._started = started
self.started_cb.call(started)
def _get_started(self):
return self._started
added = property(_get_added, _set_added)
started = property(_get_started, _set_started)
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
logger.debug("First time block is started, add block")
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.block_id)
for var in self.logconf.getVariables():
if (var.isTocVariable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
var.getStoredFetchAs(), var.getAddress())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<I', var.getAddress())
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.getName(),
self.cf.log.toc.get_element_id(
var.getName()), var.getStoredFetchAs())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(var.getName()))
logger.debug("Adding log block id {}".format(self.block_id))
self.cf.send_packet(pk)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.block_id, self.period)
self.cf.send_packet(pk)
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.link is not None):
if (self.block_id is None):
logger.warning("Stopping block, but no block registered")
else:
logger.debug("Sending stop logging for block %d", self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.block_id)
self.cf.send_packet(pk)
def close(self):
"""Delete this entry in the Crazyflie"""
if (self.cf.link is not None):
if (self.block_id is None):
logger.warning("Delete block, but no block registered")
else:
logger.debug("LogEntry: Sending delete logging for block %d"
% self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.block_id)
self.cf.send_packet(pk)
self.block_id = None # Wait until we get confirmation of delete
def unpack_log_data(self, log_data, timestamp):
"""Unpack received logging data so it represent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.logconf.getVariables():
size = LogTocElement.get_size_from_id(var.getFetchAs())
name = var.getName()
unpackstring = LogTocElement.get_unpack_string_from_id(
var.getFetchAs())
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received.call(ret_data, timestamp)
class FreeFallRecovery(QtCore.QObject):
""" Class to recover from detected freefall"""
sigRecoveryTimedOut = pyqtSignal()
def __init__(self, parent=None):
super(QtCore.QObject, self).__init__(parent)
self.parent = parent
self.useBaro = False # Using barometer for current recovery
self.useBaroNext = False # Use barometer for next recovery
self.falling = False # Free fall Recovery active
self.kill = False
self.boostMSec = 45 # Max thrust for first 30ms. State also keeps track of mode, see step()
self.nr = 0 # step nr during non baro mode
self.falloff = FallOff(self)
# why the hell do we mix signals like this?
self.auto_input_updated = Caller()
self.althold_updated = Caller()
self.timerOut = QtCore.QTimer()
def setUseBaro(self, on):
""" If we should use the barometer (ie hover mode) to help recover or not for the next fall"""
msg = " next " if self.falling else " "
if self.useBaroNext != on:
logger.info("Using Barometer for" + msg +
"freefall recovery" if on else "Not using Baro for" +
msg + "freefall recovery")
self.useBaroNext = on
def step(self, rollTrimmed, pitchTrimmed, yaw, thrust):
"""Here we handle our recover; called by the joy driver @ 100hz. We mostly pass everything through, but we modify the throttle"""
if self.useBaro > 0:
# Barometer Mode
if self.useBaro == 1:
# Loop Hover mode
thrust = int(round(-0.5 * 32767 + 32767))
elif self.useBaro == 2:
# Set hover mode
self.useBaro -= 1
self.enableBaro()
thrust = int(round(-0.5 * 32767 + 32767))
else:
# Boost mode
self.useBaro -= 1
thrust = JoystickReader.p2t(min(99, 70 + self.useBaro * 1.25))
logger.info("Thrust: %d", thrust)
else:
# Regular mode
self.nr += 1
thrust = JoystickReader.p2t(self.falloff.f(
self.nr)) #if thrust>0.05 else 0
self.auto_input_updated.call(rollTrimmed, pitchTrimmed, yaw, thrust)
def startRecovery(self):
if self.kill:
logger.info("Cannot start freefall recovery due to killswitch")
else:
logger.info("Starting freefall recovery")
self.falling = True
self.useBaro = self.boostMSec = 30 if self.useBaroNext else 0
# Start the timeout and the handler
if self.useBaro:
self.althold_updated.call("altHold.altHoldErrMax", 260.0)
self.timerOut.singleShot(
3000,
self.setTimedOut) # 6 second for recovery with barometer
#self.enableBaro()
else:
self.nr = 0
self.timerOut.singleShot(
2000, self.setTimedOut
) # 2 second for recovery without barometer
def setKillSwitch(self, on):
self.kill = on
if on:
self.abort()
def abort(self):
""" We have landed, so stop recovering"""
if self.cleanUp():
self.sigRecoveryTimedOut.emit()
logger.info("Aborted Recovery")
def setLanded(self):
""" We have landed, so stop recovering"""
if self.cleanUp():
logger.info("Recovery stopped due to landing")
def setTimedOut(self):
""" Recovering times out before we detected a landing"""
if self.cleanUp():
logger.info("Recovery stopped due to timeout")
self.sigRecoveryTimedOut.emit()
def cleanUp(self):
if self.falling:
self.falling = False
self.timerOut.stop()
if self.useBaro:
self.disableBaro()
return True
return False
def enableBaro(self):
# Make change in altitude stronger
self.althold_updated.call("altHold.altHoldErrMax", 260.0)
self.althold_updated.call("altHold.altHoldChangeSens", 50.0)
self.althold_updated.call("flightmode.althold", True)
def disableBaro(self):
self.althold_updated.call("flightmode.althold", False)
self.althold_updated.call("altHold.altHoldErrMax", 1.0)
self.althold_updated.call("altHold.altHoldChangeSens", 200.0)
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 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 CallerTest(unittest.TestCase):
def setUp(self):
self.callback_count = 0
self.sut = Caller()
def test_that_callback_is_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_callback_is_added_only_one_time(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(1, self.callback_count)
def test_that_multiple_callbacks_are_added(self):
# Fixture
# Test
self.sut.add_callback(self._callback)
self.sut.add_callback(self._callback2)
# Assert
self.sut.call()
self.assertEqual(2, self.callback_count)
def test_that_callback_is_removed(self):
# Fixture
self.sut.add_callback(self._callback)
# Test
self.sut.remove_callback(self._callback)
# Assert
self.sut.call()
self.assertEqual(0, self.callback_count)
def test_that_callback_is_called_with_arguments(self):
# Fixture
self.sut.add_callback(self._callback_with_args)
# Test
self.sut.call('The token')
# Assert
self.assertEqual('The token', self.callback_token)
def _callback(self):
self.callback_count += 1
def _callback2(self):
self.callback_count += 1
def _callback_with_args(self, token):
self.callback_token = token
class Memory():
"""Access memories on the Crazyflie"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very 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 Log():
"""Create log configuration"""
# 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"
}
def __init__(self, crazyflie=None):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
def add_config(self, logconf):
"""Add a log configuration to the logging framework.
When doing this the contents of the log configuration will be validated
and listeners for new log configurations will be notified. When
validating the configuration the variables are checked against the TOC
to see that they actually exist. If they don't then the configuration
cannot be used. Since a valid TOC is required, a Crazyflie has to be
connected when calling this method, otherwise it will fail."""
if not self.cf.link:
logger.error("Cannot add configs without being connected to a "
"Crazyflie!")
return
# If the log configuration contains variables that we added without
# type (i.e we want the stored as type for fetching as well) then
# resolve this now and add them to the block again.
for name in logconf.default_fetch_as:
var = self.toc.get_element_by_complete_name(name)
if not var:
logger.warning("%s not in TOC, this block cannot be"
" used!", name)
logconf.valid = False
return
# Now that we know what type this variable has, add it to the log
# config again with the correct type
logconf.add_variable(name, var.ctype)
# Now check that all the added variables are in the TOC and that
# the total size constraint of a data packet with logging data is
# not
size = 0
for var in logconf.variables:
size += LogTocElement.get_size_from_id(var.fetch_as)
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if var.is_toc_variable():
if (self.toc.get_element_by_complete_name(var.name) is None):
logger.warning(
"Log: %s not in TOC, this block cannot be"
" used!", var.name)
logconf.valid = False
return
if (size <= MAX_LOG_DATA_PACKET_SIZE
and (logconf.period > 0 and logconf.period < 0xFF)):
logconf.valid = True
logconf.cf = self.cf
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
else:
logconf.valid = False
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.cf.send_packet(pk, expect_answer=True)
self.log_blocks = []
self.toc = Toc()
toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
self.toc, refresh_done_callback, toc_cache)
toc_fetcher.start()
def _find_block(self, id):
for block in self.log_blocks:
if block.id == id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.datal[0]
payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
if (chan == CHAN_SETTINGS):
id = ord(payload[0])
error_status = ord(payload[1])
block = self._find_block(id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if (error_status == 0): # No error
logger.debug("Have successfully added id=%d", id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id, block.period)
self.cf.send_packet(pk, expect_answer=True)
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when adding id=%d (%s)",
error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning("No LogEntry to assign block to !!!")
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info(
"Have successfully started logging for block=%d", id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when starting id=%d (%s)",
error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(False)
block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info(
"Have successfully stopped logging for block=%d", id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
if (error_status == 0x00):
logger.info("Have successfully deleted block=%d", id)
if block:
block.started = False
block.added = False
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.datal[0]
block = self._find_block(id)
timestamps = struct.unpack("<BBB", packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8
| timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning("Error no LogEntry to handle id=%d", id)
class PoseLogger:
LOG_NAME_ESTIMATE_X = 'stateEstimate.x'
LOG_NAME_ESTIMATE_Y = 'stateEstimate.y'
LOG_NAME_ESTIMATE_Z = 'stateEstimate.z'
LOG_NAME_ESTIMATE_ROLL = 'stateEstimate.roll'
LOG_NAME_ESTIMATE_PITCH = 'stateEstimate.pitch'
LOG_NAME_ESTIMATE_YAW = 'stateEstimate.yaw'
NO_POSE = (0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
def __init__(self, cf: Crazyflie) -> None:
self._cf = cf
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self.data_received_cb = Caller()
self.error_cb = Caller()
# The pose is an array containing
# X, Y, Z, roll, pitch, yaw
# roll, pitch and yaw in degrees
self.pose = self.NO_POSE
@property
def position(self):
"""Get the position part of the full pose"""
return self.pose[0:3]
@property
def rpy(self):
"""Get the roll, pitch and yaw of the full pose in degrees"""
return self.pose[3:6]
@property
def rpy_rad(self):
"""Get the roll, pitch and yaw of the full pose in radians"""
return [
math.radians(self.pose[3]),
math.radians(self.pose[4]),
math.radians(self.pose[5])
]
def _connected(self, link_uri) -> None:
logConf = LogConfig("Pose", 40)
logConf.add_variable(self.LOG_NAME_ESTIMATE_X, "float")
logConf.add_variable(self.LOG_NAME_ESTIMATE_Y, "float")
logConf.add_variable(self.LOG_NAME_ESTIMATE_Z, "float")
logConf.add_variable(self.LOG_NAME_ESTIMATE_ROLL, "float")
logConf.add_variable(self.LOG_NAME_ESTIMATE_PITCH, "float")
logConf.add_variable(self.LOG_NAME_ESTIMATE_YAW, "float")
try:
self._cf.log.add_config(logConf)
logConf.data_received_cb.add_callback(self._data_received)
logConf.error_cb.add_callback(self._error)
logConf.start()
except KeyError as e:
logger.warning(str(e))
except AttributeError as e:
logger.warning(str(e))
def _disconnected(self, link_uri) -> None:
self.pose = self.NO_POSE
def _data_received(self, timestamp, data, logconf) -> None:
self.pose = (
data[self.LOG_NAME_ESTIMATE_X],
data[self.LOG_NAME_ESTIMATE_Y],
data[self.LOG_NAME_ESTIMATE_Z],
data[self.LOG_NAME_ESTIMATE_ROLL],
data[self.LOG_NAME_ESTIMATE_PITCH],
data[self.LOG_NAME_ESTIMATE_YAW],
)
self.data_received_cb.call(self, self.pose)
def _error(self, log_conf, msg) -> None:
self.error_cb.call(self, msg)
class Localization():
"""
Handle localization-related data communication with the Crazyflie
"""
# Implemented channels
POSITION_CH = 0
GENERIC_CH = 1
# Location message types for generig channel
RANGE_STREAM_REPORT = 0x00
RANGE_STREAM_REPORT_FP16 = 0x01
LPS_SHORT_LPP_PACKET = 0x02
def __init__(self, crazyflie=None):
"""
Initialize the Extpos object.
"""
self._cf = crazyflie
self.receivedLocationPacket = Caller()
self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def _incoming(self, packet):
"""
Callback for data received from the copter.
"""
if len(packet.data) < 1:
logger.warning('Localization packet received with incorrect' +
'length (length is {})'.format(len(packet.data)))
return
pk_type = struct.unpack('<B', packet.data[:1])[0]
data = packet.data[1:]
# Decoding the known packet types
# TODO: more generic decoding scheme?
decoded_data = None
if pk_type == self.RANGE_STREAM_REPORT:
if len(data) % 5 != 0:
logger.error('Wrong range stream report data lenght')
return
decoded_data = {}
raw_data = data
for i in range(int(len(data) / 5)):
anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
decoded_data[anchor_id] = distance
raw_data = raw_data[5:]
pk = LocalizationPacket(pk_type, data, decoded_data)
self.receivedLocationPacket.call(pk)
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._cf.send_packet(pk)
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._cf.send_packet(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 JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
self._input_device = None
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_raw_thrust = 0
self._old_alt_hold = False
self._springy_throttle = True
self._prev_values = {}
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
self._input_map = None
self._mux = [
NoMux(self),
SelectiveMux(self),
TakeOverMux(self),
MixMux(self),
TakeOverSelectiveMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
if Config().get("flightmode") is "Normal":
self.set_yaw_limit(Config().get("normal_max_yaw"))
self.set_rp_limit(Config().get("normal_max_rp"))
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self.set_yaw_limit(Config().get("max_yaw"))
self.set_rp_limit(Config().get("max_rp"))
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("min_thrust"),
Config().get("max_thrust"))
self.set_thrust_slew_limiting(Config().get("slew_rate"),
Config().get("slew_limit"))
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self._has_pressure_sensor = available
def enable_alt_hold(self, althold):
"""Enable or disable altitude hold"""
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.available_devices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
available = []
for m in self._mux:
available.append(m.name)
return available
def set_mux(self, name=None, mux=None):
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def get_mux_supported_dev_count(self):
return self._selected_mux.get_supported_dev_count()
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev.name))):
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in device_config_mapping.keys():
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons,
mapped_values] = self._input_device.read(include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
settings = ConfigManager().get_settings(input_map_name)
if settings:
self._springy_throttle = settings["springythrottle"]
self._input_map = ConfigManager().get_config(input_map_name)
if self._input_device:
self._input_device.input_map = self._input_map
Config().get("device_config_mapping")[device_name] = input_map_name
def get_device_name(self):
"""Get the name of the current open device"""
if self._input_device:
return self._input_device.name
return None
def start_input(self, device_name, config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
#device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
for d in readers.devices():
if d.name == device_name:
self._input_device = d
if not config_name:
config_name = self.get_saved_device_mapping(
device_name)
self.set_input_map(device_name, config_name)
self._input_device.open()
self._input_device.input_map = self._input_map
self._input_device.input_map_name = config_name
self._selected_mux.add_device(self._input_device, None)
# Update the UI with the limiting for this device
self.limiting_updated.call(self._input_device.limit_rp,
self._input_device.limit_yaw,
self._input_device.limit_thrust)
self._read_timer.start()
return self._input_device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def stop_input(self, device_name=None):
"""Stop reading from the input device."""
if device_name:
for d in readers.devices():
if d.name == device_name:
d.close()
else:
for d in readers.devices():
d.close()
#if self._input_device:
# self._input_device.close()
# self._input_device = None
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
for m in self._mux:
m.max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
for m in self._mux:
m.max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
for m in self._mux:
m.thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
m.thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if thrust_slew_rate > 0:
m.thrust_slew_enabled = True
else:
m.thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
for m in self._mux:
m.min_thrust = JoystickReader.p2t(min_thrust)
m.max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
for m in self._mux:
m.trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
for m in self._mux:
m.trim_pitch = trim_pitch
def _calc_rp_trim(self, key_neg, key_pos, data):
if self._check_toggle(key_neg, data) and not data[key_neg]:
return -1.0
if self._check_toggle(key_pos, data) and not data[key_pos]:
return 1.0
return 0.0
def _check_toggle(self, key, data):
if not key in self._prev_values:
self._prev_values[key] = data[key]
elif self._prev_values[key] != data[key]:
self._prev_values[key] = data[key]
return True
return False
def read_input(self):
"""Read input data from the selected device"""
try:
[roll, pitch, yaw, thrust] = self._selected_mux.read()
#if trim_roll != 0 or trim_pitch != 0:
# self._trim_roll += trim_roll
# self._trim_pitch += trim_pitch
# self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
#trimmed_roll = roll + self._trim_roll
#trimmed_pitch = pitch + self._trim_pitch
# Thrust might be <0 here, make sure it's not otherwise we'll get an error.
if thrust < 0:
thrust = 0
if thrust > 65535:
thrust = 65535
self.input_updated.call(roll, pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value / (1 - threshold)
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 LogEntry:
"""Representation of one log configuration that enables logging
from the Crazyflie"""
block_idCounter = 1
def __init__(self, crazyflie, logconf):
"""Initialize the entry"""
self.data_received = Caller()
self.error = Caller()
self.logconf = logconf
self.block_id = LogEntry.block_idCounter
LogEntry.block_idCounter += 1
self.cf = crazyflie
self.period = logconf.getPeriod() / 10
self.block_created = False
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self.block_created is False):
logger.debug("First time block is started, add block")
self.block_created = True
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.block_id)
for var in self.logconf.getVariables():
if (var.isTocVariable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
var.getStoredFetchAs(), var.getAddress())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<I', var.getAddress())
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.getName(),
self.cf.log.toc.get_element_id(
var.getName()), var.getStoredFetchAs())
pk.data += struct.pack('<B', var.getStoredFetchAs())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(var.getName()))
logger.debug("Adding log block id {}".format(self.block_id))
self.cf.send_packet(pk)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.block_id, self.period)
self.cf.send_packet(pk)
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.link is not None):
if (self.block_id is None):
logger.warning("Stopping block, but no block registered")
else:
logger.debug("Sending stop logging for block %d", self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.block_id)
self.cf.send_packet(pk)
def close(self):
"""Delete this entry in the Crazyflie"""
if (self.cf.link is not None):
if (self.block_id is None):
logger.warning("Delete block, but no block registered")
else:
logger.debug("LogEntry: Sending delete logging for block %d"
% self.block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.block_id)
self.cf.send_packet(pk)
self.block_id = None # Wait until we get confirmation of delete
def unpack_log_data(self, log_data):
"""Unpack received logging data so it represent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.logconf.getVariables():
size = LogTocElement.get_size_from_id(var.getFetchAs())
name = var.getName()
unpackstring = LogTocElement.get_unpack_string_from_id(
var.getFetchAs())
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received.call(ret_data)
class Localization():
"""
Handle localization-related data communication with the Crazyflie
"""
# Implemented channels
POSITION_CH = 0
GENERIC_CH = 1
# Location message types for generig channel
RANGE_STREAM_REPORT = 0x00
RANGE_STREAM_REPORT_FP16 = 0x01
LPS_SHORT_LPP_PACKET = 0x02
def __init__(self, crazyflie=None):
"""
Initialize the Extpos object.
"""
self._cf = crazyflie
self.receivedLocationPacket = Caller()
self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def _incoming(self, packet):
"""
Callback for data received from the copter.
"""
if len(packet.data) < 1:
logger.warning('Localization packet received with incorrect' +
'length (length is {})'.format(len(packet.data)))
return
pk_type = struct.unpack('<B', packet.data[:1])[0]
data = packet.data[1:]
# Decoding the known packet types
# TODO: more generic decoding scheme?
decoded_data = None
if pk_type == self.RANGE_STREAM_REPORT:
if len(data) % 5 != 0:
logger.error('Wrong range stream report data lenght')
return
decoded_data = {}
raw_data = data
for i in range(int(len(data) / 5)):
anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
decoded_data[anchor_id] = distance
raw_data = raw_data[5:]
pk = LocalizationPacket(pk_type, data, decoded_data)
self.receivedLocationPacket.call(pk)
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie's position estimator.
"""crazyflie
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._cf.send_packet(pk)
## print("here\n")
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._cf.send_packet(pk)
class Log():
"""Create log configuration"""
# 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):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
self._refresh_callback = None
self._toc_cache = None
self._config_id_counter = 1
def add_config(self, logconf):
"""Add a log configuration to the logging framework.
When doing this the contents of the log configuration will be validated
and listeners for new log configurations will be notified. When
validating the configuration the variables are checked against the TOC
to see that they actually exist. If they don't then the configuration
cannot be used. Since a valid TOC is required, a Crazyflie has to be
connected when calling this method, otherwise it will fail."""
if not self.cf.link:
logger.error('Cannot add configs without being connected to a '
'Crazyflie!')
return
# If the log configuration contains variables that we added without
# type (i.e we want the stored as type for fetching as well) then
# resolve this now and add them to the block again.
for name in logconf.default_fetch_as:
var = self.toc.get_element_by_complete_name(name)
if not var:
logger.warning(
'%s not in TOC, this block cannot be used!', name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(name))
# Now that we know what type this variable has, add it to the log
# config again with the correct type
logconf.add_variable(name, var.ctype)
# Now check that all the added variables are in the TOC and that
# the total size constraint of a data packet with logging data is
# not
size = 0
for var in logconf.variables:
size += LogTocElement.get_size_from_id(var.fetch_as)
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if var.is_toc_variable():
if (self.toc.get_element_by_complete_name(var.name) is None):
logger.warning(
'Log: %s not in TOC, this block cannot be used!',
var.name)
logconf.valid = False
raise KeyError('Variable {} not in TOC'.format(var.name))
if (size <= MAX_LOG_DATA_PACKET_SIZE and
(logconf.period > 0 and logconf.period < 0xFF)):
logconf.valid = True
logconf.cf = self.cf
logconf.id = self._config_id_counter
self._config_id_counter = (self._config_id_counter + 1) % 255
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
else:
logconf.valid = False
raise AttributeError(
'The log configuration is too large or has an invalid '
'parameter')
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
self._toc_cache = toc_cache
self._refresh_callback = refresh_done_callback
self.toc = None
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING,)
self.cf.send_packet(pk, expected_reply=(CMD_RESET_LOGGING,))
def _find_block(self, id):
for block in self.log_blocks:
if block.id == id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if (chan == CHAN_SETTINGS):
id = payload[0]
error_status = payload[1]
block = self._find_block(id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if error_status == 0 or error_status == errno.EEXIST:
if not block.added:
logger.debug('Have successfully added id=%d', id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id, block.period)
self.cf.send_packet(pk, expected_reply=(
CMD_START_LOGGING, id))
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when adding id=%d (%s)',
error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning('No LogEntry to assign block to !!!')
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully started logging for id=%d',
id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning('Error %d when starting id=%d (%s)',
error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(self, False)
# This is a temporary fix, we are adding a new issue
# for this. For some reason we get an error back after
# the block has been started and added. This will show
# an error in the UI, but everything is still working.
# block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info('Have successfully stopped logging for id=%d',
id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
# Accept deletion of a block that isn't added. This could
# happen due to timing (i.e add/start/delete in fast sequence)
if error_status == 0x00 or error_status == errno.ENOENT:
logger.info('Have successfully deleted id=%d', id)
if block:
block.started = False
block.added = False
if (cmd == CMD_RESET_LOGGING):
# Guard against multiple responses due to re-sending
if not self.toc:
logger.debug('Logging reset, continue with TOC download')
self.log_blocks = []
self.toc = Toc()
toc_fetcher = TocFetcher(self.cf, LogTocElement,
CRTPPort.LOGGING,
self.toc, self._refresh_callback,
self._toc_cache)
toc_fetcher.start()
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.data[0]
block = self._find_block(id)
timestamps = struct.unpack('<BBB', packet.data[1:4])
timestamp = (
timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning('Error no LogEntry to handle id=%d', id)
class JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True, cf=None):
# TODO: Should be OS dependant
self.inputdevice = AiController(cf)
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_alt_hold = False
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("min_thrust"),
Config().get("max_thrust"))
self.set_thrust_slew_limiting(Config().get("slew_rate"),
Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
def setAltHoldAvailable(self, available):
self._has_pressure_sensor = available
def setAltHold(self, althold):
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = self.inputdevice.getAvailableDevices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev["name"]))):
self._available_devices[dev["name"]] = dev["id"]
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self._read_timer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self._read_timer.stop()
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
self._max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
self._max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if (thrust_slew_rate > 0):
self._thrust_slew_enabled = True
else:
self._thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
self._min_thrust = JoystickReader.p2t(min_thrust)
self._max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def read_input(self):
"""Read input data from the selected device"""
try:
data = self.inputdevice.read_input()
roll = data["roll"] #* self._max_rp_angle
pitch = data["pitch"] #* self._max_rp_angle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
althold = data["althold"]
#Deadband the roll and pitch
roll = JoystickReader.deadband(roll, 0.2) * self._max_rp_angle
pitch = JoystickReader.deadband(pitch, 0.2) * self._max_rp_angle
if (self._old_alt_hold != althold):
self.althold_updated.call(str(althold))
self._old_alt_hold = althold
if self._emergency_stop != emergency_stop:
self._emergency_stop = emergency_stop
self.emergency_stop_updated.call(self._emergency_stop)
# Thust limiting (slew, minimum and emergency stop)
if althold and self._has_pressure_sensor:
thrust = int(
round(
JoystickReader.deadband(thrust, 0.2) * 32767 +
32767)) #Convert to uint16
else:
if raw_thrust < 0.15 or emergency_stop:
thrust = 0
else:
thrust = self._min_thrust + thrust * (self._max_thrust -
self._min_thrust)
if (self._thrust_slew_enabled == True
and self._thrust_slew_limit > thrust
and not emergency_stop):
if self._old_thrust > self._thrust_slew_limit:
self._old_thrust = self._thrust_slew_limit
if thrust < (self._old_thrust -
(self._thrust_slew_rate / 100)):
thrust = self._old_thrust - self._thrust_slew_rate / 100
if raw_thrust < 0 or thrust < self._min_thrust:
thrust = 0
self._old_thrust = thrust
# Yaw deadband
# TODO: Add to input device config?
yaw = JoystickReader.deadband(yaw, 0.2) * self._max_yaw_rate
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value / (1 - threshold)
class JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
# TODO: Should be OS dependant
self.inputdevice = PyGameReader()
self.maxRPAngle = 0
self.thrustDownSlew = 0
self.thrustSlewEnabled = False
self.slewEnableLimit = 0
self.maxYawRate = 0
self.detectAxis = False
self.emergencyStop = False
self.oldThrust = 0
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
# TODO: The polling interval should be set from config file
self.readTimer = PeriodicTimer(0.01, self.readInput)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0, self._do_device_discovery)
self._discovery_timer.start()
self._available_devices = {}
# Check if user config exists, otherwise copy files
if not os.path.isdir(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available input devices."""
devs = self.inputdevice.getAvailableDevices()
for d in devs:
self._available_devices[d["name"]] = d["id"]
return devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self.readTimer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self.readTimer.stop()
def set_yaw_limit(self, maxRate):
"""Set a new max yaw rate value."""
self.maxYawRate = maxRate
def set_rp_limit(self, maxAngle):
"""Set a new max roll/pitch value."""
self.maxRPAngle = maxAngle
def set_thrust_slew_limiting(self, thrustDownSlew, slewLimit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self.thrustDownSlew = thrustDownSlew
self.slewEnableLimit = slewLimit
if (thrustDownSlew > 0):
self.thrustSlewEnabled = True
else:
self.thrustSlewEnabled = False
def set_thrust_limits(self, minThrust, maxThrust):
"""Set a new min/max thrust limit."""
self.minThrust = minThrust
self.maxThrust = maxThrust
def _update_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def _update_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def readInput(self):
"""Read input data from the selected device"""
try:
data = self.inputdevice.readInput()
roll = data["roll"] * self.maxRPAngle
pitch = data["pitch"] * self.maxRPAngle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
if self.emergencyStop != emergency_stop:
self.emergencyStop = emergency_stop
self.emergency_stop_updated.call(self.emergencyStop)
# Thust limiting (slew, minimum and emergency stop)
if raw_thrust < 0.05 or emergency_stop:
thrust = 0
else:
thrust = self.minThrust + thrust * (self.maxThrust -
self.minThrust)
if (self.thrustSlewEnabled == True and
self.slewEnableLimit > thrust and not
emergency_stop):
if self.oldThrust > self.slewEnableLimit:
self.oldThrust = self.slewEnableLimit
if thrust < (self.oldThrust - (self.thrustDownSlew / 100)):
thrust = self.oldThrust - self.thrustDownSlew / 100
if raw_thrust < 0 or thrust < self.minThrust:
thrust = 0
self.oldThrust = thrust
# Yaw deadband
# TODO: Add to input device config?
if yaw < -0.2 or yaw > 0.2:
if yaw < 0:
yaw = (yaw + 0.2) * self.maxYawRate * 1.25
else:
yaw = (yaw - 0.2) * self.maxYawRate * 1.25
else:
self.yaw = 0
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call(
"Error reading from input device\n\n%s" %
traceback.format_exc())
self.readTimer.stop()
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 JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
# TODO: Should be OS dependant
self.SF = sensorFusion()
self.inputdevice = PyGameReader()
self.pointerDevice = psmove.PSMove()
self.PointerYaw = 0
self.kalmanPitch = KalmanFilter()
self.kalmanRoll = KalmanFilter()
self.viscousModeThrust = 67
self._emergency_landing = False
self.auto = False
self._min_thrust = 0
self._max_thrust = 0
self._maxAltitude = 0
self.currentAltitude = 0
self.minAltitude = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._canSwitch = True
self._old_thrust = 0
self._old_alt_hold = False
self._old_flip_type = -1
self._flip_time_start = -float("inf")
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
self._trim_yaw = 0.0
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(Config().get("min_thrust"),
Config().get("max_thrust"))
self.set_thrust_slew_limiting(Config().get("slew_rate"),
Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.switch_mode_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.auto_input_updated = Caller()
self.pointer_input_updated = Caller()
def setViscousModeThrust(self, thrust):
if thrust >= 0:
self.viscousModeThrust = thrust
def setEmergencyLanding(self, emergencyLanding):
self._emergency_landing = emergencyLanding
def setAltHoldAvailable(self, available):
self._has_pressure_sensor = available
def setAuto(self, auto):
self.auto = auto
def setAltHold(self, althold):
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = self.inputdevice.getAvailableDevices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev["name"]))):
self._available_devices[dev["name"]] = dev["id"]
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self._read_timer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self._read_timer.stop()
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
self._max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
self._max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if (thrust_slew_rate > 0):
self._thrust_slew_enabled = True
else:
self._thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
self._min_thrust = JoystickReader.p2t(min_thrust)
self._max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def setMaxAltitude(self, maxAltitude):
self._maxAltitude = maxAltitude
def setCurrentAltitude(self, altitude):
if altitude < self.minAltitude or self.minAltitude == 0:
self.minAltitude = altitude
self.currentAltitude = altitude
def read_input(self):
"""Read input data from the selected device"""
if self.pointerDevice is not None:
#DT = 0.001
if self.pointerDevice.poll():
buttons = self.pointerDevice.get_buttons()
if buttons & psmove.Btn_MOVE:
self.pointerDevice.set_leds(0, 255, 0)
self.pointerDevice.update_leds()
self.SF = sensorFusion()
'''
trigger_value = self.move.get_trigger()
self.move.set_leds(trigger_value, 0, 0)
self.move.update_leds()
'''
ax, ay, az = self.pointerDevice.get_accelerometer_frame(
psmove.Frame_SecondHalf)
gx, gy, gz = self.pointerDevice.get_gyroscope_frame(
psmove.Frame_SecondHalf)
gx = gx * 180 / math.pi
gy = gy * 180 / math.pi
gz = gz * 180 / math.pi
#print "A: %5.2f %5.2f %5.2f " % ( ax , ay , az )
#print "G: %8.2f %8.2f %8.2f " % ( gx , gy , gz )
self.SF.sensfusion6UpdateQ(gx, gy, gz, ax, ay, az, 1 / 100)
roll, pitch, yaw = self.SF.sensfusion6GetEulerRPY()
self.PointerYaw = -yaw
'''
# Read sensor
acc_x = self.pointerDevice.ax
acc_y = self.pointerDevice.ay
acc_z = self.pointerDevice.az
gyr_x = self.pointerDevice.gx
gyr_y = self.pointerDevice.gy
gyr_z = self.pointerDevice.gz
#// Calculate pitch and roll based only on acceleration.
acc_pitch = math.atan2(acc_x, -acc_z)
acc_roll = -math.atan2(acc_y, -acc_z)
# Perform filtering
self.kalmanPitch.kalman_innovate(acc_pitch, gyr_x, DT)
self.kalmanRoll.kalman_innovate(acc_roll, gyr_y, DT)
# The angle is stored in state 1
pitch = self.kalmanPitch.x1
roll = self.kalmanRoll.x1
cosRoll = math.cos(roll)
sinRoll = math.sin(roll)
cosPitch = math.cos(pitch)
sinPitch = math.sin(pitch)
magX = self.pointerDevice.mx * cosPitch + self.pointerDevice.mz * sinPitch
magY = self.pointerDevice.mx * sinRoll * sinPitch + self.pointerDevice.my * cosRoll - self.pointerDevice.mz * sinRoll * cosPitch
norm = math.sqrt(magX*magX + magY*magY)
magHeadingX = magX/norm
magHeadingY = -magY/norm
#Absolute Yaw
#self.PointerYaw = self.pointerDevice.mz
#roll = self.pointerDevice.gy - self.pointerDevice.gy/GYROSCOPE_SENSITIVITY*dt
#pitch = self.pointerDevice.gx - self.pointerDevice.gx/GYROSCOPE_SENSITIVITY*dt
DT = 0.001
yaw = self.pointerDevice.gz - self.pointerDevice.gz/GYROSCOPE_SENSITIVITY*DT
self.PointerYaw -= yaw*DT
if self.PointerYaw >= 180:
self.PointerYaw = -180
elif self.PointerYaw <= -180:
self.PointerYaw = 180
if self.pointerDevice.get_buttons() & psmove.Btn_MOVE: #psmove.Btn_T:
self.pointerDevice.set_leds(0, 255, 0)
self.pointerDevice.update_leds()
self.PointerYaw = 0
'''
if self.PointerYaw >= 0:
self.pointerDevice.set_leds(
int(255 * self.PointerYaw / 180), 255, 0)
else:
self.pointerDevice.set_leds(
0, 255, int(255 * math.fabs(self.PointerYaw) / 180))
self.pointerDevice.update_leds()
self.pointer_input_updated.call(self.PointerYaw, False)
try:
data = self.inputdevice.read_input()
roll = data["roll"] * self._max_rp_angle
pitch = data["pitch"] * self._max_rp_angle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
althold = data["althold"]
flipleft = data["flipleft"]
flipright = data["flipright"]
viscousMode = data["viscousMode"]
switchMode = data["switchmode"]
if switchMode and self._canSwitch:
self._canSwitch = False
self.switch_mode_updated.call()
elif not switchMode:
self._canSwitch = True
if (self._old_alt_hold != althold):
self.althold_updated.call(althold)
self._old_alt_hold = althold
if self._emergency_stop != emergency_stop:
self._emergency_stop = emergency_stop
self.emergency_stop_updated.call(self._emergency_stop)
if self.auto:
self.auto_input_updated.call(trim_roll, trim_pitch, yaw,
thrust)
else:
# Altitude Hold Mode Toggled
if (self._old_alt_hold != althold):
self.althold_updated.call(althold)
self._old_alt_hold = althold
# Disable hover mode if enabled
if self._emergency_stop:
if self._has_pressure_sensor:
if self._old_alt_hold:
self.althold_updated.call(False)
self._old_alt_hold = False
althold = False
'''
modalità in cui il quad rimane fermo in altitudine e può salire o scendere in base a come si
utilizza il joystick
thrust up (>0) => sale (costantemente)
thrust down (<0) => scende (costantemente)
'''
if viscousMode:
viscous_thrust = self.p2t(self.viscousModeThrust)
if raw_thrust > 0 and raw_thrust <= 0.5:
raw_thrust = 1
elif raw_thrust > 0.5:
raw_thrust = 2
elif raw_thrust >= -0.5 and raw_thrust < 0:
raw_thrust = -0.5
elif raw_thrust < -0.5:
raw_thrust = -1
'''
if (self.currentAltitude - self.minAltitude) == self._maxAltitude:
raw_thrust = 0
elif (self.currentAltitude - self.minAltitude) > self._maxAltitude:
raw_thrust = -0.2
'''
thrust = int(round(viscous_thrust + raw_thrust * self.p2t(10)))
# Thust limiting (slew, minimum and emergency stop)
elif (althold and self._has_pressure_sensor) or (flipleft
or flipright):
thrust = int(
round(
JoystickReader.deadband(thrust, 0.2) * 32767 +
32767)) #Convert to uint16
else:
if raw_thrust < 0.05 or emergency_stop:
thrust = 0
else:
thrust = self._min_thrust + thrust * (self._max_thrust -
self._min_thrust)
if (self._thrust_slew_enabled == True
and self._thrust_slew_limit > thrust
and not emergency_stop):
#if (self._thrust_slew_enabled == True and not emergency_stop):
if self._old_thrust > self._thrust_slew_limit:
self._old_thrust = self._thrust_slew_limit
if thrust < (self._old_thrust -
(self._thrust_slew_rate / 100)):
thrust = self._old_thrust - self._thrust_slew_rate / 100
if raw_thrust < 0 or thrust < self._min_thrust:
thrust = 0
#if trim_pitch > 0:
# thrust += self.p2t(1)
#if trim_pitch < 0:
# thrust -= self.p2t(1)
if self._emergency_landing:
thrust = self._old_thrust - self.p2t(10) * 0.2
if thrust < 0: thrust = 0
self._old_thrust = thrust
# Yaw deadband
# TODO: Add to input device config?
yaw = JoystickReader.deadband(yaw, 0.2) * self._max_yaw_rate
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
if (flipleft or flipright) and self._flip_time_start < 0:
self._flip_time_start = time.time()
#ricavo il momento in cui inizia il flip
if flipleft:
self._old_flip_type = 0
if flipright:
self._old_flip_type = 1
#if (flipleft and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 0:
if flipleft and self._old_flip_type == 0:
thrust = self.p2t(
70
) #faccio in modo che il robot rimanga nella posizione corrente
roll = 1600
#elif (flipright and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 1:
elif flipright and self._old_flip_type == 1:
#thrust = self.p2t(70)
#roll = 30
#self.input_updated.call(roll, 0, yaw, thrust)
#time.sleep(0.04)
thrust = self.p2t(50)
roll = -1000
self.input_updated.call(roll, 0, yaw, thrust)
#time.sleep(FLIP_TIME)
'''
#######
## 1 ##
#######
thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente
roll = 30
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 2 ##
#######
thrust = self.p2t(50)
roll = -1600
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 3 ##
#######
thrust = 0
roll = 0
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.0004)
#######
## 4 ##
#######
thrust = self.p2t(50)
roll = -1600
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 5 ##
#######
thrust = self.p2t(70)
roll = 30
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 6 ##
#######
thrust = self.p2t(53)
roll = 0
self.input_updated.call(roll, 0, yaw, thrust)
return;
'''
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
if not flipleft and not flipright and not self.flipTimeControl(
self._flip_time_start):
self._old_flip_type = -1
self._flip_time_start = -float("inf")
#resetto _flip_time_start
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
#yaw = yaw + self._trim_yaw
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self._read_timer.stop()
def update_trim_yaw_signal(self, yaw):
self._trim_yaw = yaw
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value / (1 - threshold)
@staticmethod
def flipTimeControl(startTime):
return (time.time() - startTime >= 0
and time.time() - startTime <= FLIP_TIME)
class Drone:
"""Represents a CrazyFlie drone."""
def __init__(self,
drone_id: str,
arena: Arena,
radio_id: int = 0,
channel: int = 80,
address: str = "E7E7E7E7E7",
data_rate: str = "2M"):
""" Initializes the drone with the given uri."""
# Initialize public variables
self.id: str = drone_id
self.var_x: float = 0
self.var_y: float = 0
self.var_z: float = 0
self.pos_x: float = 0
self.pos_y: float = 0
self.pos_z: float = 0
self.pitch: float = 0
self.roll: float = 0
self.yaw: float = 0
self.battery_voltage: float = 0
self.is_connected: bool = False
self.status: DroneState = DroneState.OFFLINE
self.link_uri: str = "radio://" + str(radio_id) + "/" + str(
channel) + "/" + data_rate + "/" + address
# Initialize limits
self._max_velocity: float = 1.0
self._min_duration: float = 1.0
self._max_yaw_rotations: float = 1.0
self._arena = arena
# Event to asynchronously wait for the connection
self._connect_event = threading.Event()
# Initialize the crazyflie
self._cf = Crazyflie(rw_cache='./cache')
# Initialize the callbacks
self._cf.connected.add_callback(self._connected)
self._cf.disconnected.add_callback(self._disconnected)
self._cf.connection_failed.add_callback(self._connection_failed)
self._cf.connection_lost.add_callback(self._connection_lost)
# Initialize events
self.drone_lost = Caller()
# Define the log configuration
self._log_config_1 = LogConfig(name='DroneLog_1', period_in_ms=500)
self._log_config_1.add_variable('kalman.varPX', 'float')
self._log_config_1.add_variable('kalman.varPY', 'float')
self._log_config_1.add_variable('kalman.varPZ', 'float')
self._log_config_1.add_variable('pm.vbat', 'float')
self._log_config_2 = LogConfig(name='DroneLog_2', period_in_ms=100)
self._log_config_2.add_variable('kalman.stateX', 'float')
self._log_config_2.add_variable('kalman.stateY', 'float')
self._log_config_2.add_variable('kalman.stateZ', 'float')
self._log_config_3 = LogConfig(name='DroneLog_3', period_in_ms=500)
self._log_config_3.add_variable('stabilizer.pitch', 'float')
self._log_config_3.add_variable('stabilizer.roll', 'float')
self._log_config_3.add_variable('stabilizer.yaw', 'float')
def connect(self, synchronous: bool = False):
"""Connects to the Crazyflie."""
self._connect_crazyflie()
if synchronous:
self._connect_event.wait()
def disconnect(self):
"""Disconnects from the Crazyflie and stops all logging."""
self._disconnect_crazyflie()
def enable_high_level_commander(self):
"""Enables the drones high level commander."""
self._cf.param.set_value('commander.enHighLevel', '1')
time.sleep(0.1)
def disable_motion_tracking(self):
"""Disables to motion control (x/y) from the flow-deck."""
self._cf.param.set_value('motion.disable', '1')
time.sleep(0.1)
def get_status(self) -> str:
"""Gets various information of the drone."""
return {
"id":
self.id,
"var_x":
self.var_x,
"var_y":
self.var_y,
"var_z":
self.var_z,
"x":
self._arena.transform_x_inverse(self.pos_x),
"y":
self._arena.transform_y_inverse(self.pos_y),
"z":
self.pos_z,
"pitch":
self.pitch,
"roll":
self.roll,
"yaw":
self.yaw,
"status":
self.status.name,
"battery_voltage":
self.battery_voltage,
"battery_percentage:":
(self.battery_voltage - 3.4) / (4.18 - 3.4) * 100
}
def reset_estimator(self) -> bool:
"""Resets the position estimates."""
self._cf.param.set_value('kalman.resetEstimation', '1')
time.sleep(0.1)
self._cf.param.set_value('kalman.resetEstimation', '0')
time.sleep(2.0)
# TODO: wait_for_position_estimator(cf)
return True
def takeoff(self,
absolute_height: float,
velocity: float,
synchronous: bool = False) -> float:
absolute_height = self._sanitize_z(absolute_height, False)
self.reset_estimator()
duration = self._convert_velocity_to_time(absolute_height, velocity)
self._cf.high_level_commander.takeoff(absolute_height, duration)
self.status = DroneState.STARTING
if synchronous:
time.sleep(duration)
return {"duration": duration, "target_z": absolute_height}
def land(self,
absolute_height: float,
velocity: float,
synchronous: bool = False) -> float:
absolute_height = self._sanitize_z(absolute_height, False)
duration = self._convert_velocity_to_time(absolute_height, velocity)
self._cf.high_level_commander.land(absolute_height, duration)
self.status = DroneState.LANDING
if synchronous:
time.sleep(duration)
return {"duration": duration, "target_z": absolute_height}
def go_to(self,
x: float,
y: float,
z: float,
yaw: float,
velocity: float,
relative: bool = False,
synchronous: bool = False) -> float:
x = self._sanitize_x(x, relative)
y = self._sanitize_y(y, relative)
z = self._sanitize_z(z, relative)
yaw = self._sanitize_yaw(yaw)
distance = self._calculate_distance(x, y, z, relative)
duration = self._convert_velocity_to_time(distance, velocity)
self._cf.high_level_commander.go_to(x, y, z, yaw, duration, relative)
self.status = DroneState.NAVIGATING
if synchronous:
time.sleep(duration)
return {
"duration": duration,
"target_x": self._arena.transform_x_inverse(x),
"target_y": self._arena.transform_y_inverse(y),
"target_z": z,
"target_yaw": yaw,
"relative": relative
}
def stop(self):
self._cf.high_level_commander.stop()
self.status = DroneState.IDLE
def _connect_crazyflie(self):
print('Connecting to %s' % self.link_uri)
self._cf.open_link(self.link_uri)
def _disconnect_crazyflie(self):
print('Disconnecting from %s' % self.link_uri)
# Stop the loggers
self._log_config_1.stop()
self._log_config_2.stop()
self._log_config_3.stop()
# Shutdown the rotors
self._shutdown()
# Disconnect
self._cf.close_link()
def _connected(self, link_uri):
"""This callback is called when the Crazyflie has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
# Setup parameters
self.disable_motion_tracking()
# Add the logger
self._cf.log.add_config(self._log_config_1)
self._cf.log.add_config(self._log_config_2)
self._cf.log.add_config(self._log_config_3)
# This callback will receive the data
self._log_config_1.data_received_cb.add_callback(
self._log_config_1_data)
self._log_config_2.data_received_cb.add_callback(
self._log_config_2_data)
self._log_config_3.data_received_cb.add_callback(
self._log_config_3_data)
# This callback will be called on errors
self._log_config_1.error_cb.add_callback(self._log_config_error)
self._log_config_2.error_cb.add_callback(self._log_config_error)
self._log_config_3.error_cb.add_callback(self._log_config_error)
# Start the logging
self._log_config_1.start()
self._log_config_2.start()
self._log_config_3.start()
# Set the connected event
self._connect_event.set()
self.is_connected = True
self.status = DroneState.IDLE
def _connection_failed(self, link_uri, msg):
"""Callback when the initial connection fails."""
print('Connection to %s failed: %s' % (link_uri, msg))
# Set the connected event
self._connect_event.set()
def _disconnected(self, link_uri):
"""Callback when the Crazyflie is disconnected."""
print('Disconnected from %s' % link_uri)
self.is_connected = False
self.status = DroneState.OFFLINE
def _connection_lost(self, link_uri, msg):
"""Callback when the connection is lost after a connection has been made."""
print('Connection to %s lost: %s' % (link_uri, msg))
self.drone_lost.call(self)
self._connect_event.set()
self.is_connected = False
self.status = DroneState.OFFLINE
def _log_config_error(self, logconf, msg):
"""Callback from the log API when an error occurs."""
print('Error when logging %s: %s' % (logconf.name, msg))
def _log_config_1_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives."""
self.var_x = data['kalman.varPX']
self.var_y = data['kalman.varPY']
self.var_z = data['kalman.varPZ']
self.battery_voltage = data['pm.vbat']
def _log_config_2_data(self, timestamp, data, logconf):
"""Callback from the log API when data arrives."""
self.pos_x = data['kalman.stateX']
self.pos_y = data['kalman.stateY']
self.pos_z = data['kalman.stateZ']
def _log_config_3_data(self, timestamp, data, logconf):
self.pitch = data['stabilizer.pitch']
self.roll = data['stabilizer.roll']
self.yaw = data['stabilizer.yaw']
def _unlock(self):
# Unlock startup thrust protection (only needed for low lewel commands)
self._cf.commander.send_setpoint(0, 0, 0, 0)
def _shutdown(self):
self._cf.commander.send_setpoint(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
def _keep_setpoint(self, roll, pitch, yawrate, thrust, keeptime):
"""Keeps the drone at the given setpoint for the given amount of time."""
while keeptime > 0:
self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)
keeptime -= 0.1
time.sleep(0.1)
def _convert_velocity_to_time(self, distance: float,
velocity: float) -> float:
"""Converts a distance and a velocity to a time."""
duration = distance / self._sanitize_velocity(velocity)
return self._sanitize_duration(duration)
def _calculate_distance(self,
x: float,
y: float,
z: float,
relative: bool = False) -> float:
"""Calculates the distance from the drone or the zero position (relative) to a given point in space."""
start_x = 0 if relative else self.pos_x
start_y = 0 if relative else self.pos_y
start_z = 0 if relative else self.pos_z
return np.sqrt((x - start_x)**2 + (y - start_y)**2 + (z - start_z)**2)
def _sanitize_velocity(self, velocity: float) -> float:
return min(velocity, self._max_velocity)
def _sanitize_duration(self, duration: float) -> float:
return max(duration, self._min_duration)
def _sanitize_yaw(self, yaw: float) -> float:
return yaw % (2 * self._max_yaw_rotations * math.pi)
def _sanitize_x(self, x: float, relative: bool) -> float:
target_x = (self.pos_x + x) if relative else x
sanitized_x = self._sanitize_number(target_x, self._arena.min_x,
self._arena.max_x)
return self._arena.transform_x(sanitized_x)
def _sanitize_y(self, y: float, relative: bool) -> float:
target_y = (self.pos_y + y) if relative else y
sanitized_y = self._sanitize_number(target_y, self._arena.min_y,
self._arena.max_y)
return self._arena.transform_y(sanitized_y)
def _sanitize_z(self, z: float, relative: bool) -> float:
return self._sanitize_number(z, self._arena.min_z, self._arena.max_z)
def _sanitize_number(self, value: float, min_value: float,
max_value: float) -> float:
return min(max(value, min_value), max_value)
class JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
self._input_device = None
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self.max_rp_angle = 0
self.max_yaw_rate = 0
self._old_thrust = 0
self._old_raw_thrust = 0
self._old_alt_hold = False
self.springy_throttle = True
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._input_map = None
self._mux = [NoMux(self), TakeOverSelectiveMux(self),
TakeOverMux(self)]
# Set NoMux as default
self._selected_mux = self._mux[0]
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(
cfclient.module_path + "/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def enable_alt_hold(self, althold):
"""Enable or disable altitude hold"""
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist) or
(self._dev_blacklist and
not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons, mapped_values] = self._input_device.read(
include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._input_map = ConfigManager().get_config(input_map_name)
self._get_device_from_name(device_name).input_map = self._input_map
self._get_device_from_name(device_name).input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp,
device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.althold:
try:
self.althold_updated.call(str(data.althold))
except Exception as e:
logger.warning(
"Exception while doing callback from input-device "
"for althold: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= 1
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += 1
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= 1
if data.toggled.rollPos and data.rollPos:
self.trim_roll += 1
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll, self.trim_pitch)
# Thrust might be <0 here, make sure it's not otherwise we'll
# get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
# If we are using alt hold the data is not in a percentage
if not data.althold:
data.thrust = JoystickReader.p2t(data.thrust)
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch,
data.yaw, data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class LogConfig(object):
"""Representation of one log configuration that enables logging
from the Crazyflie"""
_config_id_counter = 1
def __init__(self, name, period_in_ms):
"""Initialize the entry"""
self.data_received_cb = Caller()
self.error_cb = Caller()
self.started_cb = Caller()
self.added_cb = Caller()
self.err_no = 0
self.id = LogConfig._config_id_counter
LogConfig._config_id_counter = (LogConfig._config_id_counter + 1) % 255
self.cf = None
self.period = int(period_in_ms / 10)
self.period_in_ms = period_in_ms
self._added = False
self._started = False
self.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def add_variable(self, name, fetch_as=None):
"""Add a new variable to the configuration.
name - Complete name of the variable in the form group.name
fetch_as - String representation of the type the variable should be
fetched as (i.e uint8_t, float, FP16, etc)
If no fetch_as type is supplied, then the stored as type will be used
(i.e the type of the fetched variable is the same as it's stored in the
Crazyflie)."""
if fetch_as:
self.variables.append(LogVariable(name, fetch_as))
else:
# We cannot determine the default type until we have connected. So
# save the name and we will add these once we are connected.
self.default_fetch_as.append(name)
def add_memory(self, name, fetch_as, stored_as, address):
"""Add a raw memory position to log.
name - Arbitrary name of the variable
fetch_as - String representation of the type of the data the memory
should be fetch as (i.e uint8_t, float, FP16)
stored_as - String representation of the type the data is stored as
in the Crazyflie
address - The address of the data
"""
self.variables.append(
LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as,
address))
def _set_added(self, added):
if added != self._added:
self.added_cb.call(self, added)
self._added = added
def _get_added(self):
return self._added
def _set_started(self, started):
if started != self._started:
self.started_cb.call(self, started)
self._started = started
def _get_started(self):
return self._started
added = property(_get_added, _set_added)
started = property(_get_started, _set_started)
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)
for var in self.variables:
if (var.is_toc_variable() is False): # Memory location
logger.debug('Logging to raw memory %d, 0x%04X',
var.get_storage_and_fetch_byte(), var.address)
pk.data.append(
struct.pack('<B', var.get_storage_and_fetch_byte()))
pk.data.append(struct.pack('<I', var.address))
else: # Item in TOC
logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
self.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))
logger.debug('Adding log block id {}'.format(self.id))
self.cf.send_packet(pk, expected_reply=(CMD_CREATE_BLOCK, self.id))
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
self.create()
logger.debug('First time block is started, add block')
else:
logger.debug(
'Block already registered, starting logging'
' for id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.cf.send_packet(pk,
expected_reply=(CMD_START_LOGGING,
self.id))
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning('Stopping block, but no block registered')
else:
logger.debug('Sending stop logging for block id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.cf.send_packet(pk,
expected_reply=(CMD_STOP_LOGGING, self.id))
def delete(self):
"""Delete this entry in the Crazyflie"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning('Delete block, but no block registered')
else:
logger.debug(
'LogEntry: Sending delete logging for block id=%d' %
self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.cf.send_packet(pk,
expected_reply=(CMD_DELETE_BLOCK, self.id))
def unpack_log_data(self, log_data, timestamp):
"""Unpack received logging data so it represent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.variables:
size = LogTocElement.get_size_from_id(var.fetch_as)
name = var.name
unpackstring = LogTocElement.get_unpack_string_from_id(
var.fetch_as)
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received_cb.call(timestamp, ret_data, self)
class LogEntry:
blockIdCounter = 1
def __init__(self, crazyflie, logconf):
self.dataReceived = Caller()
self.error = Caller()
self.logconf = logconf
self.blockId = LogEntry.blockIdCounter
LogEntry.blockIdCounter += 1
self.cf = crazyflie
self.period = logconf.getPeriod() / 10
self.blockCreated = False
def start(self):
if (self.cf.link is not None):
if (self.blockCreated is False):
logger.debug("First time block is started, add block")
self.blockCreated = True
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.blockId)
for v in self.logconf.getVariables():
if (v.isTocVariable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
v.getStoredFetchAs(), v.getAddress())
pk.data += struct.pack('<B', v.getStoredFetchAs())
pk.data += struct.pack('<I', v.getAddress())
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
v.getName(),
self.cf.log.toc.get_element_id(
v.getName()), v.getStoredFetchAs())
pk.data += struct.pack('<B', v.getStoredFetchAs())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(v.getName()))
logger.debug("Adding log block id {}".format(self.blockId))
self.cf.send_packet(pk)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.blockId)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.blockId, self.period)
self.cf.send_packet(pk)
def stop(self):
if (self.cf.link is not None):
if (self.blockId is None):
logger.warning("Stopping block, but no block registered")
else:
logger.debug("Sending stop logging for block %d", self.blockId)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.blockId)
self.cf.send_packet(pk)
def close(self):
if (self.cf.link is not None):
if (self.blockId is None):
logger.warning("Delete block, but no block registered")
else:
logger.debug("LogEntry: Sending delete logging for block %d"
% self.blockId)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.blockId)
self.cf.send_packet(pk)
self.blockId = None # Wait until we get confirmation of delete
def unpack_log_data(self, logData):
retData = {}
dataIndex = 0
for v in self.logconf.getVariables():
size = LogTocElement.get_size_from_id(v.getFetchAs())
name = v.getName()
unpackstring = LogTocElement.get_unpack_string_from_id(
v.getFetchAs())
value = struct.unpack(unpackstring,
logData[dataIndex:dataIndex + size])[0]
dataIndex += size
retData[name] = value
self.dataReceived.call(retData)
class JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Crazyflie
"""
inputConfig = []
ASSISTED_CONTROL_ALTHOLD = 0
ASSISTED_CONTROL_POSHOLD = 1
ASSISTED_CONTROL_HEIGHTHOLD = 2
ASSISTED_CONTROL_HOVER = 3
def __init__(self, do_device_discovery=True):
self._input_device = None
self._mux = [NoMux(self), TakeOverSelectiveMux(self),
TakeOverMux(self)]
# Set NoMux as default
self._selected_mux = self._mux[0]
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self._hover_max_height = MAX_TARGET_HEIGHT
self.max_rp_angle = 0
self.max_yaw_rate = 0
try:
self.set_assisted_control(Config().get("assistedControl"))
except KeyError:
self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD)
self._old_thrust = 0
self._old_raw_thrust = 0
self.springy_throttle = True
self._target_height = INITAL_TAGET_HEIGHT
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._rp_dead_band = 0.1
self._input_map = None
if Config().get("flightmode") is "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(
cfclient.module_path + "/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.assisted_input_updated = Caller()
self.heighthold_input_updated = Caller()
self.hover_input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.assisted_control_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_hover_max_height(self, height):
self._hover_max_height = height
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def set_assisted_control(self, mode):
self._assisted_control = mode
def get_assisted_control(self):
return self._assisted_control
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist) or
(self._dev_blacklist and
not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons, mapped_values] = self._input_device.read(
include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
dev = self._get_device_from_name(device_name)
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._rp_dead_band = settings["rp_dead_band"]
self._input_map = ConfigManager().get_config(input_map_name)
dev.input_map = self._input_map
dev.input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
dev.set_dead_band(self._rp_dead_band)
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp,
device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.assistedControl:
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD or \
self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER:
if data.assistedControl and self._assisted_control != \
JoystickReader.ASSISTED_CONTROL_HOVER:
for d in self._selected_mux.devices():
d.limit_thrust = False
d.limit_rp = False
elif data.assistedControl:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = False
else:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = True
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_ALTHOLD:
self.assisted_control_updated.call(
data.assistedControl)
if ((self._assisted_control ==
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD) or
(self._assisted_control ==
JoystickReader.ASSISTED_CONTROL_HOVER)):
try:
self.assisted_control_updated.call(
data.assistedControl)
if not data.assistedControl:
# Reset height controller state to initial
# target height both in the UI and in the
# Crazyflie.
# TODO: Implement a proper state update of the
# input layer
self.heighthold_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
self.hover_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
except Exception as e:
logger.warning(
"Exception while doing callback from "
"input-device for assited "
"control: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Reset height target when height-hold is not selected
if not data.assistedControl or \
(self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD and
self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HOVER):
self._target_height = INITAL_TAGET_HEIGHT
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD \
and data.assistedControl:
vx = data.roll
vy = data.pitch
vz = data.thrust
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.assisted_input_updated.call(vy, -vx, vz, yawrate)
elif self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER \
and data.assistedControl:
vx = data.roll
vy = data.pitch
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_HOVER_HEIGHT:
self._target_height = MIN_HOVER_HEIGHT
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.hover_input_updated.call(vy, -vx, yawrate,
self._target_height)
else:
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= .2
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += .2
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= .2
if data.toggled.rollPos and data.rollPos:
self.trim_roll += .2
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll,
self.trim_pitch)
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD \
and data.assistedControl:
roll = data.roll + self.trim_roll
pitch = data.pitch + self.trim_pitch
yawrate = data.yaw
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_TARGET_HEIGHT:
self._target_height = MIN_TARGET_HEIGHT
self.heighthold_input_updated.call(roll, -pitch,
yawrate,
self._target_height)
else:
# Using alt hold the data is not in a percentage
if not data.assistedControl:
data.thrust = JoystickReader.p2t(data.thrust)
# Thrust might be <0 here, make sure it's not otherwise
# we'll get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch,
data.yaw, data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class Crazyflie():
"""The Crazyflie class"""
def __init__(self, link=None):
"""
Create the objects from this module and register callbacks.
"""
# Called on disconnect, no matter the reason
self.disconnected = Caller()
# Called on unintentional disconnect only
self.connection_lost = Caller()
# Called when the first packet in a new link is received
self.link_established = Caller()
# Called when the user requests a connection
self.connection_requested = Caller()
# Called when the link is established
self.connected = Caller()
# Called if establishing of the link fails (i.e times out)
self.connection_failed = Caller()
# Called for every packet received
self.packet_received = Caller()
# Called for every packet sent
self.packet_sent = Caller()
# Called when the link driver updates the link quality measurement
self.link_quality_updated = Caller()
self.state = State.DISCONNECTED
self.link = link
# Thread handling incoming packets
self.incoming = _IncomingPacketHandler(self)
self.incoming.setDaemon(True)
self.incoming.start()
# Thread handling outgoing packets
self.outgoing = _OutgoingPacketHandler(self)
self.outgoing.setDaemon(True)
self.outgoing.start()
self.commander = Commander(self)
self.link_uri = ''
# Used for retry when no reply was sent back
self.packet_received.add_callback(self._check_for_initial_packet_cb)
self._send_lock = Lock()
self.connected_ts = None
# Connect callbacks to logger
self.disconnected.add_callback(
lambda uri: logger.info('Callback->Disconnected from [%s]', uri))
self.disconnected.add_callback(self._disconnected)
self.link_established.add_callback(
lambda uri: logger.info('Callback->Connected to [%s]', uri))
self.connection_lost.add_callback(lambda uri, errmsg: logger.info(
'Callback->Connection lost to [%s]: %s', uri, errmsg))
self.connection_failed.add_callback(lambda uri, errmsg: logger.info(
'Callback->Connected failed to [%s]: %s', uri, errmsg))
self.connection_requested.add_callback(lambda uri: logger.info(
'Callback->Connection initialized[%s]', uri))
self.connected.add_callback(lambda uri: logger.info(
'Callback->Connection setup finished [%s]', uri))
def _disconnected(self, link_uri):
""" Callback when disconnected."""
self.connected_ts = None
def _link_error_cb(self, errmsg):
"""Called from the link driver when there's an error"""
logger.warning('Got link error callback [%s] in state [%s]', errmsg,
self.state)
if (self.link is not None):
self.link.close()
self.link = None
if (self.state == State.INITIALIZED):
self.connection_failed.call(self.link_uri, errmsg)
if (self.state == State.CONNECTED):
self.disconnected.call(self.link_uri)
self.connection_lost.call(self.link_uri, errmsg)
self.state = State.DISCONNECTED
def _link_quality_cb(self, percentage):
"""Called from link driver to report link quality"""
self.link_quality_updated.call(percentage)
def _check_for_initial_packet_cb(self, data):
"""
Called when first packet arrives from Crazyflie.
This is used to determine if we are connected to something that is
answering.
"""
self.state = State.CONNECTED
self.link_established.call(self.link_uri)
self.packet_received.remove_callback(self._check_for_initial_packet_cb)
def open_link(self, link_uri):
"""
Open the communication link to a copter at the given URI and setup the
connection
"""
self.connection_requested.call(link_uri)
self.state = State.INITIALIZED
self.link_uri = link_uri
try:
self.link = cflib.crtp.get_link_driver(link_uri,
self._link_quality_cb,
self._link_error_cb)
if not self.link:
message = 'No driver found or malformed URI: {}' \
.format(link_uri)
logger.warning(message)
self.connection_failed.call(link_uri, message)
else:
# Add a callback so we can check that any data is coming
# back from the copter
self.packet_received.add_callback(
self._check_for_initial_packet_cb)
logger.info('Connection setup finished')
self.connected_ts = datetime.datetime.now()
self.connected.call(self.link_uri)
except Exception as ex: # pylint: disable=W0703
# We want to catch every possible exception here and show
# it in the user interface
import traceback
logger.error("Couldn't load link driver: %s\n\n%s", ex,
traceback.format_exc())
exception_text = "Couldn't load link driver: %s\n\n%s" % (
ex, traceback.format_exc())
if self.link:
self.link.close()
self.link = None
self.connection_failed.call(link_uri, exception_text)
def close_link(self):
"""Close the communication link."""
logger.info('Closing link')
if (self.link is not None):
self.commander.send_setpoint(0, 0, 0, 0)
if (self.link is not None):
self.link.close()
self.link = None
self.disconnected.call(self.link_uri)
def is_connected(self):
"""Check if the communication link is open or not."""
return self.connected_ts is not None
def send_packet(self, pk):
"""
Send a packet through the link interface.
pk -- Packet to send
"""
self._send_lock.acquire()
if self.link is not None:
self.link.send_packet(pk)
# self.packet_sent.call(pk)
self._send_lock.release()
class Memory():
"""Access memories on the Crazyflie"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very 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 JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True):
# TODO: Should be OS dependant
self.SF = sensorFusion()
self.inputdevice = PyGameReader()
self.pointerDevice = psmove.PSMove()
self.PointerYaw = 0
self.kalmanPitch = KalmanFilter()
self.kalmanRoll = KalmanFilter()
self.viscousModeThrust = 67
self._emergency_landing = False
self.auto = False
self._min_thrust = 0
self._max_thrust = 0
self._maxAltitude = 0
self.currentAltitude = 0
self.minAltitude = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._canSwitch = True
self._old_thrust = 0
self._old_alt_hold = False
self._old_flip_type = -1
self._flip_time_start = -float("inf");
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
self._trim_yaw = 0.0
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(
Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("min_thrust"), Config().get("max_thrust"))
self.set_thrust_slew_limiting(
Config().get("slew_rate"), Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.switch_mode_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
self.auto_input_updated = Caller()
self.pointer_input_updated = Caller()
def setViscousModeThrust(self, thrust):
if thrust >= 0:
self.viscousModeThrust = thrust
def setEmergencyLanding(self, emergencyLanding):
self._emergency_landing = emergencyLanding
def setAltHoldAvailable(self, available):
self._has_pressure_sensor = available
def setAuto(self, auto):
self.auto = auto
def setAltHold(self, althold):
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = self.inputdevice.getAvailableDevices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist) or
(self._dev_blacklist and not
self._dev_blacklist.match(dev["name"]))):
self._available_devices[dev["name"]] = dev["id"]
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self._read_timer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self._read_timer.stop()
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
self._max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
self._max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if (thrust_slew_rate > 0):
self._thrust_slew_enabled = True
else:
self._thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
self._min_thrust = JoystickReader.p2t(min_thrust)
self._max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def setMaxAltitude(self, maxAltitude):
self._maxAltitude = maxAltitude
def setCurrentAltitude(self, altitude):
if altitude < self.minAltitude or self.minAltitude == 0:
self.minAltitude = altitude
self.currentAltitude = altitude
def read_input(self):
"""Read input data from the selected device"""
if self.pointerDevice is not None:
#DT = 0.001
if self.pointerDevice.poll():
buttons = self.pointerDevice.get_buttons()
if buttons & psmove.Btn_MOVE:
self.pointerDevice.set_leds(0, 255, 0)
self.pointerDevice.update_leds()
self.SF = sensorFusion()
'''
trigger_value = self.move.get_trigger()
self.move.set_leds(trigger_value, 0, 0)
self.move.update_leds()
'''
ax, ay, az = self.pointerDevice.get_accelerometer_frame(psmove.Frame_SecondHalf)
gx, gy, gz = self.pointerDevice.get_gyroscope_frame(psmove.Frame_SecondHalf)
gx = gx * 180 / math.pi
gy = gy * 180 / math.pi
gz = gz * 180 / math.pi
#print "A: %5.2f %5.2f %5.2f " % ( ax , ay , az )
#print "G: %8.2f %8.2f %8.2f " % ( gx , gy , gz )
self.SF.sensfusion6UpdateQ(gx, gy, gz, ax, ay, az, 1/100)
roll, pitch, yaw = self.SF.sensfusion6GetEulerRPY()
self.PointerYaw = -yaw
'''
# Read sensor
acc_x = self.pointerDevice.ax
acc_y = self.pointerDevice.ay
acc_z = self.pointerDevice.az
gyr_x = self.pointerDevice.gx
gyr_y = self.pointerDevice.gy
gyr_z = self.pointerDevice.gz
#// Calculate pitch and roll based only on acceleration.
acc_pitch = math.atan2(acc_x, -acc_z)
acc_roll = -math.atan2(acc_y, -acc_z)
# Perform filtering
self.kalmanPitch.kalman_innovate(acc_pitch, gyr_x, DT)
self.kalmanRoll.kalman_innovate(acc_roll, gyr_y, DT)
# The angle is stored in state 1
pitch = self.kalmanPitch.x1
roll = self.kalmanRoll.x1
cosRoll = math.cos(roll)
sinRoll = math.sin(roll)
cosPitch = math.cos(pitch)
sinPitch = math.sin(pitch)
magX = self.pointerDevice.mx * cosPitch + self.pointerDevice.mz * sinPitch
magY = self.pointerDevice.mx * sinRoll * sinPitch + self.pointerDevice.my * cosRoll - self.pointerDevice.mz * sinRoll * cosPitch
norm = math.sqrt(magX*magX + magY*magY)
magHeadingX = magX/norm
magHeadingY = -magY/norm
#Absolute Yaw
#self.PointerYaw = self.pointerDevice.mz
#roll = self.pointerDevice.gy - self.pointerDevice.gy/GYROSCOPE_SENSITIVITY*dt
#pitch = self.pointerDevice.gx - self.pointerDevice.gx/GYROSCOPE_SENSITIVITY*dt
DT = 0.001
yaw = self.pointerDevice.gz - self.pointerDevice.gz/GYROSCOPE_SENSITIVITY*DT
self.PointerYaw -= yaw*DT
if self.PointerYaw >= 180:
self.PointerYaw = -180
elif self.PointerYaw <= -180:
self.PointerYaw = 180
if self.pointerDevice.get_buttons() & psmove.Btn_MOVE: #psmove.Btn_T:
self.pointerDevice.set_leds(0, 255, 0)
self.pointerDevice.update_leds()
self.PointerYaw = 0
'''
if self.PointerYaw >= 0:
self.pointerDevice.set_leds(int(255*self.PointerYaw/180), 255, 0)
else:
self.pointerDevice.set_leds(0, 255, int(255*math.fabs(self.PointerYaw)/180))
self.pointerDevice.update_leds()
self.pointer_input_updated.call(self.PointerYaw, False)
try:
data = self.inputdevice.read_input()
roll = data["roll"] * self._max_rp_angle
pitch = data["pitch"] * self._max_rp_angle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
althold = data["althold"]
flipleft = data["flipleft"]
flipright = data["flipright"]
viscousMode = data["viscousMode"]
switchMode = data["switchmode"]
if switchMode and self._canSwitch:
self._canSwitch = False
self.switch_mode_updated.call()
elif not switchMode:
self._canSwitch = True
if (self._old_alt_hold != althold):
self.althold_updated.call(althold)
self._old_alt_hold = althold
if self._emergency_stop != emergency_stop:
self._emergency_stop = emergency_stop
self.emergency_stop_updated.call(self._emergency_stop)
if self.auto:
self.auto_input_updated.call(trim_roll, trim_pitch, yaw, thrust)
else:
# Altitude Hold Mode Toggled
if (self._old_alt_hold != althold):
self.althold_updated.call(althold)
self._old_alt_hold = althold
# Disable hover mode if enabled
if self._emergency_stop:
if self._has_pressure_sensor:
if self._old_alt_hold:
self.althold_updated.call(False)
self._old_alt_hold = False
althold = False
'''
modalità in cui il quad rimane fermo in altitudine e può salire o scendere in base a come si
utilizza il joystick
thrust up (>0) => sale (costantemente)
thrust down (<0) => scende (costantemente)
'''
if viscousMode:
viscous_thrust = self.p2t(self.viscousModeThrust)
if raw_thrust > 0 and raw_thrust <= 0.5:
raw_thrust = 1
elif raw_thrust > 0.5:
raw_thrust = 2
elif raw_thrust >= -0.5 and raw_thrust < 0:
raw_thrust = -0.5
elif raw_thrust < -0.5:
raw_thrust = -1
'''
if (self.currentAltitude - self.minAltitude) == self._maxAltitude:
raw_thrust = 0
elif (self.currentAltitude - self.minAltitude) > self._maxAltitude:
raw_thrust = -0.2
'''
thrust = int(round(viscous_thrust + raw_thrust*self.p2t(10)))
# Thust limiting (slew, minimum and emergency stop)
elif (althold and self._has_pressure_sensor) or (flipleft or flipright):
thrust = int(round(JoystickReader.deadband(thrust,0.2)*32767 + 32767)) #Convert to uint16
else:
if raw_thrust < 0.05 or emergency_stop:
thrust = 0
else:
thrust = self._min_thrust + thrust * (self._max_thrust - self._min_thrust)
if (self._thrust_slew_enabled == True and self._thrust_slew_limit > thrust and not emergency_stop):
#if (self._thrust_slew_enabled == True and not emergency_stop):
if self._old_thrust > self._thrust_slew_limit:
self._old_thrust = self._thrust_slew_limit
if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)):
thrust = self._old_thrust - self._thrust_slew_rate / 100
if raw_thrust < 0 or thrust < self._min_thrust:
thrust = 0
#if trim_pitch > 0:
# thrust += self.p2t(1)
#if trim_pitch < 0:
# thrust -= self.p2t(1)
if self._emergency_landing:
thrust = self._old_thrust - self.p2t(10)*0.2
if thrust < 0: thrust = 0
self._old_thrust = thrust
# Yaw deadband
# TODO: Add to input device config?
yaw = JoystickReader.deadband(yaw,0.2)*self._max_yaw_rate
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
if (flipleft or flipright) and self._flip_time_start < 0:
self._flip_time_start = time.time(); #ricavo il momento in cui inizia il flip
if flipleft:
self._old_flip_type = 0;
if flipright:
self._old_flip_type = 1;
#if (flipleft and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 0:
if flipleft and self._old_flip_type == 0:
thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente
roll = 1600
#elif (flipright and self.flipTimeControl(self._flip_time_start)) and self._old_flip_type == 1:
elif flipright and self._old_flip_type == 1:
#thrust = self.p2t(70)
#roll = 30
#self.input_updated.call(roll, 0, yaw, thrust)
#time.sleep(0.04)
thrust = self.p2t(50)
roll = -1000
self.input_updated.call(roll, 0, yaw, thrust)
#time.sleep(FLIP_TIME)
'''
#######
## 1 ##
#######
thrust = self.p2t(70) #faccio in modo che il robot rimanga nella posizione corrente
roll = 30
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 2 ##
#######
thrust = self.p2t(50)
roll = -1600
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 3 ##
#######
thrust = 0
roll = 0
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.0004)
#######
## 4 ##
#######
thrust = self.p2t(50)
roll = -1600
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 5 ##
#######
thrust = self.p2t(70)
roll = 30
self.input_updated.call(roll, 0, yaw, thrust)
time.sleep(0.004)
#######
## 6 ##
#######
thrust = self.p2t(53)
roll = 0
self.input_updated.call(roll, 0, yaw, thrust)
return;
'''
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
if not flipleft and not flipright and not self.flipTimeControl(self._flip_time_start):
self._old_flip_type = -1;
self._flip_time_start = -float("inf"); #resetto _flip_time_start
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
#yaw = yaw + self._trim_yaw
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s", traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" % traceback.format_exc())
self._read_timer.stop()
def update_trim_yaw_signal(self, yaw):
self._trim_yaw = yaw
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value/(1-threshold)
@staticmethod
def flipTimeControl(startTime):
return (time.time()-startTime >= 0 and time.time()-startTime <= FLIP_TIME)
class JoystickReader:
"""
Thread that will read input from devices/joysticks and send control-set
ponts to the Crazyflie
"""
inputConfig = []
def __init__(self, do_device_discovery=True, cf=None):
# TODO: Should be OS dependant
self.inputdevice = AiController(cf)#PyGameReader()
self._min_thrust = 0
self._max_thrust = 0
self._thrust_slew_rate = 0
self._thrust_slew_enabled = False
self._thrust_slew_limit = 0
self._emergency_stop = False
self._has_pressure_sensor = False
self._old_thrust = 0
self._old_alt_hold = False
self._trim_roll = Config().get("trim_roll")
self._trim_pitch = Config().get("trim_pitch")
if (Config().get("flightmode") is "Normal"):
self._max_yaw_rate = Config().get("normal_max_yaw")
self._max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("normal_min_thrust"),
Config().get("normal_max_thrust"))
self.set_thrust_slew_limiting(
Config().get("normal_slew_rate"),
Config().get("normal_slew_limit"))
else:
self._max_yaw_rate = Config().get("max_yaw")
self._max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.set_thrust_limits(
Config().get("min_thrust"), Config().get("max_thrust"))
self.set_thrust_slew_limiting(
Config().get("slew_rate"), Config().get("slew_limit"))
self._dev_blacklist = None
if (len(Config().get("input_device_blacklist")) > 0):
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(0.01, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(sys.path[0] +
"/cfclient/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().
configs_dir, os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.althold_updated = Caller()
def setAltHoldAvailable(self, available):
self._has_pressure_sensor = available
def setAltHold(self, althold):
self._old_alt_hold = althold
def _do_device_discovery(self):
devs = self.getAvailableDevices()
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def getAvailableDevices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = self.inputdevice.getAvailableDevices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist) or
(self._dev_blacklist and not
self._dev_blacklist.match(dev["name"]))):
self._available_devices[dev["name"]] = dev["id"]
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, deviceId):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
self.inputdevice.enableRawReading(deviceId)
def disableRawReading(self):
"""Disable raw reading of input device."""
self.inputdevice.disableRawReading()
def readRawValues(self):
""" Read raw values from the input device."""
return self.inputdevice.readRawValues()
def start_input(self, device_name, config_name):
"""
Start reading input from the device with name device_name using config
config_name
"""
try:
device_id = self._available_devices[device_name]
self.inputdevice.start_input(
device_id,
ConfigManager().get_config(config_name))
self._read_timer.start()
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
def stop_input(self):
"""Stop reading from the input device."""
self._read_timer.stop()
def set_yaw_limit(self, max_yaw_rate):
"""Set a new max yaw rate value."""
self._max_yaw_rate = max_yaw_rate
def set_rp_limit(self, max_rp_angle):
"""Set a new max roll/pitch value."""
self._max_rp_angle = max_rp_angle
def set_thrust_slew_limiting(self, thrust_slew_rate, thrust_slew_limit):
"""Set new values for limit where the slewrate control kicks in and
for the slewrate."""
self._thrust_slew_rate = JoystickReader.p2t(thrust_slew_rate)
self._thrust_slew_limit = JoystickReader.p2t(thrust_slew_limit)
if (thrust_slew_rate > 0):
self._thrust_slew_enabled = True
else:
self._thrust_slew_enabled = False
def set_thrust_limits(self, min_thrust, max_thrust):
"""Set a new min/max thrust limit."""
self._min_thrust = JoystickReader.p2t(min_thrust)
self._max_thrust = JoystickReader.p2t(max_thrust)
def set_trim_roll(self, trim_roll):
"""Set a new value for the roll trim."""
self._trim_roll = trim_roll
def set_trim_pitch(self, trim_pitch):
"""Set a new value for the trim trim."""
self._trim_pitch = trim_pitch
def read_input(self):
"""Read input data from the selected device"""
try:
data = self.inputdevice.read_input()
roll = data["roll"] * self._max_rp_angle
pitch = data["pitch"] * self._max_rp_angle
thrust = data["thrust"]
yaw = data["yaw"]
raw_thrust = data["thrust"]
emergency_stop = data["estop"]
trim_roll = data["rollcal"]
trim_pitch = data["pitchcal"]
althold = data["althold"]
if (self._old_alt_hold != althold):
self.althold_updated.call(str(althold))
self._old_alt_hold = althold
if self._emergency_stop != emergency_stop:
self._emergency_stop = emergency_stop
self.emergency_stop_updated.call(self._emergency_stop)
# Thust limiting (slew, minimum and emergency stop)
if althold and self._has_pressure_sensor:
thrust = int(round(JoystickReader.deadband(thrust,0.2)*32767 + 32767)) #Convert to uint16
else:
if raw_thrust < 0.05 or emergency_stop:
thrust = 0
else:
thrust = self._min_thrust + thrust * (self._max_thrust -
self._min_thrust)
if (self._thrust_slew_enabled == True and
self._thrust_slew_limit > thrust and not
emergency_stop):
if self._old_thrust > self._thrust_slew_limit:
self._old_thrust = self._thrust_slew_limit
if thrust < (self._old_thrust - (self._thrust_slew_rate / 100)):
thrust = self._old_thrust - self._thrust_slew_rate / 100
if raw_thrust < 0 or thrust < self._min_thrust:
thrust = 0
self._old_thrust = thrust
# Yaw deadband
# TODO: Add to input device config?
yaw = JoystickReader.deadband(yaw,0.2)*self._max_yaw_rate
if trim_roll != 0 or trim_pitch != 0:
self._trim_roll += trim_roll
self._trim_pitch += trim_pitch
self.rp_trim_updated.call(self._trim_roll, self._trim_pitch)
trimmed_roll = roll + self._trim_roll
trimmed_pitch = pitch + self._trim_pitch
self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
@staticmethod
def deadband(value, threshold):
if abs(value) < threshold:
value = 0
elif value > 0:
value -= threshold
elif value < 0:
value += threshold
return value/(1-threshold)
class LogConfig(object):
"""Representation of one log configuration that enables logging
from the Crazyflie"""
_config_id_counter = 1
def __init__(self, name, period_in_ms):
"""Initialize the entry"""
self.data_received_cb = Caller()
self.error_cb = Caller()
self.started_cb = Caller()
self.added_cb = Caller()
self.err_no = 0
self.id = LogConfig._config_id_counter
LogConfig._config_id_counter += 1 % 255
self.cf = None
self.period = period_in_ms / 10
self.period_in_ms = period_in_ms
self._added = False
self._started = False
self.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def add_variable(self, name, fetch_as=None):
"""Add a new variable to the configuration.
name - Complete name of the variable in the form group.name
fetch_as - String representation of the type the variable should be
fetched as (i.e uint8_t, float, FP16, etc)
If no fetch_as type is supplied, then the stored as type will be used
(i.e the type of the fetched variable is the same as it's stored in the
Crazyflie)."""
if fetch_as:
self.variables.append(LogVariable(name, fetch_as))
else:
# We cannot determine the default type until we have connected. So
# save the name and we will add these once we are connected.
self.default_fetch_as.append(name)
def add_memory(self, name, fetch_as, stored_as, address):
"""Add a raw memory position to log.
name - Arbitrary name of the variable
fetch_as - String representation of the type of the data the memory
should be fetch as (i.e uint8_t, float, FP16)
stored_as - String representation of the type the data is stored as
in the Crazyflie
address - The address of the data
"""
self.variables.append(LogVariable(name, fetch_as, LogVariable.MEM_TYPE,
stored_as, address))
def _set_added(self, added):
self._added = added
self.added_cb.call(added)
def _get_added(self):
return self._added
def _set_started(self, started):
self._started = started
self.started_cb.call(started)
def _get_started(self):
return self._started
added = property(_get_added, _set_added)
started = property(_get_started, _set_started)
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
logger.debug("First time block is started, add block")
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_CREATE_BLOCK, self.id)
for var in self.variables:
if (var.is_toc_variable() is False): # Memory location
logger.debug("Logging to raw memory %d, 0x%04X",
var.get_storage_and_fetch_byte(), var.address)
pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<I', var.address)
else: # Item in TOC
logger.debug("Adding %s with id=%d and type=0x%02X",
var.name,
self.cf.log.toc.get_element_id(
var.name), var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', var.get_storage_and_fetch_byte())
pk.data += struct.pack('<B', self.cf.log.toc.
get_element_id(var.name))
logger.debug("Adding log block id {}".format(self.id))
self.cf.send_packet(pk, expect_answer=True)
else:
logger.debug("Block already registered, starting logging"
" for %d", self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.cf.send_packet(pk, expect_answer=True)
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning("Stopping block, but no block registered")
else:
logger.debug("Sending stop logging for block %d", self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.cf.send_packet(pk, expect_answer=True)
def delete(self):
"""Delete this entry in the Crazyflie"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning("Delete block, but no block registered")
else:
logger.debug("LogEntry: Sending delete logging for block %d"
% self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.cf.send_packet(pk, expect_answer=True)
def unpack_log_data(self, log_data, timestamp):
"""Unpack received logging data so it represent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.variables:
size = LogTocElement.get_size_from_id(var.fetch_as)
name = var.name
unpackstring = LogTocElement.get_unpack_string_from_id(
var.fetch_as)
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received_cb.call(timestamp, ret_data, self)
class Localization():
"""
Handle localization-related data communication with the Crazyflie
"""
# Implemented channels
POSITION_CH = 0
GENERIC_CH = 1
# Location message types for generig channel
RANGE_STREAM_REPORT = 0
RANGE_STREAM_REPORT_FP16 = 1
LPS_SHORT_LPP_PACKET = 2
EMERGENCY_STOP = 3
EMERGENCY_STOP_WATCHDOG = 4
COMM_GNSS_NMEA = 6
COMM_GNSS_PROPRIETARY = 7
EXT_POSE = 8
EXT_POSE_PACKED = 9
LH_ANGLE_STREAM = 10
LH_PERSIST_DATA = 11
def __init__(self, crazyflie=None):
"""
Initialize the Extpos object.
"""
self._cf = crazyflie
self.receivedLocationPacket = Caller()
self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def _incoming(self, packet):
"""
Callback for data received from the copter.
"""
if len(packet.data) < 1:
logger.warning('Localization packet received with incorrect' +
'length (length is {})'.format(len(packet.data)))
return
pk_type = struct.unpack('<B', packet.data[:1])[0]
data = packet.data[1:]
# Decoding the known packet types
# TODO: more generic decoding scheme?
decoded_data = None
if pk_type == self.RANGE_STREAM_REPORT:
if len(data) % 5 != 0:
logger.error('Wrong range stream report data lenght')
return
decoded_data = {}
raw_data = data
for i in range(int(len(data) / 5)):
anchor_id, distance = struct.unpack('<Bf', raw_data[:5])
decoded_data[anchor_id] = distance
raw_data = raw_data[5:]
elif pk_type == self.LH_PERSIST_DATA:
decoded_data = bool(data[0])
elif pk_type == self.LH_ANGLE_STREAM:
decoded_data = self._decode_lh_angle(data)
pk = LocalizationPacket(pk_type, data, decoded_data)
self.receivedLocationPacket.call(pk)
def _decode_lh_angle(self, data):
decoded_data = {}
raw_data = struct.unpack('<Bfhhhfhhh', data)
decoded_data['basestation'] = raw_data[0]
decoded_data['x'] = [0, 0, 0, 0]
decoded_data['x'][0] = raw_data[1]
decoded_data['x'][1] = raw_data[1] - fp16_to_float(raw_data[2])
decoded_data['x'][2] = raw_data[1] - fp16_to_float(raw_data[3])
decoded_data['x'][3] = raw_data[1] - fp16_to_float(raw_data[4])
decoded_data['y'] = [0, 0, 0, 0]
decoded_data['y'][0] = raw_data[5]
decoded_data['y'][1] = raw_data[5] - fp16_to_float(raw_data[6])
decoded_data['y'][2] = raw_data[5] - fp16_to_float(raw_data[7])
decoded_data['y'][3] = raw_data[5] - fp16_to_float(raw_data[8])
return decoded_data
def send_extpos(self, pos):
"""
Send the current Crazyflie X, Y, Z position. This is going to be
forwarded to the Crazyflie's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.POSITION_CH
pk.data = struct.pack('<fff', pos[0], pos[1], pos[2])
self._cf.send_packet(pk)
def send_extpose(self, pos, quat):
"""
Send the current Crazyflie pose (position [x, y, z] and
attitude quaternion [qx, qy, qz, qw]). This is going to be forwarded
to the Crazyflie's position estimator.
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<Bfffffff', self.EXT_POSE, pos[0], pos[1],
pos[2], quat[0], quat[1], quat[2], quat[3])
self._cf.send_packet(pk)
def send_short_lpp_packet(self, dest_id, data):
"""
Send ultra-wide-band LPP packet to dest_id
"""
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BB', self.LPS_SHORT_LPP_PACKET, dest_id) + data
self._cf.send_packet(pk)
def send_lh_persist_data_packet(self, geo_list, calib_list):
"""
Send geometry and calibration data to persistent memory subsystem
"""
geo_list.sort()
calib_list.sort()
max_bs_nr = 15
if len(geo_list) > 0:
if geo_list[0] < 0 or geo_list[-1] > max_bs_nr:
raise Exception('Geometry BS list is not valid')
if len(calib_list) > 0:
if calib_list[0] < 0 or calib_list[-1] > max_bs_nr:
raise Exception('Calibration BS list is not valid')
mask_geo = 0
mask_calib = 0
for bs in geo_list:
mask_geo += 1 << bs
for bs in calib_list:
mask_calib += 1 << bs
pk = CRTPPacket()
pk.port = CRTPPort.LOCALIZATION
pk.channel = self.GENERIC_CH
pk.data = struct.pack('<BHH', self.LH_PERSIST_DATA, mask_geo,
mask_calib)
self._cf.send_packet(pk)
return pk.data
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]
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 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 JoystickReader(object):
"""
Thread that will read input from devices/joysticks and send control-set
points to the Crazyflie
"""
inputConfig = []
ASSISTED_CONTROL_ALTHOLD = 0
ASSISTED_CONTROL_POSHOLD = 1
ASSISTED_CONTROL_HEIGHTHOLD = 2
ASSISTED_CONTROL_HOVER = 3
def __init__(self, do_device_discovery=True):
self._input_device = None
self._mux = [
NoMux(self),
TakeOverSelectiveMux(self),
TakeOverMux(self)
]
# Set NoMux as default
self._selected_mux = self._mux[0]
self.min_thrust = 0
self.max_thrust = 0
self._thrust_slew_rate = 0
self.thrust_slew_enabled = False
self.thrust_slew_limit = 0
self.has_pressure_sensor = False
self._hover_max_height = MAX_TARGET_HEIGHT
self.max_rp_angle = 0
self.max_yaw_rate = 0
try:
self.set_assisted_control(Config().get("assistedControl"))
except KeyError:
self.set_assisted_control(JoystickReader.ASSISTED_CONTROL_ALTHOLD)
self._old_thrust = 0
self._old_raw_thrust = 0
self.springy_throttle = True
self._target_height = INITAL_TAGET_HEIGHT
self.trim_roll = Config().get("trim_roll")
self.trim_pitch = Config().get("trim_pitch")
self._rp_dead_band = 0.1
self._input_map = None
if Config().get("flightmode") == "Normal":
self.max_yaw_rate = Config().get("normal_max_yaw")
self.max_rp_angle = Config().get("normal_max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("normal_min_thrust")
self.max_thrust = Config().get("normal_max_thrust")
self.thrust_slew_limit = Config().get("normal_slew_limit")
self.thrust_slew_rate = Config().get("normal_slew_rate")
else:
self.max_yaw_rate = Config().get("max_yaw")
self.max_rp_angle = Config().get("max_rp")
# Values are stored at %, so use the functions to set the values
self.min_thrust = Config().get("min_thrust")
self.max_thrust = Config().get("max_thrust")
self.thrust_slew_limit = Config().get("slew_limit")
self.thrust_slew_rate = Config().get("slew_rate")
self._dev_blacklist = None
if len(Config().get("input_device_blacklist")) > 0:
self._dev_blacklist = re.compile(
Config().get("input_device_blacklist"))
logger.info("Using device blacklist [{}]".format(
Config().get("input_device_blacklist")))
self._available_devices = {}
# TODO: The polling interval should be set from config file
self._read_timer = PeriodicTimer(INPUT_READ_PERIOD, self.read_input)
if do_device_discovery:
self._discovery_timer = PeriodicTimer(1.0,
self._do_device_discovery)
self._discovery_timer.start()
# Check if user config exists, otherwise copy files
if not os.path.exists(ConfigManager().configs_dir):
logger.info("No user config found, copying dist files")
os.makedirs(ConfigManager().configs_dir)
for f in glob.glob(cfclient.module_path +
"/configs/input/[A-Za-z]*.json"):
dest = os.path.join(ConfigManager().configs_dir,
os.path.basename(f))
if not os.path.isfile(dest):
logger.debug("Copying %s", f)
shutil.copy2(f, ConfigManager().configs_dir)
ConfigManager().get_list_of_configs()
self.input_updated = Caller()
self.assisted_input_updated = Caller()
self.heighthold_input_updated = Caller()
self.hover_input_updated = Caller()
self.rp_trim_updated = Caller()
self.emergency_stop_updated = Caller()
self.device_discovery = Caller()
self.device_error = Caller()
self.assisted_control_updated = Caller()
self.alt1_updated = Caller()
self.alt2_updated = Caller()
# Call with 3 bools (rp_limiting, yaw_limiting, thrust_limiting)
self.limiting_updated = Caller()
def _get_device_from_name(self, device_name):
"""Get the raw device from a name"""
for d in readers.devices():
if d.name == device_name:
return d
return None
def set_hover_max_height(self, height):
self._hover_max_height = height
def set_alt_hold_available(self, available):
"""Set if altitude hold is available or not (depending on HW)"""
self.has_pressure_sensor = available
def _do_device_discovery(self):
devs = self.available_devices()
# This is done so that devs can easily get access
# to limits without creating lots of extra code
for d in devs:
d.input = self
if len(devs):
self.device_discovery.call(devs)
self._discovery_timer.stop()
def available_mux(self):
return self._mux
def set_mux(self, name=None, mux=None):
old_mux = self._selected_mux
if name:
for m in self._mux:
if m.name == name:
self._selected_mux = m
elif mux:
self._selected_mux = mux
old_mux.close()
logger.info("Selected MUX: {}".format(self._selected_mux.name))
def set_assisted_control(self, mode):
self._assisted_control = mode
def get_assisted_control(self):
return self._assisted_control
def available_devices(self):
"""List all available and approved input devices.
This function will filter available devices by using the
blacklist configuration and only return approved devices."""
devs = readers.devices()
devs += interfaces.devices()
approved_devs = []
for dev in devs:
if ((not self._dev_blacklist)
or (self._dev_blacklist
and not self._dev_blacklist.match(dev.name))):
dev.input = self
approved_devs.append(dev)
return approved_devs
def enableRawReading(self, device_name):
"""
Enable raw reading of the input device with id deviceId. This is used
to get raw values for setting up of input devices. Values are read
without using a mapping.
"""
if self._input_device:
self._input_device.close()
self._input_device = None
for d in readers.devices():
if d.name == device_name:
self._input_device = d
# Set the mapping to None to get raw values
self._input_device.input_map = None
self._input_device.open()
def get_saved_device_mapping(self, device_name):
"""Return the saved mapping for a given device"""
config = None
device_config_mapping = Config().get("device_config_mapping")
if device_name in list(device_config_mapping.keys()):
config = device_config_mapping[device_name]
logging.debug("For [{}] we recommend [{}]".format(device_name, config))
return config
def stop_raw_reading(self):
"""Disable raw reading of input device."""
if self._input_device:
self._input_device.close()
self._input_device = None
def read_raw_values(self):
""" Read raw values from the input device."""
[axes, buttons,
mapped_values] = self._input_device.read(include_raw=True)
dict_axes = {}
dict_buttons = {}
for i, a in enumerate(axes):
dict_axes[i] = a
for i, b in enumerate(buttons):
dict_buttons[i] = b
return [dict_axes, dict_buttons, mapped_values]
def set_raw_input_map(self, input_map):
"""Set an input device map"""
# TODO: Will not work!
if self._input_device:
self._input_device.input_map = input_map
def set_input_map(self, device_name, input_map_name):
"""Load and set an input device map with the given name"""
dev = self._get_device_from_name(device_name)
settings = ConfigManager().get_settings(input_map_name)
if settings:
self.springy_throttle = settings["springythrottle"]
self._rp_dead_band = settings["rp_dead_band"]
self._input_map = ConfigManager().get_config(input_map_name)
dev.input_map = self._input_map
dev.input_map_name = input_map_name
Config().get("device_config_mapping")[device_name] = input_map_name
dev.set_dead_band(self._rp_dead_band)
def start_input(self, device_name, role="Device", config_name=None):
"""
Start reading input from the device with name device_name using config
config_name. Returns True if device supports mapping, otherwise False
"""
try:
# device_id = self._available_devices[device_name]
# Check if we supplied a new map, if not use the preferred one
device = self._get_device_from_name(device_name)
self._selected_mux.add_device(device, role)
# Update the UI with the limiting for this device
self.limiting_updated.call(device.limit_rp, device.limit_yaw,
device.limit_thrust)
self._read_timer.start()
return device.supports_mapping
except Exception:
self.device_error.call(
"Error while opening/initializing input device\n\n%s" %
(traceback.format_exc()))
if not self._input_device:
self.device_error.call(
"Could not find device {}".format(device_name))
return False
def resume_input(self):
self._selected_mux.resume()
self._read_timer.start()
def pause_input(self, device_name=None):
"""Stop reading from the input device."""
self._read_timer.stop()
self._selected_mux.pause()
def _set_thrust_slew_rate(self, rate):
self._thrust_slew_rate = rate
if rate > 0:
self.thrust_slew_enabled = True
else:
self.thrust_slew_enabled = False
def _get_thrust_slew_rate(self):
return self._thrust_slew_rate
def read_input(self):
"""Read input data from the selected device"""
try:
data = self._selected_mux.read()
if data:
if data.toggled.assistedControl:
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD or \
self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER:
if data.assistedControl and self._assisted_control != \
JoystickReader.ASSISTED_CONTROL_HOVER:
for d in self._selected_mux.devices():
d.limit_thrust = False
d.limit_rp = False
elif data.assistedControl:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = False
else:
for d in self._selected_mux.devices():
d.limit_thrust = True
d.limit_rp = True
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_ALTHOLD:
self.assisted_control_updated.call(
data.assistedControl)
if ((self._assisted_control
== JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD)
or (self._assisted_control
== JoystickReader.ASSISTED_CONTROL_HOVER)):
try:
self.assisted_control_updated.call(
data.assistedControl)
if not data.assistedControl:
# Reset height controller state to initial
# target height both in the UI and in the
# Crazyflie.
# TODO: Implement a proper state update of the
# input layer
self.heighthold_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
self.hover_input_updated.\
call(0, 0,
0, INITAL_TAGET_HEIGHT)
except Exception as e:
logger.warning(
"Exception while doing callback from "
"input-device for assited "
"control: {}".format(e))
if data.toggled.estop:
try:
self.emergency_stop_updated.call(data.estop)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for estop: {}".format(e))
if data.toggled.alt1:
try:
self.alt1_updated.call(data.alt1)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt1: {}".format(e))
if data.toggled.alt2:
try:
self.alt2_updated.call(data.alt2)
except Exception as e:
logger.warning("Exception while doing callback from"
"input-device for alt2: {}".format(e))
# Reset height target when height-hold is not selected
if not data.assistedControl or \
(self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD and
self._assisted_control !=
JoystickReader.ASSISTED_CONTROL_HOVER):
self._target_height = INITAL_TAGET_HEIGHT
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_POSHOLD \
and data.assistedControl:
vx = data.roll
vy = data.pitch
vz = data.thrust
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.assisted_input_updated.call(vy, -vx, vz, yawrate)
elif self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HOVER \
and data.assistedControl:
vx = data.roll
vy = data.pitch
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_HOVER_HEIGHT:
self._target_height = MIN_HOVER_HEIGHT
yawrate = data.yaw
# The odd use of vx and vy is to map forward on the
# physical joystick to positiv X-axis
self.hover_input_updated.call(vy, -vx, yawrate,
self._target_height)
else:
# Update the user roll/pitch trim from device
if data.toggled.pitchNeg and data.pitchNeg:
self.trim_pitch -= .2
if data.toggled.pitchPos and data.pitchPos:
self.trim_pitch += .2
if data.toggled.rollNeg and data.rollNeg:
self.trim_roll -= .2
if data.toggled.rollPos and data.rollPos:
self.trim_roll += .2
if data.toggled.pitchNeg or data.toggled.pitchPos or \
data.toggled.rollNeg or data.toggled.rollPos:
self.rp_trim_updated.call(self.trim_roll,
self.trim_pitch)
if self._assisted_control == \
JoystickReader.ASSISTED_CONTROL_HEIGHTHOLD \
and data.assistedControl:
roll = data.roll + self.trim_roll
pitch = data.pitch + self.trim_pitch
yawrate = data.yaw
# Scale thrust to a value between -1.0 to 1.0
vz = (data.thrust - 32767) / 32767.0
# Integrate velosity setpoint
self._target_height += vz * INPUT_READ_PERIOD
# Cap target height
if self._target_height > self._hover_max_height:
self._target_height = self._hover_max_height
if self._target_height < MIN_TARGET_HEIGHT:
self._target_height = MIN_TARGET_HEIGHT
self.heighthold_input_updated.call(
roll, -pitch, yawrate, self._target_height)
else:
# Using alt hold the data is not in a percentage
if not data.assistedControl:
data.thrust = JoystickReader.p2t(data.thrust)
# Thrust might be <0 here, make sure it's not otherwise
# we'll get an error.
if data.thrust < 0:
data.thrust = 0
if data.thrust > 0xFFFF:
data.thrust = 0xFFFF
self.input_updated.call(data.roll + self.trim_roll,
data.pitch + self.trim_pitch,
data.yaw, data.thrust)
else:
self.input_updated.call(0, 0, 0, 0)
except Exception:
logger.warning("Exception while reading inputdevice: %s",
traceback.format_exc())
self.device_error.call("Error reading from input device\n\n%s" %
traceback.format_exc())
self.input_updated.call(0, 0, 0, 0)
self._read_timer.stop()
@staticmethod
def p2t(percentage):
"""Convert a percentage to raw thrust"""
return int(MAX_THRUST * (percentage / 100.0))
thrust_slew_rate = property(_get_thrust_slew_rate, _set_thrust_slew_rate)
class LogConfig(object):
"""Representation of one log configuration that enables logging
from the Crazyflie"""
# Maximum log payload length (4 bytes are used for block id and timestamp)
MAX_LEN = 26
def __init__(self, name, period_in_ms):
"""Initialize the entry"""
self.data_received_cb = Caller()
self.error_cb = Caller()
self.started_cb = Caller()
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
self._started = False
self.pending = False
self.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def add_variable(self, name, fetch_as=None):
"""Add a new variable to the configuration.
name - Complete name of the variable in the form group.name
fetch_as - String representation of the type the variable should be
fetched as (i.e uint8_t, float, FP16, etc)
If no fetch_as type is supplied, then the stored as type will be used
(i.e the type of the fetched variable is the same as it's stored in the
Crazyflie)."""
if fetch_as:
self.variables.append(LogVariable(name, fetch_as))
else:
# We cannot determine the default type until we have connected. So
# save the name and we will add these once we are connected.
self.default_fetch_as.append(name)
def add_memory(self, name, fetch_as, stored_as, address):
"""Add a raw memory position to log.
name - Arbitrary name of the variable
fetch_as - String representation of the type of the data the memory
should be fetch as (i.e uint8_t, float, FP16)
stored_as - String representation of the type the data is stored as
in the Crazyflie
address - The address of the data
"""
self.variables.append(
LogVariable(name, fetch_as, LogVariable.MEM_TYPE, stored_as,
address))
def _set_added(self, added):
if added != self._added:
self.added_cb.call(self, added)
self._added = added
def _get_added(self):
return self._added
def _set_started(self, started):
if started != self._started:
self.started_cb.call(self, started)
self._started = started
def _get_started(self):
return self._started
added = property(_get_added, _set_added)
started = property(_get_started, _set_started)
def _cmd_create_block(self):
if self.useV2:
return CMD_CREATE_BLOCK_V2
else:
return CMD_CREATE_BLOCK
def _cmd_append_block(self):
if self.useV2:
return CMD_APPEND_BLOCK_V2
else:
return CMD_APPEND_BLOCK
def _setup_log_elements(self, pk, next_to_add):
i = next_to_add
for i in range(next_to_add, len(self.variables)):
var = self.variables[i]
if (var.is_toc_variable() is False): # Memory location
logger.debug('Logging to raw memory %d, 0x%04X',
var.get_storage_and_fetch_byte(), var.address)
pk.data.append(
struct.pack('<B', var.get_storage_and_fetch_byte()))
pk.data.append(struct.pack('<I', var.address))
else: # Item in TOC
element_id = self.cf.log.toc.get_element_id(var.name)
logger.debug('Adding %s with id=%d and type=0x%02X', var.name,
element_id, var.get_storage_and_fetch_byte())
pk.data.append(var.get_storage_and_fetch_byte())
if self.useV2:
size_to_add = 2
if pk.available_data_size() >= size_to_add:
pk.data.append(element_id & 0x0ff)
pk.data.append((element_id >> 8) & 0x0ff)
else:
# Packet is full
return False, i
else:
pk.data.append(element_id)
return True, i
def create(self):
"""Save the log configuration in the Crazyflie"""
command = self._cmd_create_block()
next_to_add = 0
is_done = False
num_variables = 0
pending = 0
for block in self.cf.log.log_blocks:
if block.pending or block.added or block.started:
pending += 1
num_variables += len(block.variables)
if pending < Log.MAX_BLOCKS:
#
# The Crazyflie firmware can only handle 128 variables before
# erroring out with ENOMEM.
#
if num_variables + len(self.variables) > Log.MAX_VARIABLES:
raise AttributeError(
('Adding this configuration would exceed max number '
'of variables (%d)' % Log.MAX_VARIABLES))
else:
raise AttributeError(
'Configuration has max number of blocks (%d)' % Log.MAX_BLOCKS)
self.pending += 1
while not is_done:
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (command, self.id)
is_done, next_to_add = self._setup_log_elements(pk, next_to_add)
logger.debug('Adding/appending log block id {}'.format(self.id))
self.cf.send_packet(pk, expected_reply=(command, self.id))
# Use append if we have to add more variables
command = self._cmd_append_block()
def start(self):
"""Start the logging for this entry"""
if (self.cf.link is not None):
if (self._added is False):
self.create()
logger.debug('First time block is started, add block')
else:
logger.debug(
'Block already registered, starting logging'
' for id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, self.id, self.period)
self.cf.send_packet(pk,
expected_reply=(CMD_START_LOGGING,
self.id))
def stop(self):
"""Stop the logging for this entry"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning('Stopping block, but no block registered')
else:
logger.debug('Sending stop logging for block id=%d', self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_STOP_LOGGING, self.id)
self.cf.send_packet(pk,
expected_reply=(CMD_STOP_LOGGING, self.id))
def delete(self):
"""Delete this entry in the Crazyflie"""
if (self.cf.link is not None):
if (self.id is None):
logger.warning('Delete block, but no block registered')
else:
logger.debug(
'LogEntry: Sending delete logging for block id=%d' %
self.id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_DELETE_BLOCK, self.id)
self.cf.send_packet(pk,
expected_reply=(CMD_DELETE_BLOCK, self.id))
def unpack_log_data(self, log_data, timestamp):
"""Unpack received logging data so it represent real values according
to the configuration in the entry"""
ret_data = {}
data_index = 0
for var in self.variables:
size = LogTocElement.get_size_from_id(var.fetch_as)
name = var.name
unpackstring = LogTocElement.get_unpack_string_from_id(
var.fetch_as)
value = struct.unpack(unpackstring,
log_data[data_index:data_index + size])[0]
data_index += size
ret_data[name] = value
self.data_received_cb.call(timestamp, ret_data, self)
class Log():
"""Create log configuration"""
# 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"
}
def __init__(self, crazyflie=None):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
def add_config(self, logconf):
"""Add a log configuration to the logging framework.
When doing this the contents of the log configuration will be validated
and listeners for new log configurations will be notified. When
validating the configuration the variables are checked against the TOC
to see that they actually exist. If they don't then the configuration
cannot be used. Since a valid TOC is required, a Crazyflie has to be
connected when calling this method, otherwise it will fail."""
if not self.cf.link:
logger.error("Cannot add configs without being connected to a "
"Crazyflie!")
return
# If the log configuration contains variables that we added without
# type (i.e we want the stored as type for fetching as well) then
# resolve this now and add them to the block again.
for name in logconf.default_fetch_as:
var = self.toc.get_element_by_complete_name(name)
if not var:
logger.warning("%s not in TOC, this block cannot be"
" used!", name)
logconf.valid = False
return
# Now that we know what type this variable has, add it to the log
# config again with the correct type
logconf.add_variable(name, var.ctype)
# Now check that all the added variables are in the TOC and that
# the total size constraint of a data packet with logging data is
# not
size = 0
for var in logconf.variables:
size += LogTocElement.get_size_from_id(var.fetch_as)
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if var.is_toc_variable():
if (self.toc.get_element_by_complete_name(
var.name) is None):
logger.warning("Log: %s not in TOC, this block cannot be"
" used!", var.name)
logconf.valid = False
return
if (size <= MAX_LOG_DATA_PACKET_SIZE and
(logconf.period > 0 and logconf.period < 0xFF)):
logconf.valid = True
logconf.cf = self.cf
self.log_blocks.append(logconf)
self.block_added_cb.call(logconf)
else:
logconf.valid = False
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.cf.send_packet(pk, expect_answer=True)
self.log_blocks = []
self.toc = Toc()
toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
self.toc, refresh_done_callback, toc_cache)
toc_fetcher.start()
def _find_block(self, id):
for block in self.log_blocks:
if block.id == id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.datal[0]
payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
if (chan == CHAN_SETTINGS):
id = ord(payload[0])
error_status = ord(payload[1])
block = self._find_block(id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if (error_status == 0): # No error
logger.debug("Have successfully added id=%d",
id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, id,
block.period)
self.cf.send_packet(pk, expect_answer=True)
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when adding id=%d (%s)"
, error_status, id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error_cb.call(block, msg)
else:
logger.warning("No LogEntry to assign block to !!!")
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info("Have successfully started logging for block=%d",
id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when starting id=%d (%s)"
, error_status, id, msg)
if block:
block.err_no = error_status
block.started_cb.call(False)
block.error_cb.call(block, msg)
if (cmd == CMD_STOP_LOGGING):
if (error_status == 0x00):
logger.info("Have successfully stopped logging for block=%d",
id)
if block:
block.started = False
if (cmd == CMD_DELETE_BLOCK):
if (error_status == 0x00):
logger.info("Have successfully deleted block=%d",
id)
if block:
block.started = False
block.added = False
if (chan == CHAN_LOGDATA):
chan = packet.channel
id = packet.datal[0]
block = self._find_block(id)
timestamps = struct.unpack("<BBB", packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning("Error no LogEntry to handle id=%d", id)
class Log():
"""Create log configuration"""
# 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"
}
def __init__(self, crazyflie=None):
self.log_blocks = []
# Called with newly created blocks
self.block_added_cb = Caller()
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updated = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
def create_log_packet(self, logconf):
"""Create a new log configuration"""
size = 0
period = logconf.getPeriod() / 10
for var in logconf.getVariables():
size += LogTocElement.get_size_from_id(var.getFetchAs())
# Check that we are able to find the variable in the TOC so
# we can return error already now and not when the config is sent
if (var.isTocVariable()):
if (self.toc.get_element_by_complete_name(
var.getName()) is None):
logger.warning("Log: %s not in TOC, this block cannot be"
" used!", var.getName())
return None
if (size <= MAX_LOG_DATA_PACKET_SIZE and period > 0 and period < 0xFF):
block = LogEntry(self.cf, logconf)
self.log_blocks.append(block)
self.block_added_cb.call(block)
return block
else:
return None
def refresh_toc(self, refresh_done_callback, toc_cache):
"""Start refreshing the table of loggale variables"""
pk = CRTPPacket()
pk.set_header(CRTPPort.LOGGING, CHAN_SETTINGS)
pk.data = (CMD_RESET_LOGGING, )
self.cf.send_packet(pk)
self.log_blocks = []
self.toc = Toc()
toc_fetcher = TocFetcher(self.cf, LogTocElement, CRTPPort.LOGGING,
self.toc, refresh_done_callback, toc_cache)
toc_fetcher.start()
def _find_block(self, block_id):
for block in self.log_blocks:
if block.block_id == block_id:
return block
return None
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets with TOC information"""
chan = packet.channel
cmd = packet.datal[0]
payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
if (chan == CHAN_SETTINGS):
block_id = ord(payload[0])
error_status = ord(payload[1])
block = self._find_block(block_id)
if (cmd == CMD_CREATE_BLOCK):
if (block is not None):
if (error_status == 0): # No error
logger.debug("Have successfully added block_id=%d",
block_id)
pk = CRTPPacket()
pk.set_header(5, CHAN_SETTINGS)
pk.data = (CMD_START_LOGGING, block_id,
block.period)
self.cf.send_packet(pk)
block.added = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when adding block_id=%d (%s)"
, error_status, block_id, msg)
block.err_no = error_status
block.added_cb.call(False)
block.error.call(block.logconf, msg)
else:
logger.warning("No LogEntry to assign block to !!!")
if (cmd == CMD_START_LOGGING):
if (error_status == 0x00):
logger.info("Have successfully logging for block=%d",
block_id)
if block:
block.started = True
else:
msg = self._err_codes[error_status]
logger.warning("Error %d when starting block_id=%d (%s)"
, error_status, new_block_id, msg)
if block:
block.err_no = error_status
block.started_cb.call(False)
block.error.call(block.logconf, msg)
if (chan == CHAN_LOGDATA):
chan = packet.channel
block_id = packet.datal[0]
block = self._find_block(block_id)
timestamps = struct.unpack("<BBB", packet.data[1:4])
timestamp = (timestamps[0] | timestamps[1] << 8 | timestamps[2] << 16)
logdata = packet.data[4:]
if (block is not None):
block.unpack_log_data(logdata, timestamp)
else:
logger.warning("Error no LogEntry to handle block=%d", block_id)
