def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# UDP socket for interfacing with GCS
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self._sock.bind(('127.0.0.1', 14551))
# Create a Espdrone object without specifying any cache dirs
self._ed = Espdrone()
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
threading.Thread(target=self._server).start()
def __init__(self, URI):
QtWidgets.QMainWindow.__init__(self)
self.resize(700, 500)
self.setWindowTitle('Multi-ranger point cloud')
self.canvas = Canvas(self.updateHover)
self.canvas.create_native()
self.canvas.native.setParent(self)
self.setCentralWidget(self.canvas.native)
edlib.crtp.init_drivers(enable_debug_driver=False)
self.ed = Espdrone(ro_cache=None, rw_cache='cache')
# Connect callbacks from the Espdrone API
self.ed.connected.add_callback(self.connected)
self.ed.disconnected.add_callback(self.disconnected)
# Connect to the Espdrone
self.ed.open_link(URI)
self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3}
self.hoverTimer = QtCore.QTimer()
self.hoverTimer.timeout.connect(self.sendHoverCommand)
self.hoverTimer.setInterval(100)
self.hoverTimer.start()
def __init__(self, link_uri):
self._ed = Espdrone()
self._link_uri = link_uri
# Add some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
# Initialize variables
self.connected = False
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._ed = Espdrone(rw_cache='./cache')
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
self._ed.open_link(link_uri)
print('Connecting to %s' % link_uri)
def __init__(self, link_uri, ed=None):
""" Create a synchronous Espdrone instance with the specified
link_uri """
if ed:
self.ed = ed
else:
self.ed = Espdrone()
self._link_uri = link_uri
self._connect_event = Event()
self._is_link_open = False
self._error_message = None
class LpsRebootToBootloader:
"""Example that connects to a Espdrone and ramps the motors up/down and
the disconnects"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._ed = Espdrone()
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
self._ed.open_link(link_uri)
print('Connecting to %s' % link_uri)
def _connected(self, link_uri):
""" This callback is called form the Espdrone API when a Espdrone
has been connected and the TOCs have been downloaded."""
# Start a separate thread to do the motor test.
# Do not hijack the calling thread!
Thread(target=self._reboot_thread).start()
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Espdrone
at the specified address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Espdrone moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Espdrone is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
def _reboot_thread(self):
anchors = LoPoAnchor(self._ed)
print('Sending reboot signal to all anchors 10 times in a row ...')
for retry in range(10):
for anchor_id in range(6):
anchors.reboot(anchor_id, anchors.REBOOT_TO_BOOTLOADER)
time.sleep(0.1)
self._ed.close_link()
def __init__(self):
"""Initialize the headless client and libraries"""
edlib.crtp.init_drivers()
self._jr = JoystickReader(do_device_discovery=False)
self._cf = Espdrone(ro_cache=None,
rw_cache=edclient.config_path + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._devs = []
for d in self._jr.available_devices():
self._devs.append(d.name)
def simple_sequence():
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
with PositionHlCommander(sed) as pc:
pc.forward(1.0)
pc.left(1.0)
pc.back(1.0)
pc.go_to(0.0, 0.0, 1.0)
def slightly_more_complex_usage():
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
with PositionHlCommander(
sed,
x=0.0,
y=0.0,
z=0.0,
default_velocity=0.3,
default_height=0.5,
controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
# Go to a coordinate
pc.go_to(1.0, 1.0, 1.0)
# Move relative to the current position
pc.right(1.0)
# Go to a coordinate and use default height
pc.go_to(0.0, 0.0)
# Go slowly to a coordinate
pc.go_to(1.0, 1.0, velocity=0.2)
# Set new default velocity and height
pc.set_default_velocity(0.3)
pc.set_default_height(1.0)
pc.go_to(0.0, 0.0)
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
self._ed = Espdrone(rw_cache='./cache')
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def test_that_the_same_ed_object_can_be_connected_multiple_times(self):
# Fixture
self.test_rig_support.restart_devices(self.test_rig_support.all_uris)
ed = Espdrone(rw_cache='./cache')
# Test
for uri in self.test_rig_support.all_uris:
with SyncEspdrone(uri, ed=ed):
pass
def test_that_requested_logging_is_received_properly_from_one_ed(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
with SyncEspdrone(uri, ed=ed) as sed:
self.assert_add_logging_and_get_non_zero_value(sed)
def test_memory_mapping_with_one_ed(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
with SyncEspdrone(uri, ed=ed) as sed:
self.assert_memory_mapping(sed)
def test_memory_mapping_with_reuse_of_ed_object(self):
# Fixture
uri = self.test_rig_support.all_uris[0]
self.test_rig_support.restart_devices([uri])
ed = Espdrone(rw_cache='./cache')
# Test and Assert
for connections in range(10):
with SyncEspdrone(uri, ed=ed) as sed:
for mem_ops in range(5):
self.assert_memory_mapping(sed)
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Espdrone object without specifying any cache dirs
self._ed = Espdrone()
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
self._mems_to_update = 0
def __init__(self, uri):
self.got_data = False
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
mems = sed.ed.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
print('Rquesting data')
mems[0].update(self._data_updated)
while not self.got_data:
time.sleep(1)
def __init__(self, uri, bs1, bs2):
self.data_written = False
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
mems = sed.ed.mem.get_mems(MemoryElement.TYPE_LH)
count = len(mems)
if count != 1:
raise Exception('Unexpected nr of memories found:', count)
mems[0].geometry_data = [bs1, bs2]
print('Writing data')
mems[0].write_data(self._data_written)
while not self.data_written:
time.sleep(1)
def __init__(self, base_url):
"""Start threads and bind ports"""
edlib.crtp.init_drivers(enable_debug_driver=True)
self._cf = Espdrone(ro_cache=None,
rw_cache=edclient.config_path + "/cache")
signal.signal(signal.SIGINT, signal.SIG_DFL)
self._base_url = base_url
self._context = zmq.Context()
cmd_srv = self._bind_zmq_socket(zmq.REP, "cmd", ZMQ_SRV_PORT)
log_srv = self._bind_zmq_socket(zmq.PUB, "log", ZMQ_LOG_PORT)
param_srv = self._bind_zmq_socket(zmq.PUB, "param", ZMQ_PARAM_PORT)
ctrl_srv = self._bind_zmq_socket(zmq.PULL, "ctrl", ZMQ_CTRL_PORT)
conn_srv = self._bind_zmq_socket(zmq.PUB, "conn", ZMQ_CONN_PORT)
self._scan_thread = _SrvThread(cmd_srv, log_srv, param_srv, conn_srv,
self._cf)
self._scan_thread.start()
self._ctrl_thread = _CtrlThread(ctrl_srv, self._cf)
self._ctrl_thread.start()
print('Grab ended')
grabbed = trigger
if trigger:
curr = [-1 * pose[2][3], -1 * pose[0][3], pose[1][3]]
setpoint = vector_add(
grab_setpoint_start,
vector_substract(curr, grab_controller_start))
ed.commander.send_position_setpoint(setpoint[0], setpoint[1],
setpoint[2], 0)
time.sleep(0.02)
ed.commander.send_setpoint
(0, 0, 0, 0)
# Make sure that the last packet leaves before the link is closed
# since the message queue is not flushed before closing
time.sleep(0.1)
if __name__ == '__main__':
edlib.crtp.init_drivers(enable_debug_driver=False)
with SyncEspdrone(uri, ed=Espdrone(rw_cache='./cache')) as sed:
reset_estimator(sed)
# start_position_printing(sed)
run_sequence(sed)
openvr.shutdown()
class MainWindow(QtWidgets.QMainWindow):
def __init__(self, URI):
QtWidgets.QMainWindow.__init__(self)
self.resize(700, 500)
self.setWindowTitle('Multi-ranger point cloud')
self.canvas = Canvas(self.updateHover)
self.canvas.create_native()
self.canvas.native.setParent(self)
self.setCentralWidget(self.canvas.native)
edlib.crtp.init_drivers(enable_debug_driver=False)
self.ed = Espdrone(ro_cache=None, rw_cache='cache')
# Connect callbacks from the Espdrone API
self.ed.connected.add_callback(self.connected)
self.ed.disconnected.add_callback(self.disconnected)
# Connect to the Espdrone
self.ed.open_link(URI)
self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3}
self.hoverTimer = QtCore.QTimer()
self.hoverTimer.timeout.connect(self.sendHoverCommand)
self.hoverTimer.setInterval(100)
self.hoverTimer.start()
def sendHoverCommand(self):
self.ed.commander.send_hover_setpoint(self.hover['x'], self.hover['y'],
self.hover['yaw'],
self.hover['height'])
def updateHover(self, k, v):
if (k != 'height'):
self.hover[k] = v * SPEED_FACTOR
else:
self.hover[k] += v
def disconnected(self, URI):
print('Disconnected')
def connected(self, URI):
print('We are now connected to {}'.format(URI))
# The definition of the logconfig can be made before connecting
lpos = LogConfig(name='Position', period_in_ms=100)
lpos.add_variable('stateEstimate.x')
lpos.add_variable('stateEstimate.y')
lpos.add_variable('stateEstimate.z')
try:
self.ed.log.add_config(lpos)
lpos.data_received_cb.add_callback(self.pos_data)
lpos.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Position log config, bad configuration.')
lmeas = LogConfig(name='Meas', period_in_ms=100)
lmeas.add_variable('range.front')
lmeas.add_variable('range.back')
lmeas.add_variable('range.up')
lmeas.add_variable('range.left')
lmeas.add_variable('range.right')
lmeas.add_variable('range.zrange')
lmeas.add_variable('stabilizer.roll')
lmeas.add_variable('stabilizer.pitch')
lmeas.add_variable('stabilizer.yaw')
try:
self.ed.log.add_config(lmeas)
lmeas.data_received_cb.add_callback(self.meas_data)
lmeas.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Measurement log config, bad configuration.')
def pos_data(self, timestamp, data, logconf):
position = [
data['stateEstimate.x'], data['stateEstimate.y'],
data['stateEstimate.z']
]
self.canvas.set_position(position)
def meas_data(self, timestamp, data, logconf):
measurement = {
'roll': data['stabilizer.roll'],
'pitch': data['stabilizer.pitch'],
'yaw': data['stabilizer.yaw'],
'front': data['range.front'],
'back': data['range.back'],
'up': data['range.up'],
'down': data['range.zrange'],
'left': data['range.left'],
'right': data['range.right']
}
self.canvas.set_measurement(measurement)
def closeEvent(self, event):
if (self.ed is not None):
self.ed.close_link()
def is_close(range):
MIN_DISTANCE = 0.2 # m
if range is None:
return False
else:
return range < MIN_DISTANCE
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
ed = Espdrone(rw_cache='./cache')
with SyncEspdrone(URI, ed=ed) as sed:
with MotionCommander(sed) as motion_commander:
with Multiranger(sed) as multiranger:
keep_flying = True
while keep_flying:
VELOCITY = 0.5
velocity_x = 0.0
velocity_y = 0.0
if is_close(multiranger.front):
velocity_x -= VELOCITY
if is_close(multiranger.back):
velocity_x += VELOCITY
class Flasher(object):
"""
A class that can flash the DS28E05 EEPROM via CRTP.
"""
def __init__(self, link_uri):
self._ed = Espdrone()
self._link_uri = link_uri
# Add some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
# Initialize variables
self.connected = False
# Public methods
def connect(self):
"""
Connect to the espdrone.
"""
print('Connecting to %s' % self._link_uri)
self._ed.open_link(self._link_uri)
def disconnect(self):
print('Disconnecting from %s' % self._link_uri)
self._ed.close_link()
def wait_for_connection(self, timeout=10):
"""
Busy loop until connection is established.
Will abort after timeout (seconds). Return value is a boolean, whether
connection could be established.
"""
start_time = datetime.datetime.now()
while True:
if self.connected:
return True
now = datetime.datetime.now()
if (now - start_time).total_seconds() > timeout:
return False
time.sleep(0.5)
def search_memories(self):
"""
Search and return list of 1-wire memories.
"""
if not self.connected:
raise NotConnected()
return self._ed.mem.get_mems(MemoryElement.TYPE_1W)
# Callbacks
def _connected(self, link_uri):
print('Connected to %s' % link_uri)
self.connected = True
def _disconnected(self, link_uri):
print('Disconnected from %s' % link_uri)
self.connected = False
def _connection_failed(self, link_uri, msg):
print('Connection to %s failed: %s' % (link_uri, msg))
self.connected = False
def _connection_lost(self, link_uri, msg):
print('Connection to %s lost: %s' % (link_uri, msg))
self.connected = False
class EEPROMExample:
"""
Simple example listing the EEPROMs found and writes the default values
in it.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Espdrone object without specifying any cache dirs
self._ed = Espdrone()
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
def _connected(self, link_uri):
""" This callback is called form the Espdrone API when a Espdrone
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
mems = self._ed.mem.get_mems(MemoryElement.TYPE_I2C)
print('Found {} EEPOM(s)'.format(len(mems)))
if len(mems) > 0:
print('Writing default configuration to'
' memory {}'.format(mems[0].id))
elems = mems[0].elements
elems['version'] = 1
elems['pitch_trim'] = 0.0
elems['roll_trim'] = 0.0
elems['radio_channel'] = 80
elems['radio_speed'] = 0
elems['radio_address'] = 0xE7E7E7E7E7
mems[0].write_data(self._data_written)
def _data_written(self, mem, addr):
print('Data written, reading back...')
mem.update(self._data_updated)
def _data_updated(self, mem):
print('Updated id={}'.format(mem.id))
print('\tType : {}'.format(mem.type))
print('\tSize : {}'.format(mem.size))
print('\tValid : {}'.format(mem.valid))
print('\tElements : ')
for key in mem.elements:
print('\t\t{}={}'.format(key, mem.elements[key]))
self._ed.close_link()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Espdrone
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Espdrone moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Espdrone is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
class OWExample:
"""
Simple example listing the 1-wire memories found and lists its contents.
"""
def __init__(self, link_uri):
""" Initialize and run the example with the specified link_uri """
# Create a Espdrone object without specifying any cache dirs
self._ed = Espdrone()
# Connect some callbacks from the Espdrone API
self._ed.connected.add_callback(self._connected)
self._ed.disconnected.add_callback(self._disconnected)
self._ed.connection_failed.add_callback(self._connection_failed)
self._ed.connection_lost.add_callback(self._connection_lost)
print('Connecting to %s' % link_uri)
# Try to connect to the Espdrone
self._ed.open_link(link_uri)
# Variable used to keep main loop occupied until disconnect
self.is_connected = True
self._mems_to_update = 0
def _connected(self, link_uri):
""" This callback is called form the Espdrone API when a Espdrone
has been connected and the TOCs have been downloaded."""
print('Connected to %s' % link_uri)
mems = self._ed.mem.get_mems(MemoryElement.TYPE_1W)
self._mems_to_update = len(mems)
print('Found {} 1-wire memories'.format(len(mems)))
for m in mems:
print('Updating id={}'.format(m.id))
m.update(self._data_updated)
def _data_updated(self, mem):
print('Updated id={}'.format(mem.id))
print('\tAddr : {}'.format(mem.addr))
print('\tType : {}'.format(mem.type))
print('\tSize : {}'.format(mem.size))
print('\tValid : {}'.format(mem.valid))
print('\tName : {}'.format(mem.name))
print('\tVID : 0x{:02X}'.format(mem.vid))
print('\tPID : 0x{:02X}'.format(mem.pid))
print('\tPins : 0x{:02X}'.format(mem.pins))
print('\tElements : ')
for key in mem.elements:
print('\t\t{}={}'.format(key, mem.elements[key]))
self._mems_to_update -= 1
if self._mems_to_update == 0:
self._ed.close_link()
def _stab_log_error(self, logconf, msg):
"""Callback from the log API when an error occurs"""
print('Error when logging %s: %s' % (logconf.name, msg))
def _stab_log_data(self, timestamp, data, logconf):
"""Callback froma the log API when data arrives"""
print('[%d][%s]: %s' % (timestamp, logconf.name, data))
def _connection_failed(self, link_uri, msg):
"""Callback when connection initial connection fails (i.e no Espdrone
at the speficied address)"""
print('Connection to %s failed: %s' % (link_uri, msg))
self.is_connected = False
def _connection_lost(self, link_uri, msg):
"""Callback when disconnected after a connection has been made (i.e
Espdrone moves out of range)"""
print('Connection to %s lost: %s' % (link_uri, msg))
def _disconnected(self, link_uri):
"""Callback when the Espdrone is disconnected (called in all cases)"""
print('Disconnected from %s' % link_uri)
self.is_connected = False
def construct(self, uri):
ed = Espdrone(ro_cache=self.ro_cache, rw_cache=self.rw_cache)
return SyncEspdrone(uri, ed=ed)
* ESP Drone - NH
* Camera
"""
import logging
import argparse
import edlib.crtp # noqa
from edlib.espdrone import Espdrone
import cv2
import sys
import time
# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)
ed = Espdrone(rw_cache='./cache')
camera = ed.camera
def show_image(image, fps):
global is_streaming
cv2.imshow('unique_window_identifier', image)
cv2.setWindowTitle("unique_window_identifier", f"fps= {fps}")
# Press Q to Quit
if cv2.waitKey(1) == ord('q'):
camera.image_received_cb.remove_callback(show_image)
is_streaming = False
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
parser = argparse.ArgumentParser()
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Espdrone(ro_cache=None,
rw_cache=edclient.config_path + "/cache")
edlib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.connect_input = QShortcut("Ctrl+I", self.connectButton,
self._connect)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# Add things to helper so tabs can access it
edclient.ui.pluginhelper.cf = self.cf
edclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
edclient.ui.pluginhelper.logConfigReader = self.logConfigReader
edclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(edclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(edclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(edclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(edclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(edclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in edclient.ui.tabs.available:
tab = tabClass(self.tabs, edclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
for t_class in edclient.ui.toolboxes.toolboxes:
toolbox = t_class(edclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [dockToolbox, ]
# Add menu item for the toolbox
item = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
class MainUI(QtWidgets.QMainWindow, main_window_class):
connectionLostSignal = pyqtSignal(str, str)
connectionInitiatedSignal = pyqtSignal(str)
batteryUpdatedSignal = pyqtSignal(int, object, object)
connectionDoneSignal = pyqtSignal(str)
connectionFailedSignal = pyqtSignal(str, str)
disconnectedSignal = pyqtSignal(str)
linkQualitySignal = pyqtSignal(int)
_input_device_error_signal = pyqtSignal(str)
_input_discovery_signal = pyqtSignal(object)
_log_error_signal = pyqtSignal(object, str)
def __init__(self, *args):
super(MainUI, self).__init__(*args)
self.setupUi(self)
# Restore window size if present in the config file
try:
size = Config().get("window_size")
self.resize(size[0], size[1])
except KeyError:
pass
######################################################
# By lxrocks
# 'Skinny Progress Bar' tweak for Yosemite
# Tweak progress bar - artistic I am not - so pick your own colors !!!
# Only apply to Yosemite
######################################################
import platform
if platform.system() == 'Darwin':
(Version, junk, machine) = platform.mac_ver()
logger.info("This is a MAC - checking if we can apply Progress "
"Bar Stylesheet for Yosemite Skinny Bars ")
yosemite = (10, 10, 0)
tVersion = tuple(map(int, (Version.split("."))))
if tVersion >= yosemite:
logger.info("Found Yosemite - applying stylesheet")
tcss = """
QProgressBar {
border: 1px solid grey;
border-radius: 5px;
text-align: center;
}
QProgressBar::chunk {
background-color: """ + COLOR_BLUE + """;
}
"""
self.setStyleSheet(tcss)
else:
logger.info("Pre-Yosemite - skinny bar stylesheet not applied")
######################################################
self.cf = Espdrone(ro_cache=None,
rw_cache=edclient.config_path + "/cache")
edlib.crtp.init_drivers(enable_debug_driver=Config()
.get("enable_debug_driver"))
zmq_params = ZMQParamAccess(self.cf)
zmq_params.start()
zmq_leds = ZMQLEDDriver(self.cf)
zmq_leds.start()
self.scanner = ScannerThread()
self.scanner.interfaceFoundSignal.connect(self.foundInterfaces)
self.scanner.start()
# Create and start the Input Reader
self._statusbar_label = QLabel("No input-device found, insert one to"
" fly.")
self.statusBar().addWidget(self._statusbar_label)
self.joystickReader = JoystickReader()
self._active_device = ""
# self.configGroup = QActionGroup(self._menu_mappings, exclusive=True)
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
# TODO: Need to reload configs
# ConfigManager().conf_needs_reload.add_callback(self._reload_configs)
self.connect_input = QShortcut("Ctrl+I", self.connectButton,
self._connect)
self.cf.connection_failed.add_callback(
self.connectionFailedSignal.emit)
self.connectionFailedSignal.connect(self._connection_failed)
self._input_device_error_signal.connect(
self._display_input_device_error)
self.joystickReader.device_error.add_callback(
self._input_device_error_signal.emit)
self._input_discovery_signal.connect(self.device_discovery)
self.joystickReader.device_discovery.add_callback(
self._input_discovery_signal.emit)
# Hide the 'File' menu on OS X, since its only item, 'Exit', gets
# merged into the application menu.
if sys.platform == 'darwin':
self.menuFile.menuAction().setVisible(False)
# Connect UI signals
self.logConfigAction.triggered.connect(self._show_connect_dialog)
self.interfaceCombo.currentIndexChanged['QString'].connect(
self.interfaceChanged)
self.connectButton.clicked.connect(self._connect)
self.scanButton.clicked.connect(self._scan)
self.menuItemConnect.triggered.connect(self._connect)
self.menuItemConfInputDevice.triggered.connect(
self._show_input_device_config_dialog)
self.menuItemExit.triggered.connect(self.closeAppRequest)
self.batteryUpdatedSignal.connect(self._update_battery)
self._menuitem_rescandevices.triggered.connect(self._rescan_devices)
self._menuItem_openconfigfolder.triggered.connect(
self._open_config_folder)
self._auto_reconnect_enabled = Config().get("auto_reconnect")
self.autoReconnectCheckBox.toggled.connect(
self._auto_reconnect_changed)
self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect"))
self._disable_input = False
self.joystickReader.input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_setpoint(*args))
self.joystickReader.assisted_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_velocity_world_setpoint(*args))
self.joystickReader.heighthold_input_updated.add_callback(
lambda *args: self._disable_input or
self.cf.commander.send_zdistance_setpoint(*args))
self.joystickReader.hover_input_updated.add_callback(
self.cf.commander.send_hover_setpoint)
# Connection callbacks and signal wrappers for UI protection
self.cf.connected.add_callback(self.connectionDoneSignal.emit)
self.connectionDoneSignal.connect(self._connected)
self.cf.disconnected.add_callback(self.disconnectedSignal.emit)
self.disconnectedSignal.connect(self._disconnected)
self.cf.connection_lost.add_callback(self.connectionLostSignal.emit)
self.connectionLostSignal.connect(self._connection_lost)
self.cf.connection_requested.add_callback(
self.connectionInitiatedSignal.emit)
self.connectionInitiatedSignal.connect(self._connection_initiated)
self._log_error_signal.connect(self._logging_error)
self.batteryBar.setTextVisible(False)
self.batteryBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
self.linkQualityBar.setTextVisible(False)
self.linkQualityBar.setStyleSheet(progressbar_stylesheet(COLOR_BLUE))
# Connect link quality feedback
self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit)
self.linkQualitySignal.connect(
lambda percentage: self.linkQualityBar.setValue(percentage))
self._selected_interface = None
self._initial_scan = True
self._scan()
# Parse the log configuration files
self.logConfigReader = LogConfigReader(self.cf)
self._current_input_config = None
self._active_config = None
self._active_config = None
self.inputConfig = None
# Add things to helper so tabs can access it
edclient.ui.pluginhelper.cf = self.cf
edclient.ui.pluginhelper.inputDeviceReader = self.joystickReader
edclient.ui.pluginhelper.logConfigReader = self.logConfigReader
edclient.ui.pluginhelper.mainUI = self
self.logConfigDialogue = LogConfigDialogue(edclient.ui.pluginhelper)
self._bootloader_dialog = BootloaderDialog(edclient.ui.pluginhelper)
self._cf2config_dialog = Cf2ConfigDialog(edclient.ui.pluginhelper)
self._cf1config_dialog = Cf1ConfigDialog(edclient.ui.pluginhelper)
self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show)
self._about_dialog = AboutDialog(edclient.ui.pluginhelper)
self.menuItemAbout.triggered.connect(self._about_dialog.show)
self._menu_cf2_config.triggered.connect(self._cf2config_dialog.show)
self._menu_cf1_config.triggered.connect(self._cf1config_dialog.show)
# Load and connect tabs
self.tabsMenuItem = QMenu("Tabs", self.menuView, enabled=True)
self.menuView.addMenu(self.tabsMenuItem)
# self.tabsMenuItem.setMenu(QtWidgets.QMenu())
tabItems = {}
self.loadedTabs = []
for tabClass in edclient.ui.tabs.available:
tab = tabClass(self.tabs, edclient.ui.pluginhelper)
item = QtWidgets.QAction(tab.getMenuName(), self, checkable=True)
item.toggled.connect(tab.toggleVisibility)
self.tabsMenuItem.addAction(item)
tabItems[tab.getTabName()] = item
self.loadedTabs.append(tab)
if not tab.enabled:
item.setEnabled(False)
# First instantiate all tabs and then open them in the correct order
try:
for tName in Config().get("open_tabs").split(","):
t = tabItems[tName]
if (t is not None and t.isEnabled()):
# Toggle though menu so it's also marked as open there
t.toggle()
except Exception as e:
logger.warning("Exception while opening tabs [{}]".format(e))
# Loading toolboxes (A bit of magic for a lot of automatic)
self.toolboxesMenuItem = QMenu("Toolboxes", self.menuView,
enabled=True)
self.menuView.addMenu(self.toolboxesMenuItem)
self.toolboxes = []
for t_class in edclient.ui.toolboxes.toolboxes:
toolbox = t_class(edclient.ui.pluginhelper)
dockToolbox = MyDockWidget(toolbox.getName())
dockToolbox.setWidget(toolbox)
self.toolboxes += [dockToolbox, ]
# Add menu item for the toolbox
item = QtWidgets.QAction(toolbox.getName(), self)
item.setCheckable(True)
item.triggered.connect(self.toggleToolbox)
self.toolboxesMenuItem.addAction(item)
dockToolbox.closed.connect(lambda: self.toggleToolbox(False))
# Setup some introspection
item.dockToolbox = dockToolbox
item.menuItem = item
dockToolbox.dockToolbox = dockToolbox
dockToolbox.menuItem = item
# References to all the device sub-menus in the "Input device" menu
self._all_role_menus = ()
# Used to filter what new devices to add default mapping to
self._available_devices = ()
# Keep track of mux nodes so we can enable according to how many
# devices we have
self._all_mux_nodes = ()
# Check which Input muxes are available
self._mux_group = QActionGroup(self._menu_inputdevice, exclusive=True)
for m in self.joystickReader.available_mux():
node = QAction(m.name,
self._menu_inputdevice,
checkable=True,
enabled=False)
node.toggled.connect(self._mux_selected)
self._mux_group.addAction(node)
self._menu_inputdevice.addAction(node)
self._all_mux_nodes += (node,)
mux_subnodes = ()
for name in m.supported_roles():
sub_node = QMenu(" {}".format(name),
self._menu_inputdevice,
enabled=False)
self._menu_inputdevice.addMenu(sub_node)
mux_subnodes += (sub_node,)
self._all_role_menus += ({"muxmenu": node,
"rolemenu": sub_node},)
node.setData((m, mux_subnodes))
self._mapping_support = True
def disable_input(self, disable):
"""
Disable the gamepad input to be able to send setpoint from a tab
"""
self._disable_input = disable
def interfaceChanged(self, interface):
if interface == INTERFACE_PROMPT_TEXT:
self._selected_interface = None
else:
self._selected_interface = interface
self._update_ui_state()
def foundInterfaces(self, interfaces):
selected_interface = self._selected_interface
self.interfaceCombo.clear()
if interfaces:
for interface in interfaces:
self.interfaceCombo.addItems(interface)
else:
self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT)
if self._initial_scan:
self._initial_scan = False
try:
if len(Config().get("link_uri")) > 0:
interfaces.index(Config().get("link_uri"))
selected_interface = Config().get("link_uri")
except KeyError:
# The configuration for link_uri was not found
pass
except ValueError:
# The saved URI was not found while scanning
pass
if len(interfaces) == 1 and selected_interface is None:
selected_interface = interfaces[0][0]
newIndex = 0
if selected_interface is not None:
try:
newIndex = interfaces.index(selected_interface) + 1
except ValueError:
pass
self.interfaceCombo.setCurrentIndex(newIndex)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _update_ui_state(self):
if self.uiState == UIState.DISCONNECTED:
self.setWindowTitle("Not connected")
canConnect = self._selected_interface is not None
self.menuItemConnect.setText("Connect to Espdrone")
self.menuItemConnect.setEnabled(canConnect)
self.connectButton.setText("Connect")
self.connectButton.setToolTip("Connect to the Espdrone on"
"the selected interface (Ctrl+I)")
self.connectButton.setEnabled(canConnect)
self.scanButton.setText("Scan")
self.scanButton.setEnabled(True)
self.address.setEnabled(True)
self.batteryBar.setValue(3000)
self._menu_cf2_config.setEnabled(False)
self._menu_cf1_config.setEnabled(True)
self.linkQualityBar.setValue(0)
self.menuItemBootloader.setEnabled(True)
self.logConfigAction.setEnabled(False)
self.interfaceCombo.setEnabled(True)
elif self.uiState == UIState.CONNECTED:
s = "Connected on %s" % self._selected_interface
self.setWindowTitle(s)
self.menuItemConnect.setText("Disconnect")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Disconnect")
self.connectButton.setToolTip("Disconnect from"
"the Espdrone (Ctrl+I)")
self.scanButton.setEnabled(False)
self.logConfigAction.setEnabled(True)
# Find out if there's an I2C EEPROM, otherwise don't show the
# dialog.
if len(self.cf.mem.get_mems(MemoryElement.TYPE_I2C)) > 0:
self._menu_cf2_config.setEnabled(True)
self._menu_cf1_config.setEnabled(False)
elif self.uiState == UIState.CONNECTING:
s = "Connecting to {} ...".format(self._selected_interface)
self.setWindowTitle(s)
self.menuItemConnect.setText("Cancel")
self.menuItemConnect.setEnabled(True)
self.connectButton.setText("Cancel")
self.connectButton.setToolTip(
"Cancel connecting to the Espdrone")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
elif self.uiState == UIState.SCANNING:
self.setWindowTitle("Scanning ...")
self.connectButton.setText("Connect")
self.menuItemConnect.setEnabled(False)
self.connectButton.setText("Connect")
self.connectButton.setEnabled(False)
self.scanButton.setText("Scanning...")
self.scanButton.setEnabled(False)
self.address.setEnabled(False)
self.menuItemBootloader.setEnabled(False)
self.interfaceCombo.setEnabled(False)
@pyqtSlot(bool)
def toggleToolbox(self, display):
menuItem = self.sender().menuItem
dockToolbox = self.sender().dockToolbox
if display and not dockToolbox.isVisible():
dockToolbox.widget().enable()
self.addDockWidget(dockToolbox.widget().preferedDockArea(),
dockToolbox)
dockToolbox.show()
elif not display:
dockToolbox.widget().disable()
self.removeDockWidget(dockToolbox)
dockToolbox.hide()
menuItem.setChecked(False)
def _rescan_devices(self):
self._statusbar_label.setText("No inputdevice connected!")
self._menu_devices.clear()
self._active_device = ""
self.joystickReader.stop_input()
# for c in self._menu_mappings.actions():
# c.setEnabled(False)
# devs = self.joystickReader.available_devices()
# if (len(devs) > 0):
# self.device_discovery(devs)
def _show_input_device_config_dialog(self):
self.inputConfig = InputConfigDialogue(self.joystickReader)
self.inputConfig.show()
def _auto_reconnect_changed(self, checked):
self._auto_reconnect_enabled = checked
Config().set("auto_reconnect", checked)
logger.info("Auto reconnect enabled: {}".format(checked))
def _show_connect_dialog(self):
self.logConfigDialogue.show()
def _update_battery(self, timestamp, data, logconf):
self.batteryBar.setValue(int(data["pm.vbat"] * 1000))
color = COLOR_BLUE
# TODO firmware reports fully-charged state as 'Battery',
# rather than 'Charged'
if data["pm.state"] in [BatteryStates.CHARGING, BatteryStates.CHARGED]:
color = COLOR_GREEN
elif data["pm.state"] == BatteryStates.LOW_POWER:
color = COLOR_RED
self.batteryBar.setStyleSheet(progressbar_stylesheet(color))
self._aff_volts.setText(("%.3f" % data["pm.vbat"]))
def _connected(self):
self.uiState = UIState.CONNECTED
self._update_ui_state()
Config().set("link_uri", str(self._selected_interface))
lg = LogConfig("Battery", 1000)
lg.add_variable("pm.vbat", "float")
lg.add_variable("pm.state", "int8_t")
try:
self.cf.log.add_config(lg)
lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit)
lg.error_cb.add_callback(self._log_error_signal.emit)
lg.start()
except KeyError as e:
logger.warning(str(e))
mems = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)
if len(mems) > 0:
mems[0].write_data(self._led_write_done)
def _disconnected(self):
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
def _connection_initiated(self):
self.uiState = UIState.CONNECTING
self._update_ui_state()
def _led_write_done(self, mem, addr):
logger.info("LED write done callback")
def _logging_error(self, log_conf, msg):
QMessageBox.about(self, "Log error", "Error when starting log config"
" [{}]: {}".format(log_conf.name,
msg))
def _connection_lost(self, linkURI, msg):
if not self._auto_reconnect_enabled:
if self.isActiveWindow():
warningCaption = "Communication failure"
error = "Connection lost to {}: {}".format(linkURI, msg)
QMessageBox.critical(self, warningCaption, error)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def _connection_failed(self, linkURI, error):
if not self._auto_reconnect_enabled:
msg = "Failed to connect on {}: {}".format(linkURI, error)
warningCaption = "Communication failure"
QMessageBox.critical(self, warningCaption, msg)
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self._connect()
def closeEvent(self, event):
self.hide()
self.cf.close_link()
Config().save_file()
def resizeEvent(self, event):
Config().set("window_size", [event.size().width(),
event.size().height()])
def _connect(self):
if self.uiState == UIState.CONNECTED:
self.cf.close_link()
elif self.uiState == UIState.CONNECTING:
self.cf.close_link()
self.uiState = UIState.DISCONNECTED
self._update_ui_state()
else:
self.cf.open_link(self._selected_interface)
def _scan(self):
self.uiState = UIState.SCANNING
self._update_ui_state()
self.scanner.scanSignal.emit(self.address.value())
def _display_input_device_error(self, error):
self.cf.close_link()
QMessageBox.critical(self, "Input device error", error)
def _mux_selected(self, checked):
"""Called when a new mux is selected. The menu item contains a
reference to the raw mux object as well as to the associated device
sub-nodes"""
if not checked:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(False)
else:
(mux, sub_nodes) = self.sender().data()
for s in sub_nodes:
s.setEnabled(True)
self.joystickReader.set_mux(mux=mux)
# Go though the tree and select devices/mapping that was
# selected before it was disabled.
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
dev_node.toggled.emit(True)
self._update_input_device_footer()
def _get_dev_status(self, device):
msg = "{}".format(device.name)
if device.supports_mapping:
map_name = "No input mapping"
if device.input_map:
map_name = device.input_map_name
msg += " ({})".format(map_name)
return msg
def _update_input_device_footer(self):
"""Update the footer in the bottom of the UI with status for the
input device and its mapping"""
msg = ""
if len(self.joystickReader.available_devices()) > 0:
mux = self.joystickReader._selected_mux
msg = "Using {} mux with ".format(mux.name)
for key in list(mux._devs.keys())[:-1]:
if mux._devs[key]:
msg += "{}, ".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A, "
# Last item
key = list(mux._devs.keys())[-1]
if mux._devs[key]:
msg += "{}".format(self._get_dev_status(mux._devs[key]))
else:
msg += "N/A"
else:
msg = "No input device found"
self._statusbar_label.setText(msg)
def _inputdevice_selected(self, checked):
"""Called when a new input device has been selected from the menu. The
data in the menu object is the associated map menu (directly under the
item in the menu) and the raw device"""
(map_menu, device, mux_menu) = self.sender().data()
if not checked:
if map_menu:
map_menu.setEnabled(False)
# Do not close the device, since we don't know exactly
# how many devices the mux can have open. When selecting a
# new mux the old one will take care of this.
else:
if map_menu:
map_menu.setEnabled(True)
(mux, sub_nodes) = mux_menu.data()
for role_node in sub_nodes:
for dev_node in role_node.children():
if type(dev_node) is QAction and dev_node.isChecked():
if device.id == dev_node.data()[1].id \
and dev_node is not self.sender():
dev_node.setChecked(False)
role_in_mux = str(self.sender().parent().title()).strip()
logger.info("Role of {} is {}".format(device.name,
role_in_mux))
Config().set("input_device", str(device.name))
self._mapping_support = self.joystickReader.start_input(
device.name,
role_in_mux)
self._update_input_device_footer()
def _inputconfig_selected(self, checked):
"""Called when a new configuration has been selected from the menu. The
data in the menu object is a referance to the device QAction in parent
menu. This contains a referance to the raw device."""
if not checked:
return
selected_mapping = str(self.sender().text())
device = self.sender().data().data()[1]
self.joystickReader.set_input_map(device.name, selected_mapping)
self._update_input_device_footer()
def device_discovery(self, devs):
"""Called when new devices have been added"""
for menu in self._all_role_menus:
role_menu = menu["rolemenu"]
mux_menu = menu["muxmenu"]
dev_group = QActionGroup(role_menu, exclusive=True)
for d in devs:
dev_node = QAction(d.name, role_menu, checkable=True,
enabled=True)
role_menu.addAction(dev_node)
dev_group.addAction(dev_node)
dev_node.toggled.connect(self._inputdevice_selected)
map_node = None
if d.supports_mapping:
map_node = QMenu(" Input map", role_menu, enabled=False)
map_group = QActionGroup(role_menu, exclusive=True)
# Connect device node to map node for easy
# enabling/disabling when selection changes and device
# to easily enable it
dev_node.setData((map_node, d))
for c in ConfigManager().get_list_of_configs():
node = QAction(c, map_node, checkable=True,
enabled=True)
node.toggled.connect(self._inputconfig_selected)
map_node.addAction(node)
# Connect all the map nodes back to the device
# action node where we can access the raw device
node.setData(dev_node)
map_group.addAction(node)
# If this device hasn't been found before, then
# select the default mapping for it.
if d not in self._available_devices:
last_map = Config().get("device_config_mapping")
if d.name in last_map and last_map[d.name] == c:
node.setChecked(True)
role_menu.addMenu(map_node)
dev_node.setData((map_node, d, mux_menu))
# Update the list of what devices we found
# to avoid selecting default mapping for all devices when
# a new one is inserted
self._available_devices = ()
for d in devs:
self._available_devices += (d,)
# Only enable MUX nodes if we have enough devies to cover
# the roles
for mux_node in self._all_mux_nodes:
(mux, sub_nodes) = mux_node.data()
if len(mux.supported_roles()) <= len(self._available_devices):
mux_node.setEnabled(True)
# TODO: Currently only supports selecting default mux
if self._all_mux_nodes[0].isEnabled():
self._all_mux_nodes[0].setChecked(True)
# If the previous length of the available devies was 0, then select
# the default on. If that's not available then select the first
# on in the list.
# TODO: This will only work for the "Normal" mux so this will be
# selected by default
if Config().get("input_device") in [d.name for d in devs]:
for dev_menu in self._all_role_menus[0]["rolemenu"].actions():
if dev_menu.text() == Config().get("input_device"):
dev_menu.setChecked(True)
else:
# Select the first device in the first mux (will always be "Normal"
# mux)
self._all_role_menus[0]["rolemenu"].actions()[0].setChecked(True)
logger.info("Select first device")
self._update_input_device_footer()
def _open_config_folder(self):
QDesktopServices.openUrl(
QUrl("file:///" +
QDir.toNativeSeparators(edclient.config_path)))
def closeAppRequest(self):
self.close()
sys.exit(0)
Uploader().upload(trajectory_mem)
ed.high_level_commander.define_trajectory(trajectory_id, 0,
len(trajectory_mem.poly4Ds))
return total_duration
def run_sequence(ed, trajectory_id, duration):
commander = ed.high_level_commander
commander.takeoff(1.0, 2.0)
time.sleep(3.0)
relative = True
#commander.start_trajectory(trajectory_id, 1.0, relative)
time.sleep(duration)
commander.land(0.0, 2.0)
time.sleep(2)
commander.stop()
if __name__ == '__main__':
edlib.crtp.init_drivers(enable_debug_driver=False)
ed = Espdrone(uri)
trajectory_id = 1
#activate_high_level_commander(ed)
# activate_mellinger_controller(ed)
#duration = upload_trajectory(ed, trajectory_id, figure8)
#print('The sequence is {:.1f} seconds long'.format(duration))
#reset_estimator(ed)
run_sequence(ed, trajectory_id, 1)
logging.basicConfig(level=logging.ERROR)
if __name__ == '__main__':
# Initialize the low-level drivers (don't list the debug drivers)
edlib.crtp.init_drivers(enable_debug_driver=False)
# Scan for Espdrones and use the first one found
print('Scanning interfaces for Espdrones...')
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
for i in available:
print(i[0])
if len(available) == 0:
print('No Espdrones found, cannot run example')
else:
with SyncEspdrone(available[0][0],
ed=Espdrone(rw_cache='./cache')) as sed:
# We take off when the commander is created
with MotionCommander(sed) as mc:
time.sleep(1)
# There is a set of functions that move a specific distance
# We can move in all directions
mc.forward(0.8)
mc.back(0.8)
time.sleep(1)
mc.up(0.5)
mc.down(0.5)
time.sleep(1)
# We can also set the velocity
