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()
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()
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
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)
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()
parser.add_argument("--uri", help='The ip address of the drone, e.g. "192.168.0.102"')
args = parser.parse_args()
if args.uri:
uri = args.uri
else:
uri = '192.168.43.42'
available = edlib.crtp.scan_interfaces()
print('Espdrones found:')
if available:
print(available[0])
uri = available[0][0]
else:
quit()
ed.open_link(uri)
ed.link.socket.settimeout(None)
is_streaming = True
camera.start()
camera.image_received_cb.add_callback(show_image)
while is_streaming and ed.link:
time.sleep(1)
camera.stop()
cv2.destroyAllWindows()
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 LoggingExample:
"""
Simple logging example class that logs the Stabilizer from a supplied
link uri and disconnects after 10s.
"""
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 _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)
# The definition of the logconfig can be made before connecting
self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=10)
self._lg_stab.add_variable('stabilizer.roll', 'float')
self._lg_stab.add_variable('stabilizer.pitch', 'float')
self._lg_stab.add_variable('stabilizer.yaw', 'float')
# Adding the configuration cannot be done until a Espdrone is
# connected, since we need to check that the variables we
# would like to log are in the TOC.
try:
self._ed.log.add_config(self._lg_stab)
# This callback will receive the data
self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
# This callback will be called on errors
self._lg_stab.error_cb.add_callback(self._stab_log_error)
# Start the logging
self._lg_stab.start()
except KeyError as e:
print('Could not start log configuration,'
'{} not found in TOC'.format(str(e)))
except AttributeError:
print('Could not add Stabilizer log config, bad configuration.')
# Start a timer to disconnect in 10s
t = Timer(10, self._ed.close_link)
t.start()
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 SyncEspdrone:
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
def open_link(self):
if (self.is_link_open()):
raise Exception('Link already open')
self._add_callbacks()
print('Connecting to %s' % self._link_uri)
self.ed.open_link(self._link_uri)
self._connect_event.wait()
if not self._is_link_open:
self._remove_callbacks()
raise Exception(self._error_message)
def __enter__(self):
self.open_link()
return self
def close_link(self):
self.ed.close_link()
self._remove_callbacks()
self._is_link_open = False
def __exit__(self, exc_type, exc_val, exc_tb):
self.close_link()
def is_link_open(self):
return self._is_link_open
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)
self._is_link_open = True
self._connect_event.set()
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))
self._is_link_open = False
self._error_message = msg
self._connect_event.set()
def _disconnected(self, link_uri):
self._remove_callbacks()
self._is_link_open = False
def _add_callbacks(self):
self.ed.connected.add_callback(self._connected)
self.ed.connection_failed.add_callback(self._connection_failed)
self.ed.disconnected.add_callback(self._disconnected)
def _remove_callbacks(self):
def remove_callback(container, callback):
try:
container.remove_callback(callback)
except ValueError:
pass
remove_callback(self.ed.connected, self._connected)
remove_callback(self.ed.connection_failed, self._connection_failed)
remove_callback(self.ed.disconnected, self._disconnected)
class HeadlessClient():
"""Espdrone headless client"""
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 setup_controller(self, input_config, input_device=0, xmode=False):
"""Set up the device reader"""
# Set up the joystick reader
self._jr.device_error.add_callback(self._input_dev_error)
print("Client side X-mode: %s" % xmode)
if (xmode):
self._cf.commander.set_client_xmode(xmode)
devs = self._jr.available_devices() # noqa, is this a bug?
print("Will use [%s] for input" % self._devs[input_device])
self._jr.start_input(self._devs[input_device])
self._jr.set_input_map(self._devs[input_device], input_config)
def controller_connected(self):
""" Return True if a controller is connected"""
return True if (len(self._jr.available_devices()) > 0) else False
def list_controllers(self):
"""List the available controllers and input mapping"""
print("\nAvailable controllers:")
for i, dev in enumerate(self._devs):
print(" - Controller #{}: {}".format(i, dev))
print("\nAvailable input mapping:")
for map in os.listdir(edclient.config_path + '/input'):
print(" - " + map.split(".json")[0])
def connect_espdrone(self, link_uri):
"""Connect to a Espdrone on the given link uri"""
self._cf.connection_failed.add_callback(self._connection_failed)
# 2014-11-25 chad: Add a callback for when we have a good connection.
self._cf.connected.add_callback(self._connected)
self._cf.param.add_update_callback(
group="imu_sensors",
name="HMC5883L",
cb=(lambda name, found: self._jr.set_alt_hold_available(eval(found)
)))
self._jr.assisted_control_updated.add_callback(
lambda enabled: self._cf.param.set_value("flightmode.althold",
enabled))
self._cf.open_link(link_uri)
self._jr.input_updated.add_callback(self._cf.commander.send_setpoint)
def _connected(self, link):
"""Callback for a successful Espdrone connection."""
print("Connected to {}".format(link))
def _connection_failed(self, link, message):
"""Callback for a failed Espdrone connection"""
print("Connection failed on {}: {}".format(link, message))
sys.exit(-1)
def _input_dev_error(self, message):
"""Callback for an input device error"""
print("Error when reading device: {}".format(message))
sys.exit(-1)
class MotorRampExample:
"""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(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 _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._ramp_motors).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 _ramp_motors(self):
thrust_mult = 1
thrust_step = 500
thrust = 20000
pitch = 0
roll = 0
yawrate = 0
# Unlock startup thrust protection
self._ed.commander.send_setpoint(0, 0, 0, 0)
while thrust >= 20000:
self._ed.commander.send_setpoint(roll, pitch, yawrate, thrust)
time.sleep(0.1)
if thrust >= 25000:
thrust_mult = -1
thrust += thrust_step * thrust_mult
self._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)
self._ed.close_link()
class WriteOwExample:
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_1W)
print('Found {} 1-wire memories'.format(len(mems)))
if len(mems) > 0:
print('Writing test configuration to'
' memory {}'.format(mems[0].id))
mems[0].vid = 0xBC
mems[0].pid = 0xFF
board_name_id = OWElement.element_mapping[1]
board_rev_id = OWElement.element_mapping[2]
mems[0].elements[board_name_id] = 'Test board'
mems[0].elements[board_rev_id] = 'A'
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('\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._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 RadioBridge:
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 _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)
self._ed.packet_received.add_callback(self._got_packet)
def _got_packet(self, pk):
if pk.port == CRTP_PORT_MAVLINK:
self._sock.sendto(pk.data, ('127.0.0.1', 14550))
def _forward(self, data):
pk = CRTPPacket()
pk.port = CRTP_PORT_MAVLINK # CRTPPort.COMMANDER
pk.data = data # struct.pack('<fffH', roll, -pitch, yaw, thrust)
self._ed.send_packet(pk)
def _server(self):
while True:
print('\nwaiting to receive message')
# Only receive what can be sent in one message
data, address = self._sock.recvfrom(256)
print('received %s bytes from %s' % (len(data), address))
for i in range(0, len(data), 30):
self._forward(data[i:(i + 30)])
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
