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()
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 __init__(self, crazyflie=None):
"""Instantiate class and connect callbacks"""
# 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._write_requests_lock = Lock()
self._clear_state()
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 __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._disconnected)
self.all_updated = Caller()
self.is_updated = False
self.values = {}
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 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 __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 __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 __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 __init__(self, crazyflie=None):
"""
Initialize the Extpos object.
"""
self._cf = crazyflie
self.receivedLocationPacket = Caller()
self._cf.add_port_callback(CRTPPort.LOCALIZATION, self._incoming)
def setUp(self):
self.uri = 'radio://0/60/2M'
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(cb=self.cf_mock.connected,
args=[self.uri],
delay=0.2).trigger
self.close_link_mock = AsyncCallbackCaller(
cb=self.cf_mock.disconnected, args=[self.uri], delay=0.2)
self.cf_mock.close_link = self.close_link_mock.trigger
self.sut = SyncCrazyflie(self.uri, self.cf_mock)
class ConfigManager():
""" Singleton class for managing input processing """
conf_needs_reload = Caller()
configs_dir = sys.path[1] + "/input"
__metaclass__ = Singleton
def __init__(self):
"""Initialize and create empty config list"""
self._list_of_configs = []
def get_config(self, config_name):
"""Get the configuration for an input device."""
try:
idx = self._list_of_configs.index(config_name)
return self._input_config[idx]
except:
return None
def get_list_of_configs(self):
"""Reload the configurations from file"""
try:
configs = [
os.path.basename(f)
for f in glob.glob(self.configs_dir + "/[A-Za-z]*.json")
]
self._input_config = []
self._list_of_configs = []
for conf in configs:
logger.info("Parsing [%s]", conf)
json_data = open(self.configs_dir + "/%s" % conf)
data = json.load(json_data)
new_input_device = {}
for a in data["inputconfig"]["inputdevice"]["axis"]:
axis = {}
axis["scale"] = a["scale"]
axis["type"] = a["type"]
axis["key"] = a["key"]
axis["name"] = a["name"]
try:
ids = a["ids"]
except:
ids = [a["id"]]
for id in ids:
locaxis = copy.deepcopy(axis)
if "ids" in a:
if id == a["ids"][0]:
locaxis["scale"] = locaxis["scale"] * -1
locaxis["id"] = id
# 'type'-'id' defines unique index for axis
index = "%s-%d" % (a["type"], id)
new_input_device[index] = locaxis
self._input_config.append(new_input_device)
json_data.close()
self._list_of_configs.append(conf[:-5])
except Exception as e:
logger.warning("Exception while parsing inputconfig file: %s ", e)
return self._list_of_configs
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 = 0
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
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)
def add_update_callback(self, paramname, cb):
"""
Add a callback for a specific parameter name. This callback will be
executed when a new value is read from the Crazyflie.
"""
if ((paramname in self.paramUpdateCallbacks) is False):
self.paramUpdateCallbacks[paramname] = Caller()
self.paramUpdateCallbacks[paramname].add_callback(cb)
def __init__(self, crazyflie=None):
self.log_blocks = []
self.cf = crazyflie
self.toc = None
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.toc_updatedd = Caller()
self.state = IDLE
self.fake_toc_crc = 0xDEADBEEF
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 = 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 __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 __init__(self, crazyflie=None):
self.logBlocks = []
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.LOGGING, self._new_packet_cb)
self.tocUpdated = Caller()
self.state = IDLE
self.fakeTOCCRC = 0xBABEBABA
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 _clear_state(self):
self.mems = []
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called to signal completion of read or write
self.mem_read_cb = Caller()
self.mem_read_failed_cb = Caller()
self.mem_write_cb = Caller()
self.mem_write_failed_cb = Caller()
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 __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
class PeriodicTimer:
"""Create a periodic timer that will periodically call a callback"""
def __init__(self, period, callback):
self._callbacks = Caller()
self._callbacks.add_callback(callback)
self._started = False
self._period = period
self._thread = None
def start(self):
"""Start the timer"""
self._thread = _PeriodicTimerThread(self._period, self._callbacks)
self._thread.setDaemon(True)
self._thread.start()
def stop(self):
"""Stop the timer"""
if self._thread:
self._thread.stop()
self._thread = None
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 __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 __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
class PeriodicTimer:
"""Create a periodic timer that will periodically call a callback"""
def __init__(self, period, callback):
self._callbacks = Caller()
self._callbacks.add_callback(callback)
self._started = False
self._period = period
self._thread = None
def start(self):
"""Start the timer"""
self._thread = _PeriodicTimerThread(self._period, self._callbacks)
self._thread.setDaemon(True)
self._thread.start()
def stop(self):
"""Stop the timer"""
if self._thread:
self._thread.stop()
self._thread = None
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.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
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
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')
# guojun: print console information for debug
print(console_text, end='', flush=True)
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)
def setUp(self):
self.uri = uri_helper.uri_from_env(
default='radio://0/80/2M/E7E7E7E7E7')
self.cf_mock = MagicMock(spec=Crazyflie)
self.cf_mock.connected = Caller()
self.cf_mock.connection_failed = Caller()
self.cf_mock.disconnected = Caller()
self.cf_mock.fully_connected = Caller()
self.cf_mock.open_link = AsyncCallbackCaller(cb=self.cf_mock.connected,
args=[self.uri],
delay=0.2).trigger
self.close_link_mock = AsyncCallbackCaller(
cb=self.cf_mock.disconnected, args=[self.uri], delay=0.2)
self.cf_mock.close_link = self.close_link_mock.trigger
# Register a callback to be called when connected. Use it to trigger a callback
# to trigger the call to the param.all_updated() callback
self.cf_mock.connected.add_callback(self._connected_callback)
self.sut = SyncCrazyflie(self.uri, cf=self.cf_mock)
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 = 0
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 __init__(self, crazyflie=None):
"""Instantiate class and connect callbacks"""
# 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._write_requests_lock = Lock()
self._clear_state()
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 = 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 __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.valid = False
self.variables = []
self.default_fetch_as = []
self.name = name
def setUp(self):
self.cf_mock = MagicMock(spec=Crazyflie)
self.log_config_mock = MagicMock(spec=LogConfig)
self.log_config_mock.data_received_cb = Caller()
self.log_mock = MagicMock(spec=Log)
self.cf_mock.log = self.log_mock
self.front_data = 2345
self.back_data = 2345
self.left_data = 123
self.right_data = 5432
self.up_data = 3456
self.down_data = 1212
self.log_data = {
self.FRONT: self.front_data,
self.BACK: self.back_data,
self.LEFT: self.left_data,
self.RIGHT: self.right_data,
self.UP: self.up_data,
self.DOWN: self.down_data,
}
class Console:
"""
Crazyflie console is used to receive characters printed using printf
from the firmware.
"""
receivedChar = Caller()
def __init__(self, crazyflie):
"""
Initialize the console and register it to receive data from the copter.
"""
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.CONSOLE, self.incoming)
def incoming(self, packet):
"""
Callback for data received from the copter.
"""
us = "%is" % len(packet.data)
# This might be done prettier ;-)
s = "%s" % struct.unpack(us, packet.data)
self.receivedChar.call(s)
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 __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()
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 Memory:
"""Access memories on the Crazyflie"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very stange
# in the UI, so they are redefined here
_err_codes = {
errno.ENOMEM: "No more memory available",
errno.ENOEXEC: "Command not found",
errno.ENOENT: "No such block id",
errno.E2BIG: "Block too large",
errno.EEXIST: "Block already exists",
}
def __init__(self, crazyflie=None):
"""Instantiate class and connect callbacks"""
self.mems = []
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called when new data has been read
self.mem_read_cb = Caller()
self.mem_write_cb = Caller()
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
self._refresh_callback = None
self._fetch_id = 0
self.nbr_of_mems = 0
self._ow_mem_fetch_index = 0
self._elem_data = ()
self._read_requests = {}
self._write_requests = {}
self._ow_mems_left_to_update = []
self._getting_count = False
def _mem_update_done(self, mem):
"""Callback from each individual memory (only 1-wire) when reading of header/elements are done"""
if mem.id in self._ow_mems_left_to_update:
self._ow_mems_left_to_update.remove(mem.id)
logger.info(mem)
if len(self._ow_mems_left_to_update) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
def get_mem(self, id):
"""Fetch the memory with the supplied id"""
for m in self.mems:
if m.id == id:
return m
return None
def get_mems(self, type):
"""Fetch all the memories of the supplied type"""
ret = ()
for m in self.mems:
if m.type == type:
ret += (m,)
return ret
def write(self, memory, addr, data):
"""Write the specified data to the given memory at the given address"""
if memory.id in self._write_requests:
logger.warning("There is already a write operation ongoing for memory id {}".format(memory.id))
return False
wreq = _WriteRequest(memory, addr, data, self.cf)
self._write_requests[memory.id] = wreq
wreq.start()
return True
def read(self, memory, addr, length):
"""Read the specified amount of bytes from the given memory at the given address"""
if memory.id in self._read_requests:
logger.warning("There is already a read operation ongoing for memory id {}".format(memory.id))
return False
rreq = _ReadRequest(memory, addr, length, self.cf)
self._read_requests[memory.id] = rreq
rreq.start()
return True
def refresh(self, refresh_done_callback):
"""Start fetching all the detected memories"""
self._refresh_callback = refresh_done_callback
self._fetch_id = 0
for m in self.mems:
try:
self.mem_read_cb.remove_callback(m.new_data)
m.disconnect()
except Exception as e:
logger.info("Error when removing memory after update: {}".format(e))
self.mems = []
self.nbr_of_mems = 0
self._getting_count = False
logger.info("Requesting number of memories")
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_NBR,)
self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,))
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets for the memory port"""
chan = packet.channel
cmd = packet.datal[0]
payload = struct.pack("B" * (len(packet.datal) - 1), *packet.datal[1:])
# logger.info("--------------->CHAN:{}=>{}".format(chan, struct.unpack("B"*len(payload), payload)))
if chan == CHAN_INFO:
if cmd == CMD_INFO_NBR:
self.nbr_of_mems = ord(payload[0])
logger.info("{} memories found".format(self.nbr_of_mems))
# Start requesting information about the memories, if there are any...
if self.nbr_of_mems > 0:
if not self._getting_count:
self._getting_count = True
logger.info("Requesting first id")
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, 0)
self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, 0))
else:
self._refresh_callback()
if cmd == CMD_INFO_DETAILS:
# Did we get a good reply, otherwise try again:
if len(payload) < 5:
# Workaround for 1-wire bug when memory is detected
# but updating the info crashes the communication with
# the 1-wire. Fail by saying we only found 1 memory (the I2C).
logger.error("-------->Got good count, but no info on mem!")
self.nbr_of_mems = 1
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
return
# Create information about a new memory
# Id - 1 byte
mem_id = ord(payload[0])
# Type - 1 byte
mem_type = ord(payload[1])
# Size 4 bytes (as addr)
mem_size = struct.unpack("I", payload[2:6])[0]
# Addr (only valid for 1-wire?)
mem_addr_raw = struct.unpack("B" * 8, payload[6:14])
mem_addr = ""
for m in mem_addr_raw:
mem_addr += "{:02X}".format(m)
if not self.get_mem(mem_id):
if mem_type == MemoryElement.TYPE_1W:
mem = OWElement(id=mem_id, type=mem_type, size=mem_size, addr=mem_addr, mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
self._ow_mems_left_to_update.append(mem.id)
elif mem_type == MemoryElement.TYPE_I2C:
mem = I2CElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self)
logger.info(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
else:
mem = MemoryElement(id=mem_id, type=mem_type, size=mem_size, mem_handler=self)
logger.info(mem)
self.mems.append(mem)
self.mem_added_cb.call(mem)
# logger.info(mem)
self._fetch_id = mem_id + 1
if self.nbr_of_mems - 1 >= self._fetch_id:
logger.info("Requesting information about memory {}".format(self._fetch_id))
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, self._fetch_id)
self.cf.send_packet(pk, expected_reply=(CMD_INFO_DETAILS, self._fetch_id))
else:
logger.info("Done getting all the memories, start reading the OWs")
ows = self.get_mems(MemoryElement.TYPE_1W)
# If there are any OW mems start reading them, otherwise we are done
for ow_mem in self.get_mems(MemoryElement.TYPE_1W):
ow_mem.update(self._mem_update_done)
if len(self.get_mems(MemoryElement.TYPE_1W)) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
if chan == CHAN_WRITE:
id = cmd
(addr, status) = struct.unpack("<IB", payload[0:5])
logger.info("WRITE: Mem={}, addr=0x{:X}, status=0x{}".format(id, addr, status))
# Find the read request
if id in self._write_requests:
wreq = self._write_requests[id]
if status == 0:
if wreq.write_done(addr):
self._write_requests.pop(id, None)
self.mem_write_cb.call(wreq.mem, wreq.addr)
else:
wreq.resend()
if chan == CHAN_READ:
id = cmd
(addr, status) = struct.unpack("<IB", payload[0:5])
data = struct.unpack("B" * len(payload[5:]), payload[5:])
logger.info("READ: Mem={}, addr=0x{:X}, status=0x{}, data={}".format(id, addr, status, data))
# Find the read request
if id in self._read_requests:
logger.info("READING: We are still interested in request for mem {}".format(id))
rreq = self._read_requests[id]
if status == 0:
if rreq.add_data(addr, payload[5:]):
self._read_requests.pop(id, None)
self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
else:
rreq.resend()
class Memory():
"""Access memories on the Crazyflie"""
# These codes can be decoded using os.stderror, but
# some of the text messages will look very strange
# in the UI, so they are redefined here
_err_codes = {
errno.ENOMEM: 'No more memory available',
errno.ENOEXEC: 'Command not found',
errno.ENOENT: 'No such block id',
errno.E2BIG: 'Block too large',
errno.EEXIST: 'Block already exists'
}
def __init__(self, crazyflie=None):
"""Instantiate class and connect callbacks"""
self.mems = []
# Called when new memories have been added
self.mem_added_cb = Caller()
# Called when new data has been read
self.mem_read_cb = Caller()
self.mem_write_cb = Caller()
self.cf = crazyflie
self.cf.add_port_callback(CRTPPort.MEM, self._new_packet_cb)
self.cf.disconnected.add_callback(self._disconnected)
self._refresh_callback = None
self._fetch_id = 0
self.nbr_of_mems = 0
self._ow_mem_fetch_index = 0
self._elem_data = ()
self._read_requests = {}
self._read_requests_lock = Lock()
self._write_requests = {}
self._write_requests_lock = Lock()
self._ow_mems_left_to_update = []
self._getting_count = False
def _mem_update_done(self, mem):
"""
Callback from each individual memory (only 1-wire) when reading of
header/elements are done
"""
if mem.id in self._ow_mems_left_to_update:
self._ow_mems_left_to_update.remove(mem.id)
logger.info(mem)
if len(self._ow_mems_left_to_update) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
def get_mem(self, id):
"""Fetch the memory with the supplied id"""
for m in self.mems:
if m.id == id:
return m
return None
def get_mems(self, type):
"""Fetch all the memories of the supplied type"""
ret = ()
for m in self.mems:
if m.type == type:
ret += (m,)
return ret
def ow_search(self, vid=0xBC, pid=None, name=None):
"""Search for specific memory id/name and return it"""
for m in self.get_mems(MemoryElement.TYPE_1W):
if pid and m.pid == pid or name and m.name == name:
return m
return None
def write(self, memory, addr, data, flush_queue=False):
"""Write the specified data to the given memory at the given address"""
wreq = _WriteRequest(memory, addr, data, self.cf)
if memory.id not in self._write_requests:
self._write_requests[memory.id] = []
# Workaround until we secure the uplink and change messages for
# mems to non-blocking
self._write_requests_lock.acquire()
if flush_queue:
self._write_requests[memory.id] = self._write_requests[
memory.id][:1]
self._write_requests[memory.id].insert(len(self._write_requests), wreq)
if len(self._write_requests[memory.id]) == 1:
wreq.start()
self._write_requests_lock.release()
return True
def read(self, memory, addr, length):
"""
Read the specified amount of bytes from the given memory at the given
address
"""
if memory.id in self._read_requests:
logger.warning('There is already a read operation ongoing for '
'memory id {}'.format(memory.id))
return False
rreq = _ReadRequest(memory, addr, length, self.cf)
self._read_requests[memory.id] = rreq
rreq.start()
return True
def refresh(self, refresh_done_callback):
"""Start fetching all the detected memories"""
self._refresh_callback = refresh_done_callback
self._fetch_id = 0
for m in self.mems:
try:
self.mem_read_cb.remove_callback(m.new_data)
m.disconnect()
except Exception as e:
logger.info(
'Error when removing memory after update: {}'.format(e))
self.mems = []
self.nbr_of_mems = 0
self._getting_count = False
logger.info('Requesting number of memories')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_NBR,)
self.cf.send_packet(pk, expected_reply=(CMD_INFO_NBR,))
def _disconnected(self, uri):
"""The link to the Crazyflie has been broken. Reset state"""
for m in self.mems:
try:
m.disconnect()
except Exception as e:
logger.info(
'Error when resetting after disconnect: {}'.format(e))
def _new_packet_cb(self, packet):
"""Callback for newly arrived packets for the memory port"""
chan = packet.channel
cmd = packet.data[0]
payload = packet.data[1:]
if chan == CHAN_INFO:
if cmd == CMD_INFO_NBR:
self.nbr_of_mems = payload[0]
logger.info('{} memories found'.format(self.nbr_of_mems))
# Start requesting information about the memories,
# if there are any...
if self.nbr_of_mems > 0:
if not self._getting_count:
self._getting_count = True
logger.info('Requesting first id')
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, 0)
self.cf.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, 0))
else:
self._refresh_callback()
if cmd == CMD_INFO_DETAILS:
# Did we get a good reply, otherwise try again:
if len(payload) < 5:
# Workaround for 1-wire bug when memory is detected
# but updating the info crashes the communication with
# the 1-wire. Fail by saying we only found 1 memory
# (the I2C).
logger.error(
'-------->Got good count, but no info on mem!')
self.nbr_of_mems = 1
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
return
# Create information about a new memory
# Id - 1 byte
mem_id = payload[0]
# Type - 1 byte
mem_type = payload[1]
# Size 4 bytes (as addr)
mem_size = struct.unpack('I', payload[2:6])[0]
# Addr (only valid for 1-wire?)
mem_addr_raw = struct.unpack('B' * 8, payload[6:14])
mem_addr = ''
for m in mem_addr_raw:
mem_addr += '{:02X}'.format(m)
if (not self.get_mem(mem_id)):
if mem_type == MemoryElement.TYPE_1W:
mem = OWElement(id=mem_id, type=mem_type,
size=mem_size,
addr=mem_addr, mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
self._ow_mems_left_to_update.append(mem.id)
elif mem_type == MemoryElement.TYPE_I2C:
mem = I2CElement(id=mem_id, type=mem_type,
size=mem_size,
mem_handler=self)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_DRIVER_LED:
mem = LEDDriverMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.info(mem)
self.mem_read_cb.add_callback(mem.new_data)
self.mem_write_cb.add_callback(mem.write_done)
elif mem_type == MemoryElement.TYPE_LOCO:
mem = LocoMemory(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.info(mem)
self.mem_read_cb.add_callback(mem.new_data)
else:
mem = MemoryElement(id=mem_id, type=mem_type,
size=mem_size, mem_handler=self)
logger.info(mem)
self.mems.append(mem)
self.mem_added_cb.call(mem)
# logger.info(mem)
self._fetch_id = mem_id + 1
if self.nbr_of_mems - 1 >= self._fetch_id:
logger.info(
'Requesting information about memory {}'.format(
self._fetch_id))
pk = CRTPPacket()
pk.set_header(CRTPPort.MEM, CHAN_INFO)
pk.data = (CMD_INFO_DETAILS, self._fetch_id)
self.cf.send_packet(pk, expected_reply=(
CMD_INFO_DETAILS, self._fetch_id))
else:
logger.info(
'Done getting all the memories, start reading the OWs')
ows = self.get_mems(MemoryElement.TYPE_1W)
# If there are any OW mems start reading them, otherwise
# we are done
for ow_mem in ows:
ow_mem.update(self._mem_update_done)
if len(ows) == 0:
if self._refresh_callback:
self._refresh_callback()
self._refresh_callback = None
if chan == CHAN_WRITE:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
logger.info(
'WRITE: Mem={}, addr=0x{:X}, status=0x{}'.format(
id, addr, status))
# Find the read request
if id in self._write_requests:
self._write_requests_lock.acquire()
wreq = self._write_requests[id][0]
if status == 0:
if wreq.write_done(addr):
# self._write_requests.pop(id, None)
# Remove the first item
self._write_requests[id].pop(0)
self.mem_write_cb.call(wreq.mem, wreq.addr)
# Get a new one to start (if there are any)
if len(self._write_requests[id]) > 0:
self._write_requests[id][0].start()
else:
logger.info('Status {}: write resending...'.format(status))
wreq.resend()
self._write_requests_lock.release()
if chan == CHAN_READ:
id = cmd
(addr, status) = struct.unpack('<IB', payload[0:5])
data = struct.unpack('B' * len(payload[5:]), payload[5:])
logger.info('READ: Mem={}, addr=0x{:X}, status=0x{}, '
'data={}'.format(id, addr, status, data))
# Find the read request
if id in self._read_requests:
logger.info(
'READING: We are still interested in request for '
'mem {}'.format(id))
rreq = self._read_requests[id]
if status == 0:
if rreq.add_data(addr, payload[5:]):
self._read_requests.pop(id, None)
self.mem_read_cb.call(rreq.mem, rreq.addr, rreq.data)
else:
logger.info('Status {}: resending...'.format(status))
rreq.resend()
class 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)
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()
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)
import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.utils.callbacks import Caller
from cflib.crazyflie.log import LogConfig
URI = 'radio://0/80/250K'
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
# Called when the link is established and the TOCs (that are not
# cached) have been downloaded
connected = Caller()
cflib.crtp.init_drivers(enable_debug_driver=False)
cf = Crazyflie()
cf.open_link(URI)
time.sleep(8.0)
cf.commander.send_setpoint(0, 0, 0, 0)
def poshold(cf, t, z):
steps = t * 10
for r in range(steps):
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 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 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 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 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 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)
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 __init__(self, crazyflie):
self._cf = crazyflie
self.packet_received = Caller()
self._cf.add_port_callback(CRTPPort.PLATFORM, self._incoming)
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)
def __init__(self, period, callback):
self._callbacks = Caller()
self._callbacks.add_callback(callback)
self._started = False
self._period = period
self._thread = None
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)
