class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0): """Set up the device reader""" # Set values for input from config (advanced) self._jr.set_thrust_limits( self._p2t(Config().get("min_thrust")), self._p2t(Config().get("max_thrust"))) self._jr.set_rp_limit(Config().get("max_rp")) self._jr.set_yaw_limit(Config().get("max_yaw")) self._jr.set_thrust_slew_limiting( self._p2t(Config().get("slew_rate")), self._p2t(Config().get("slew_limit"))) # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connectionFailed.add_callback(self._connection_failed) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() 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) def _p2t(self, percentage): """Convert a percentage to raw thrust""" return int(65000 * (percentage / 100.0))
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) 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.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connection_failed.add_callback(self._connection_failed) self._cf.param.add_update_callback( group="imu_sensors", name="HMC5883L", cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found)))) self._jr.althold_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 _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() 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 HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0]+"/cflib/cache", rw_cache=sys.path[1]+"/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) 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.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connection_failed.add_callback(self._connection_failed) self._cf.param.add_update_callback(group="imu_sensors", name="HMC5883L", cb=(lambda name, found: self._jr.setAltHoldAvailable(eval(found)))) self._jr.althold_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 _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() 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 HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") signal.signal(signal.SIGINT, signal.SIG_DFL) def setup_controller(self, input_config, input_device=0): """Set up the device reader""" # Set values for input from config (advanced) self._jr.set_thrust_limits(self._p2t(Config().get("min_thrust")), self._p2t(Config().get("max_thrust"))) self._jr.set_rp_limit(Config().get("max_rp")) self._jr.set_yaw_limit(Config().get("max_yaw")) self._jr.set_thrust_slew_limiting( self._p2t(Config().get("slew_rate")), self._p2t(Config().get("slew_limit"))) # Set up the joystick reader self._jr.device_error.add_callback(self._input_dev_error) devs = self._jr.getAvailableDevices() print "Will use [%s] for input" % devs[input_device]["name"] self._jr.start_input(devs[input_device]["name"], input_config) def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print "Controller #{}: {}".format(dev["id"], dev["name"]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie on the given link uri""" self._cf.connectionFailed.add_callback(self._connection_failed) self._cf.open_link(link_uri) self._jr.input_updated.add_callback(self._cf.commander.send_setpoint) def _connection_failed(self, link, message): """Callback for a failed Crazyflie connection""" print "Connection failed on {}: {}".format(link, message) self._jr.stop_input() 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) def _p2t(self, percentage): """Convert a percentage to raw thrust""" return int(65000 * (percentage / 100.0))
def main(): # Creating a soca bridge serial_dev = '/dev/ttyS11' serial_dev_agent = '/dev/ttyS10' socat_process = subprocess.Popen( ["socat", "PTY,link=/dev/ttyS10", "PTY,link=" + serial_dev]) time.sleep(1) print("Use " + serial_dev_agent + " for micro-ROS agent connection") # Set logging level logging.basicConfig(level=logging.INFO) # Open serial port serialport = serial.Serial(serial_dev, baudrate=115200, timeout=0.1) # Retrieve controller jr = JoystickReader(do_device_discovery=False) available_devices = jr.available_devices() if len(available_devices): used_device = available_devices[0] print("Using input device: " + str(used_device)) jr.device_error.add_callback( lambda msg: print("Input device error: " + str(msg))) jr.start_input(used_device.name) jr.set_input_map(used_device.name, "PS3_Mode_1") else: used_device = None print("Using no input device") # Configure radio link cflib.crtp.radiodriver.set_retries_before_disconnect(-1) cflib.crtp.radiodriver.set_retries(100) cflib.crtp.init_drivers(enable_debug_driver=False) link_uri = "radio://0/65/2M" try: while True: cf = CrazyflieBridgedController(link_uri, serialport, jr if used_device else None) while cf.is_connected: time.sleep(0.1) print("Disconected from: " + str(link_uri)) except KeyboardInterrupt: pass os.killpg(os.getpgid(socat_process.pid), signal.SIGTERM)
class MainUI(QtGui.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) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers( enable_debug_driver=GuiConfig().get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback( self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) 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) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect( self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked( GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback(self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [ dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().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 GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != 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 [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.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.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about( self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get("device_config_mapping")[ self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text( ) GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText( "Using [%s] with config [%s]" % (self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[str( self._active_device)] else: self._current_input_config = self._menu_mappings.actions( )[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text( ) # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl( QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class MainUI(QtGui.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) self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers(enable_debug_driver=GuiConfig() .get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader(cf=self.cf) self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() self._update_input ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) self.connectionDoneSignal.connect(self.connectionDone) self.cf.connection_failed.add_callback(self.connectionFailedSignal.emit) self.connectionFailedSignal.connect(self.connectionFailed) self._input_device_error_signal.connect(self.inputDeviceError) 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) # Connect UI signals self.menuItemConnect.triggered.connect(self.connectButtonClicked) self.logConfigAction.triggered.connect(self.doLogConfigDialogue) self.connectButton.clicked.connect(self.connectButtonClicked) self.quickConnectButton.clicked.connect(self.quickConnect) self.menuItemQuickConnect.triggered.connect(self.quickConnect) self.menuItemConfInputDevice.triggered.connect(self.configInputDevice) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self.updateBatteryVoltage) self._menuitem_rescandevices.triggered.connect(self._rescan_devices) self._menuItem_openconfigfolder.triggered.connect(self._open_config_folder) self._auto_reconnect_enabled = GuiConfig().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(GuiConfig().get("auto_reconnect")) # Do not queue data from the controller output to the Crazyflie wrapper # to avoid latency #self.joystickReader.sendControlSetpointSignal.connect( # self.cf.commander.send_setpoint, # Qt.DirectConnection) self.joystickReader.input_updated.add_callback( self.cf.commander.send_setpoint) # Connection callbacks and signal wrappers for UI protection self.cf.connected.add_callback( self.connectionDoneSignal.emit) self.connectionDoneSignal.connect(self.connectionDone) self.cf.disconnected.add_callback(self.disconnectedSignal.emit) self.disconnectedSignal.connect( lambda linkURI: self.setUIState(UIState.DISCONNECTED, linkURI)) self.cf.connection_lost.add_callback(self.connectionLostSignal.emit) self.connectionLostSignal.connect(self.connectionLost) self.cf.connection_requested.add_callback( self.connectionInitiatedSignal.emit) self.connectionInitiatedSignal.connect( lambda linkURI: self.setUIState(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self.setUIState(UIState.DISCONNECTED) # Parse the log configuration files self.logConfigReader = LogConfigReader(self.cf) # Add things to helper so tabs can access it cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.ui.pluginhelper) self.menuItemAbout.triggered.connect(self._about_dialog.show) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().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 GuiConfig().get("open_tabs").split(","): t = tabItems[tName] if (t != 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 [%s]", e) def setUIState(self, newState, linkURI=""): self.uiState = newState if (newState == UIState.DISCONNECTED): self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.setEnabled(True) self.batteryBar.setValue(3000) self.linkQualityBar.setValue(0) self.menuItemBootloader.setEnabled(True) self.logConfigAction.setEnabled(False) if (len(GuiConfig().get("link_uri")) > 0): self.quickConnectButton.setEnabled(True) if (newState == UIState.CONNECTED): s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") self.logConfigAction.setEnabled(True) if (newState == UIState.CONNECTING): s = "Connecting to %s ..." % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.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.getAvailableDevices() if (len(devs) > 0): self.device_discovery(devs) def configInputDevice(self): self.inputConfig = InputConfigDialogue(self.joystickReader) self.inputConfig.show() def _auto_reconnect_changed(self, checked): self._auto_reconnect_enabled = checked GuiConfig().set("auto_reconnect", checked) logger.info("Auto reconnect enabled: %s", checked) def doLogConfigDialogue(self): self.logConfigDialogue.show() def updateBatteryVoltage(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) cfclient.ui.pluginhelper.inputDeviceReader.inputdevice.setBatteryData(int(data["pm.vbat"] * 1000)) def connectionDone(self, linkURI): self.setUIState(UIState.CONNECTED, linkURI) GuiConfig().set("link_uri", linkURI) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") self.cf.log.add_config(lg) if lg.valid: lg.data_received_cb.add_callback(self.batteryUpdatedSignal.emit) lg.error_cb.add_callback(self._log_error_signal.emit) lg.start() else: logger.warning("Could not setup loggingblock!") def _logging_error(self, log_conf, msg): QMessageBox.about(self, "Log error", "Error when starting log config" " [%s]: %s" % (log_conf.name, msg)) def connectionLost(self, linkURI, msg): if not self._auto_reconnect_enabled: if (self.isActiveWindow()): warningCaption = "Communication failure" error = "Connection lost to %s: %s" % (linkURI, msg) QMessageBox.critical(self, warningCaption, error) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def connectionFailed(self, linkURI, error): if not self._auto_reconnect_enabled: msg = "Failed to connect on %s: %s" % (linkURI, error) warningCaption = "Communication failure" QMessageBox.critical(self, warningCaption, msg) self.setUIState(UIState.DISCONNECTED, linkURI) else: self.quickConnect() def closeEvent(self, event): self.hide() self.cf.close_link() GuiConfig().save_file() def connectButtonClicked(self): if (self.uiState == UIState.CONNECTED): self.cf.close_link() elif (self.uiState == UIState.CONNECTING): self.cf.close_link() self.setUIState(UIState.DISCONNECTED) else: self.connectDialogue.show() def inputDeviceError(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self.joystickReader.stop_input() self._active_config = str(config) self._active_device = str(device) GuiConfig().set("input_device", self._active_device) GuiConfig().get( "device_config_mapping" )[self._active_device] = self._active_config self.joystickReader.start_input(self._active_device, self._active_config) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [%s] - " "No input config selected" % (self._active_device)) else: self._statusbar_label.setText("Using [%s] with config [%s]" % (self._active_device, self._active_config)) def _inputdevice_selected(self, checked): if (not checked): return self.joystickReader.stop_input() sender = self.sender() self._active_device = sender.text() device_config_mapping = GuiConfig().get("device_config_mapping") if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() GuiConfig().set("input_device", str(self._active_device)) for c in self._menu_mappings.actions(): if (c.text() == self._current_input_config): c.setChecked(True) self.joystickReader.start_input(str(sender.text()), self._current_input_config) self._statusbar_label.setText("Using [%s] with config [%s]" % ( self._active_device, self._current_input_config)) def _inputconfig_selected(self, checked): if (not checked): return self._update_input(self._active_device, self.sender().text()) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=True) for d in devs: node = QAction(d["name"], self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if (d["name"] == GuiConfig().get("input_device")): self._active_device = d["name"] if (len(self._active_device) == 0): self._active_device = self._menu_devices.actions()[0].text() device_config_mapping = GuiConfig().get("device_config_mapping") if (device_config_mapping): if (self._active_device in device_config_mapping.keys()): self._current_input_config = device_config_mapping[ str(self._active_device)] else: self._current_input_config = self._menu_mappings.actions()[0].text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_mappings.actions(): c.setEnabled(True) if (c.text() == self._current_input_config): c.setChecked(True) for c in self._menu_devices.actions(): if (c.text() == self._active_device): c.setChecked(True) def quickConnect(self): try: self.cf.open_link(GuiConfig().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class MainUI(QtGui.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) ###################################################### ### 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:") tcss = """ QProgressBar { border: 2px solid grey; border-radius: 5px; text-align: center; } QProgressBar::chunk { background-color: #05B8CC; } """ self.setStyleSheet(tcss) else: logger.info( "Pre-Yosemite") ###################################################### self.cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/cache") cflib.crtp.init_drivers(enable_debug_driver=Config() .get("enable_debug_driver")) # Create the connection dialogue self.connectDialogue = ConnectDialogue() # Create and start the Input Reader self._statusbar_label = QLabel("Loading device and configuration.") self.statusBar().addWidget(self._statusbar_label) self.joystickReader = JoystickReader() self._active_device = "" self.configGroup = QActionGroup(self._menu_mappings, exclusive=True) self._load_input_data() ConfigManager().conf_needs_reload.add_callback(self._reload_configs) # Connections for the Connect Dialogue self.connectDialogue.requestConnectionSignal.connect(self.cf.open_link) 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) # Connect UI signals self.menuItemConnect.triggered.connect(self._connect) self.logConfigAction.triggered.connect(self._show_connect_dialog) self.connectButton.clicked.connect(self._connect) self.quickConnectButton.clicked.connect(self._quick_connect) self.menuItemQuickConnect.triggered.connect(self._quick_connect) self.menuItemConfInputDevice.triggered.connect(self._show_input_device_config_dialog) self.menuItemExit.triggered.connect(self.closeAppRequest) self.batteryUpdatedSignal.connect(self._update_vbatt) 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.joystickReader.input_updated.add_callback( self.cf.commander.send_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( lambda linkURI: self._update_ui_state(UIState.DISCONNECTED, linkURI)) 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( lambda linkURI: self._update_ui_state(UIState.CONNECTING, linkURI)) self._log_error_signal.connect(self._logging_error) # Connect link quality feedback self.cf.link_quality_updated.add_callback(self.linkQualitySignal.emit) self.linkQualitySignal.connect( lambda percentage: self.linkQualityBar.setValue(percentage)) # Set UI state in disconnected buy default self._update_ui_state(UIState.DISCONNECTED) # 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 cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.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) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().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 != 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)) # Check which Input muxes are available self._mux_group = QActionGroup(self._menu_mux, exclusive=True) for m in self.joystickReader.available_mux(): node = QAction(m, self._menu_mux, checkable=True, enabled=True) node.toggled.connect(self._mux_selected) self._mux_group.addAction(node) self._menu_mux.addAction(node) # TODO: Temporary self._input_dev_stack = [] self._menu_mux.actions()[0].setChecked(True) if Config().get("enable_input_muxing"): self._menu_mux.setEnabled(True) else: logger.info("Input device muxing disabled in config") self._mapping_support = True def _update_ui_state(self, newState, linkURI=""): self.uiState = newState if newState == UIState.DISCONNECTED: self.setWindowTitle("Not connected") self.menuItemConnect.setText("Connect to Crazyflie") self.connectButton.setText("Connect") self.menuItemQuickConnect.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) if len(Config().get("link_uri")) > 0: self.quickConnectButton.setEnabled(True) if newState == UIState.CONNECTED: s = "Connected on %s" % linkURI self.setWindowTitle(s) self.menuItemConnect.setText("Disconnect") self.connectButton.setText("Disconnect") 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) if newState == UIState.CONNECTING: s = "Connecting to {} ...".format(linkURI) self.setWindowTitle(s) self.menuItemConnect.setText("Cancel") self.connectButton.setText("Cancel") self.quickConnectButton.setEnabled(False) self.menuItemBootloader.setEnabled(False) self.menuItemQuickConnect.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_vbatt(self, timestamp, data, logconf): self.batteryBar.setValue(int(data["pm.vbat"] * 1000)) def _connected(self, linkURI): self._update_ui_state(UIState.CONNECTED, linkURI) Config().set("link_uri", str(linkURI)) lg = LogConfig("Battery", 1000) lg.add_variable("pm.vbat", "float") 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)) 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._update_ui_state(UIState.DISCONNECTED, linkURI) else: self._quick_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._update_ui_state(UIState.DISCONNECTED, linkURI) else: self._quick_connect() def closeEvent(self, event): self.hide() self.cf.close_link() Config().save_file() def _connect(self): if self.uiState == UIState.CONNECTED: self.cf.close_link() elif self.uiState == UIState.CONNECTING: self.cf.close_link() self._update_ui_state(UIState.DISCONNECTED) else: self.connectDialogue.show() def _display_input_device_error(self, error): self.cf.close_link() QMessageBox.critical(self, "Input device error", error) def _load_input_data(self): self.joystickReader.stop_input() # Populate combo box with available input device configurations for c in ConfigManager().get_list_of_configs(): node = QAction(c, self._menu_mappings, checkable=True, enabled=False) node.toggled.connect(self._inputconfig_selected) self.configGroup.addAction(node) self._menu_mappings.addAction(node) def _reload_configs(self, newConfigName): # remove the old actions from the group and the menu for action in self._menu_mappings.actions(): self.configGroup.removeAction(action) self._menu_mappings.clear() # reload the conf files, and populate the menu self._load_input_data() self._update_input(self._active_device, newConfigName) def _update_input(self, device="", config=""): self._active_config = str(config) self._active_device = str(device) Config().set("input_device", str(self._active_device)) # update the checked state of the menu items for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._active_config: c.setChecked(True) for c in self._menu_devices.actions(): c.setEnabled(True) if c.text() == self._active_device: c.setChecked(True) # update label if device == "" and config == "": self._statusbar_label.setText("No input device selected") elif config == "": self._statusbar_label.setText("Using [{}] - " "No input config selected".format (self._active_device)) else: self._statusbar_label.setText("Using [{}] with config [{}]".format (self._active_device, self._active_config)) def _mux_selected(self, checked): if not checked: return selected_mux_name = str(self.sender().text()) self.joystickReader.set_mux(name=selected_mux_name) logger.debug("Selected mux supports {} devices".format(self.joystickReader.get_mux_supported_dev_count())) self._adjust_nbr_of_selected_devices() def _get_saved_device_mapping(self, device_name): """Return the saved mapping for a given device""" config = None device_config_mapping = Config().get("device_config_mapping") if device_name in device_config_mapping.keys(): config = device_config_mapping[device_name] logging.debug("For [{}] we recommend [{}]".format(device_name, config)) return config def _update_input_device_footer(self, device_name=None, mapping_name=None): """Update the footer in the bottom of the UI with status for the input device and its mapping""" if not device_name and not mapping_name: self._statusbar_label.setText("No input device selected") elif self._mapping_support and not mapping_name: self._statusbar_label.setText("Using [{}] - " "No input config selected".format( device_name)) elif not self._mapping_support: self._statusbar_label.setText("Using [{}]".format(device_name)) else: self._statusbar_label.setText("Using [{}] with config [{}]".format( device_name, mapping_name)) def _adjust_nbr_of_selected_devices(self): nbr_of_selected = len(self._input_dev_stack) nbr_of_supported = self.joystickReader.get_mux_supported_dev_count() while len(self._input_dev_stack) > nbr_of_supported: to_close = self._input_dev_stack.pop(0) # Close and de-select it in the UI self.joystickReader.stop_input(to_close) for c in self._menu_devices.actions(): if c.text() == to_close: c.setChecked(False) def _inputdevice_selected(self, checked): """Called when a new input device has been selected from the menu""" if not checked: return self._input_dev_stack.append(self.sender().text()) selected_device_name = str(self.sender().text()) self._active_device = selected_device_name # Save the device as "last used device" Config().set("input_device", str(selected_device_name)) # Read preferred config used for this controller from config, # if found then select this config in the menu self._mapping_support = self.joystickReader.start_input(selected_device_name) self._adjust_nbr_of_selected_devices() if self.joystickReader.get_mux_supported_dev_count() == 1: preferred_config = self.joystickReader.get_saved_device_mapping(selected_device_name) if preferred_config: for c in self._menu_mappings.actions(): if c.text() == preferred_config: c.setChecked(True) def _inputconfig_selected(self, checked): """Called when a new configuration has been selected from the menu""" if not checked: return selected_mapping = str(self.sender().text()) self.joystickReader.set_input_map(self._active_device, selected_mapping) self._update_input_device_footer(self._active_device, selected_mapping) def device_discovery(self, devs): group = QActionGroup(self._menu_devices, exclusive=False) for d in devs: node = QAction(d.name, self._menu_devices, checkable=True) node.toggled.connect(self._inputdevice_selected) group.addAction(node) self._menu_devices.addAction(node) if d.name == Config().get("input_device"): self._active_device = d.name if len(self._active_device) == 0: self._active_device = str(self._menu_devices.actions()[0].text()) device_config_mapping = Config().get("device_config_mapping") if device_config_mapping: if self._active_device in device_config_mapping.keys(): self._current_input_config = device_config_mapping[ self._active_device] else: self._current_input_config = self._menu_mappings.actions()[0]\ .text() else: self._current_input_config = self._menu_mappings.actions()[0].text() # Now we know what device to use and what mapping, trigger the events # to change the menus and start the input for c in self._menu_devices.actions(): if c.text() == self._active_device: c.setChecked(True) for c in self._menu_mappings.actions(): c.setEnabled(True) if c.text() == self._current_input_config: c.setChecked(True) def _quick_connect(self): try: self.cf.open_link(Config().get("link_uri")) except KeyError: self.cf.open_link("") def _open_config_folder(self): QDesktopServices.openUrl(QUrl("file:///" + QDir.toNativeSeparators(sys.path[1]))) def closeAppRequest(self): self.close() sys.exit(0)
class MainUI(QtGui.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) ###################################################### # 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 = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.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.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.address.setValue(0xE7E7E7E7E7) self._auto_reconnect_enabled = Config().get("auto_reconnect") self.autoReconnectCheckBox.toggled.connect( self._auto_reconnect_changed) self.autoReconnectCheckBox.setChecked(Config().get("auto_reconnect")) self.joystickReader.input_updated.add_callback( self.cf.commander.send_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 cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.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) # Loading toolboxes (A bit of magic for a lot of automatic) self.toolboxes = [] self.toolboxesMenuItem.setMenu(QtGui.QMenu()) for t_class in cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.ui.pluginhelper) dockToolbox = MyDockWidget(toolbox.getName()) dockToolbox.setWidget(toolbox) self.toolboxes += [dockToolbox, ] # Add menu item for the toolbox item = QtGui.QAction(toolbox.getName(), self) item.setCheckable(True) item.triggered.connect(self.toggleToolbox) self.toolboxesMenuItem.menu().addAction(item) dockToolbox.closed.connect(lambda: self.toggleToolbox(False)) # Setup some introspection item.dockToolbox = dockToolbox item.menuItem = item dockToolbox.dockToolbox = dockToolbox dockToolbox.menuItem = item # Load and connect tabs self.tabsMenuItem.setMenu(QtGui.QMenu()) tabItems = {} self.loadedTabs = [] for tabClass in cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.ui.pluginhelper) item = QtGui.QAction(tab.getMenuName(), self) item.setCheckable(True) item.toggled.connect(tab.toggleVisibility) self.tabsMenuItem.menu().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)) # 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 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() self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT) formatted_interfaces = [] for i in interfaces: if len(i[1]) > 0: interface = "%s - %s" % (i[0], i[1]) else: interface = i[0] formatted_interfaces.append(interface) self.interfaceCombo.addItems(formatted_interfaces) if self._initial_scan: self._initial_scan = False try: selected_interface = Config().get("link_uri") except KeyError: pass newIndex = 0 if selected_interface is not None: try: newIndex = formatted_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 Crazyflie") self.menuItemConnect.setEnabled(canConnect) self.connectButton.setText("Connect") self.connectButton.setToolTip( "Connect to the Crazyflie on the selected interface") 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 Crazyflie") 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 Crazyflie") 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)) 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)) mem = self.cf.mem.get_mems(MemoryElement.TYPE_DRIVER_LED)[0] mem.write_data(self._led_write_done) # self._led_write_test = 0 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.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") # self._led_write_test += 1 # mem.leds[self._led_write_test] = [10, 20, 30] # mem.write_data(self._led_write_done) 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 _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 = "N/A" 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(cfclient.config_path))) def closeAppRequest(self): self.close() sys.exit(0)
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 = Crazyflie(ro_cache=None, rw_cache=cfclient.config_path + "/cache") cflib.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.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.address.setValue(0xE7E7E7E7E7) 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 cfclient.ui.pluginhelper.cf = self.cf cfclient.ui.pluginhelper.inputDeviceReader = self.joystickReader cfclient.ui.pluginhelper.logConfigReader = self.logConfigReader cfclient.ui.pluginhelper.mainUI = self self.logConfigDialogue = LogConfigDialogue(cfclient.ui.pluginhelper) self._bootloader_dialog = BootloaderDialog(cfclient.ui.pluginhelper) self._cf2config_dialog = Cf2ConfigDialog(cfclient.ui.pluginhelper) self._cf1config_dialog = Cf1ConfigDialog(cfclient.ui.pluginhelper) self.menuItemBootloader.triggered.connect(self._bootloader_dialog.show) self._about_dialog = AboutDialog(cfclient.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 cfclient.ui.tabs.available: tab = tabClass(self.tabs, cfclient.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 cfclient.ui.toolboxes.toolboxes: toolbox = t_class(cfclient.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() self.interfaceCombo.addItem(INTERFACE_PROMPT_TEXT) formatted_interfaces = [] for i in interfaces: if len(i[1]) > 0: interface = "%s - %s" % (i[0], i[1]) else: interface = i[0] formatted_interfaces.append(interface) self.interfaceCombo.addItems(formatted_interfaces) if self._initial_scan: self._initial_scan = False try: if len(Config().get("link_uri")) > 0: formatted_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 = formatted_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 Crazyflie") self.menuItemConnect.setEnabled(canConnect) self.connectButton.setText("Connect") self.connectButton.setToolTip( "Connect to the Crazyflie on the selected interface") 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 Crazyflie") 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 Crazyflie") 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)) 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(cfclient.config_path))) def closeAppRequest(self): self.close() sys.exit(0)
class HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/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() 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(sys.path[1] + '/input'): print(" - " + map.split(".json")[0]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie 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.althold_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 Crazyflie connection.""" print("Connected to {}".format(link)) def _connection_failed(self, link, message): """Callback for a failed Crazyflie 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 HeadlessClient(): """Crazyflie headless client""" def __init__(self): """Initialize the headless client and libraries""" cflib.crtp.init_drivers() self._jr = JoystickReader(do_device_discovery=False) self._cf = Crazyflie(ro_cache=sys.path[0] + "/cflib/cache", rw_cache=sys.path[1] + "/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() 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(sys.path[1] + '/input'): print(" - " + map.split(".json")[0]) def connect_crazyflie(self, link_uri): """Connect to a Crazyflie 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.althold_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 Crazyflie connection.""" print("Connected to {}".format(link)) def _connection_failed(self, link, message): """Callback for a failed Crazyflie 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 LogFlight(): def __init__(self, args): self.args = args self.optitrack_enabled = False self.console_dump_enabled = False cflib.crtp.init_drivers(enable_debug_driver=False) self._cf = Crazyflie(rw_cache="./cache") self._jr = JoystickReader(do_device_discovery=False) # Set flight mode if self.args["trajectory"] is None or \ self.args["trajectory"][0] == "none": self.mode = Mode.DONT_FLY print("Mode set to [DONT_FLY]") elif self.args["trajectory"][0] == "manual": self.mode = Mode.MANUAL print("Mode set to [MANUAL]") elif self.args["safetypilot"]: self.mode = Mode.MODE_SWITCH print("Mode set to [MODE_SWITCH]") else: self.mode = Mode.AUTO print("Mode set to [AUTO]") # Setup for specified mode if self.mode == Mode.AUTO: self.is_in_manual_control = False # Make sure drone is setup to perform autonomous flight if args["uwb"] == "none": assert args["optitrack"] == "state", "OptiTrack state needed in absence of UWB" assert args["estimator"] == "kalman", "OptiTrack state needs Kalman estimator" elif self.mode == Mode.DONT_FLY: self.is_in_manual_control = False else: # Check if controller is connected assert self.controller_connected(), "No controller detected." self.setup_controller(map="flappy") self.is_in_manual_control = True # Setup the logging framework self.setup_logger() # Setup optitrack if required if not args["optitrack"] == "none": self.setup_optitrack() def get_filename(self): # create default fileroot if not provided if self.args["fileroot"] is None: date = datetime.today().strftime(r"%Y_%m_%d") self.args["fileroot"] = "data/" + date fileroot = self.args["fileroot"] # create default filename if not provided if self.args["filename"] is None: estimator = self.args["estimator"] uwb = self.args["uwb"] optitrack = self.args["optitrack"] trajectory = self.args["trajectory"] date = datetime.today().strftime(r"%Y-%m-%d+%H:%M:%S") traj = '_'.join(trajectory) if optitrack == "logging": options = "{}+{}+optitracklog+{}".format(estimator, uwb, traj) elif optitrack == "state": options = "{}+{}+optitrackstate+{}".format(estimator, uwb, traj) else: options = "{}+{}+{}".format(estimator, uwb, traj) name = "{}+{}.csv".format(date, options) fname = os.path.normpath(os.path.join(os.getcwd(), fileroot, name)) else: # make sure provided filename is unique if self.args["filename"].endswith(".csv"): name = self.args["filename"][:-4] else: name = self.args["filename"] new_name = name + ".csv" fname = os.path.normpath(os.path.join( os.getcwd(), fileroot, new_name)) i = 0 while os.path.isfile(fname): i = i + 1 new_name = name + "_" + str(i) + ".csv" fname = os.path.normpath(os.path.join( os.getcwd(), fileroot, new_name)) return fname def setup_logger(self): # Create filename from options and date self.log_file = self.get_filename() # Create directory if not there Path(self.args["fileroot"]).mkdir(exist_ok=True) print("Log location: {}".format(self.log_file)) # Logger setup logconfig = self.args["logconfig"] self.flogger = FileLogger(self._cf, logconfig, self.log_file) self.flogger.enableAllConfigs() def setup_optitrack(self): self.ot_id = self.args["optitrack_id"] self.ot_position = np.zeros(3) self.ot_attitude = np.zeros(3) self.ot_quaternion = np.zeros(4) self.filtered_pos = np.zeros(3) self.ot_filter_sos = scipy.signal.butter(N=4, Wn=0.1, btype='low', analog=False, output='sos') self.pos_filter_zi = [scipy.signal.sosfilt_zi(self.ot_filter_sos), scipy.signal.sosfilt_zi(self.ot_filter_sos), scipy.signal.sosfilt_zi(self.ot_filter_sos)] # Streaming client in separate thread streaming_client = NatNetClient() streaming_client.newFrameListener = self.ot_receive_new_frame streaming_client.rigidBodyListener = self.ot_receive_rigidbody_frame streaming_client.run() self.optitrack_enabled = True print("OptiTrack streaming client started") # TODO: do we need to return StreamingClient? def reset_estimator(self): # Kalman if self.args["estimator"] == "kalman": self._cf.param.set_value("kalman.resetEstimation", "1") time.sleep(1) self._cf.param.set_value("kalman.resetEstimation", "0") # Complementary (needs changes to firmware) if self.args["estimator"] == "complementary": try: self._cf.param.set_value("complementaryFilter.reset", "1") time.sleep(1) self._cf.param.set_value("complementaryFilter.reset", "0") except: pass def ot_receive_new_frame(self, *args, **kwargs): pass def ot_receive_rigidbody_frame(self, id, position, rotation): # Check ID if id in self.ot_id: # get optitrack data in crazyflie global frame pos_in_cf_frame = util.ot2control(position) att_in_cf_frame = util.quat2euler(rotation) quat_in_cf_frame = util.ot2control_quat(rotation) idx = self.ot_id.index(id) if idx==0: # main drone ot_dict = { "otX0": pos_in_cf_frame[0], "otY0": pos_in_cf_frame[1], "otZ0": pos_in_cf_frame[2], "otRoll0": att_in_cf_frame[0], "otPitch0": att_in_cf_frame[1], "otYaw0": att_in_cf_frame[2] } self.ot_position = pos_in_cf_frame self.ot_attitude = att_in_cf_frame self.ot_quaternion = quat_in_cf_frame self.flogger.registerData("ot0", ot_dict) (self.filtered_pos[0], self.pos_filter_zi[0]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[0]], zi=self.pos_filter_zi[0] ) (self.filtered_pos[1], self.pos_filter_zi[1]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[1]], zi=self.pos_filter_zi[1] ) (self.filtered_pos[2], self.pos_filter_zi[2]) = scipy.signal.sosfilt( self.ot_filter_sos, [self.ot_position[2]], zi=self.pos_filter_zi[2] ) elif idx==1: ot_dict = { "otX1": pos_in_cf_frame[0], "otY1": pos_in_cf_frame[1], "otZ1": pos_in_cf_frame[2], "otRoll1": att_in_cf_frame[0], "otPitch1": att_in_cf_frame[1], "otYaw1": att_in_cf_frame[2] } self.flogger.registerData("ot1", ot_dict) def do_taskdump(self): self._cf.param.set_value("system.taskDump", "1") def process_taskdump(self, console_log): file = self.get_filename() # Dataframe placeholders label_data, load_data, stack_data = [], [], [] # Get headers headers = [] for i, line in enumerate(console_log): if "Task dump" in line: headers.append(i) # None indicates the end of the list headers.append(None) # Get one task dump for i in range(len(headers) - 1): dump = console_log[headers[i] + 2 : headers[i + 1]] # Process strings: strip \n, \t, spaces, SYSLOAD: loads, stacks, labels = [], [], [] for line in dump: entries = line.strip("SYSLOAD: ").split("\t") loads.append(entries[0].strip()) # no sep means strip all space, \n, \t stacks.append(entries[1].strip()) labels.append(entries[2].strip()) # Store labels if not label_data: label_data = labels # Append to placeholders load_data.append(loads) stack_data.append(stacks) # Check if we have data at all if headers[0] is not None and label_data: # Put in dataframe load_data = pd.DataFrame(load_data, columns=label_data) stack_data = pd.DataFrame(stack_data, columns=label_data) # Save dataframes load_data.to_csv(file.strip(".csv") + "+load.csv", sep=",", index=False) stack_data.to_csv(file.strip(".csv") + "+stackleft.csv", sep=",", index=False) else: print("No task dump data found") def controller_connected(self): """ Return True if a controller is connected """ return len(self._jr.available_devices()) > 0 def setup_controller(self, map="PS3_Mode_1"): devs = [] for d in self._jr.available_devices(): devs.append(d.name) if len(devs)==1: input_device = 0 else: print("Multiple controllers detected:") for i, dev in enumerate(devs): print(" - Controller #{}: {}".format(i, dev)) input_device = int(input("Select controller: ")) if not input_device in range(len(devs)): raise ValueError self._jr.start_input(devs[input_device]) self._jr.set_input_map(devs[input_device], map) def connect_crazyflie(self, uri): """Connect to a Crazyflie on the given link uri""" # Connect some callbacks from the Crazyflie API self._cf.connected.add_callback(self._connected) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.connection_lost.add_callback(self._connection_lost) if self.mode == Mode.AUTO or self.mode == Mode.DONT_FLY: self._cf.open_link(uri) else: # Add callbacks for manual control self._cf.param.add_update_callback( group="imu_sensors", name="AK8963", 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(uri) self._jr.input_updated.add_callback(self.controller_input_cb) if self.mode == Mode.MODE_SWITCH: self._jr.alt1_updated.add_callback(self.mode_switch_cb) def _connected(self, link): """This callback is called form the Crazyflie API when a Crazyflie has been connected and the TOCs have been downloaded.""" print("Connected to %s" % link) # set estimator if args["estimator"]=="kalman": self._cf.param.set_value("stabilizer.estimator", "2") self.flogger.start() print("logging started") def _connection_failed(self, link_uri, msg): print("Connection to %s failed: %s" % (link_uri, msg)) self.flogger.is_connected = False def _connection_lost(self, link_uri, msg): print("Connection to %s lost: %s" % (link_uri, msg)) self.flogger.is_connected = False def _disconnected(self, link_uri): print("Disconnected from %s" % link_uri) self.flogger.is_connected = False def ready_to_fly(self): # Wait for connection timeout = 10 while not self._cf.is_connected(): print("Waiting for Crazyflie connection...") time.sleep(2) timeout -= 1 if timeout<=0: return False # Wait for optitrack if self.optitrack_enabled: while (self.ot_position == 0).any(): print("Waiting for OptiTrack fix...") time.sleep(2) timeout -= 1 if timeout <= 0: return False print("OptiTrack fix acquired") print("Reset Estimator...") self.reset_estimator() time.sleep(2) # wait for kalman to stabilize return True def start_flight(self): if self.ready_to_fly(): if self.mode == Mode.MANUAL: print("Manual Flight - Ready to fly") self.manual_flight() elif self.mode == Mode.DONT_FLY: print("Ready to not fly") try: while True: pass except KeyboardInterrupt: print("Flight stopped") else: # Build trajectory setpoints = self.build_trajectory(self.args["trajectory"], self.args["space"]) # Do flight if self.mode == Mode.AUTO: print("Autonomous Flight - Starting flight") self.follow_setpoints(self._cf, setpoints, self.args["optitrack"]) print("Flight complete.") else: print("Ready to fly") self.manual_flight() print("Starting Trajectory") self.follow_setpoints(self._cf, setpoints, self.args["optitrack"]) else: print("Timeout while waiting for flight ready.") def controller_input_cb(self, *data): # only forward control in manual mode if self.is_in_manual_control: self._cf.commander.send_setpoint(*data) def mode_switch_cb(self, auto_mode): if auto_mode: print("Switching autonomous flight") self.is_in_manual_control = False else: print("Switching to manual control") self.is_in_manual_control = True def manual_flight(self): self.is_in_manual_control = True while(self.is_in_manual_control): if self.args["optitrack"]=="state": # self._cf.extpos.send_extpos( # self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2] # ) self._cf.extpos.send_extpos( self.ot_position[0], self.ot_position[1], self.ot_position[2] ) # self._cf.extpos.send_extpose( # self.ot_position[0], self.ot_position[1], self.ot_position[2], # self.ot_quaternion[0], self.ot_quaternion[1], self.ot_quaternion[2], self.ot_quaternion[3] # ) time.sleep(0.01) def build_trajectory(self, trajectories, space): # Load yaml file with space specification with open(space, "r") as f: space = yaml.full_load(f) home = space["home"] ranges = space["range"] # Account for height offset altitude = home["z"] + ranges["z"] side_length = min([ranges["x"], ranges["y"]]) * 2 radius = min([ranges["x"], ranges["y"]]) x_bound = [home["x"] - ranges["x"], home["x"] + ranges["x"]] y_bound = [home["y"] - ranges["y"], home["y"] + ranges["y"]] # Build trajectory # Takeoff setpoints = takeoff(home["x"], home["y"], altitude, 0.0) for trajectory in trajectories: # If nothing, only nothing if trajectory == "nothing": setpoints = None return setpoints elif trajectory == "hover": setpoints += hover(home["x"], home["y"], altitude) elif trajectory == "hover_fw": setpoints += hover_fw(home["x"], home["y"], altitude) elif trajectory == "square": setpoints += square(home["x"], home["y"], side_length, altitude) elif trajectory == "square_fw": setpoints += square_fw(home["x"], home["y"], side_length, altitude) elif trajectory == "octagon": setpoints += octagon(home["x"], home["y"], radius, altitude) elif trajectory == "triangle": setpoints += triangle(home["x"], home["y"], radius, altitude) elif trajectory == "hourglass": setpoints += hourglass(home["x"], home["y"], side_length, altitude) elif trajectory == "random": setpoints += randoms(home["x"], home["y"], x_bound, y_bound, altitude) elif trajectory == "scan": setpoints += scan(home["x"], home["y"], x_bound, y_bound, altitude) else: raise ValueError("{} is an unknown trajectory".format(trajectory)) # Add landing setpoints += landing(home["x"], home["y"], altitude, 0.0) return setpoints def follow_setpoints(self, cf, setpoints, optitrack): # Counter for task dump logging time_since_dump = 0.0 # Start try: print("Flight started") # Do nothing, just sit on the ground if setpoints is None: while True: time.sleep(0.05) time_since_dump += 0.05 # Task dump if time_since_dump > 2: print("Do task dump") self.do_taskdump() time_since_dump = 0.0 # Do actual flight else: for i, point in enumerate(setpoints): print("Next setpoint: {}".format(point)) # Compute time based on distance # Take-off if i == 0: distance = point[2] # No take-off else: distance = np.sqrt( ((np.array(point[:3]) - np.array(setpoints[i - 1][:3])) ** 2).sum() ) # If zero distance, at least some wait time if distance == 0.0: wait = 5 else: wait = distance * 2 # Send position and wait time_passed = 0.0 while time_passed < wait: if self.is_in_manual_control: self.manual_flight() # If we use OptiTrack for control, send position to Crazyflie if optitrack == "state": cf.extpos.send_extpos( self.filtered_pos[0], self.filtered_pos[1], self.filtered_pos[2] ) cf.commander.send_position_setpoint(*point) time.sleep(0.05) time_passed += 0.05 time_since_dump += 0.05 # Task dump # if time_since_dump > 2: # print("Do task dump") # self.do_taskdump() # time_since_dump = 0.0 # Finished cf.commander.send_stop_setpoint() # Prematurely break off flight / quit doing nothing except KeyboardInterrupt: if setpoints is None: print("Quit doing nothing!") else: print("Emergency landing!") wait = setpoints[i][2] * 2 cf.commander.send_position_setpoint(setpoints[i][0], setpoints[i][1], 0.0, 0.0) time.sleep(wait) cf.commander.send_stop_setpoint() def setup_console_dump(self): # Console dump file self.console_log = [] console = Console(self._cf) console.receivedChar.add_callback(self._console_cb) self.console_dump_enabled = True def _console_cb(self, text): # print(text) self.console_log.append(text) def end(self): self._cf.close_link() # Process task dumps # TODO: add timestamps / ticks (like logging) to this if self.console_dump_enabled: self.process_taskdump(self.console_log)
class Teleop(): """Crazyflie Joystick Controller""" def __init__(self): """Initialize the joystick reader""" self._althold = False self._jr = JoystickReader(do_device_discovery=False) self._jr.device_error.add_callback(self._input_dev_error) self._jr.althold_updated.add_callback(self._althold_updated) self._jr.input_updated.add_callback(self._input_updated) self._command_pub = rospy.Publisher('cfjoy', CFJoyMsg) def start_controller(self, input_config, input_device=0): """Set up the device reader""" # Frequency of joystick messages is handled by joystick driver # TODO: make that configurable or write handler for ROS joystick # Frequency of sending to flie is handled by crazyflie_node #TODO: connect to a config message from the flie? self._jr.setAltHoldAvailable(False) devs = self._jr.getAvailableDevices() if len(devs) < 1: raise ControllerNotFound("Device count: %d" % len(devs)) dev_name = devs[input_device]["name"] rospy.loginfo("Using [%s] for input with [%s]", dev_name, input_config) self._jr.start_input(dev_name, input_config) def controller_connected(self): """ Return True if a controller is connected""" return True if (len(self._jr.getAvailableDevices()) > 0) else False def list_controllers(self): """List the available controllers""" for dev in self._jr.getAvailableDevices(): print("Controller #%s: %s" % (dev["id"], dev["name"])) def _althold_updated(self, enabled): #TODO: enabled is a string! Stop that! self._althold = (enabled == 'True') rospy.logdebug("Althold set: enabled=%i", self._althold) def _input_updated(self, roll, pitch, yaw, thrust): #Sent from joystickreader: #self.input_updated.call(trimmed_roll, trimmed_pitch, yaw, thrust) msg = self._create_joy_msg(roll, pitch, yaw, thrust, self._althold); self._command_pub.publish(msg); rospy.logdebug("Updated: r=%.4f, p=%.4f, y=%.4f, t=%.4f, h=%i", roll, pitch, yaw, thrust, self._althold) @staticmethod def _input_dev_error(message): """Callback for an input device error""" rospy.logerr("Error when reading device: %s", message) rospy.signal_shutdown("Error when reading device: %s" % message) sys.exit(-1) @staticmethod def _create_joy_msg(roll, pitch, yaw, thrust, althold): h = std_msgs.msg.Header() h.stamp = rospy.Time.now() return CFJoyMsg(h, roll, pitch, yaw, thrust, althold)
class CrazyflieController(): def __init__(self, link_uri, radio_port, serialport, xmode=True): """Initialize the headless client and libraries""" from cfclient.utils.input import JoystickReader self._cf = Crazyflie() self._serial = serialport self._cf.commander.set_client_xmode(xmode) self._cf.open_link(link_uri) self._port = radio_port self.is_connected = True self.running = True self._console = "" # Callbacks self._cf.connected.add_callback( lambda msg: print("Connected to " + msg)) self._cf.disconnected.add_callback(self._disconnected) self._cf.connection_failed.add_callback(self._connection_failed) self._cf.console.receivedChar.add_callback(self._console_char) self._cf.packet_received.add_callback(self._data_received) # Search for controllers self._jr = JoystickReader(do_device_discovery=False) available_devices = self._jr.available_devices() if len(available_devices): used_device = available_devices[0] print("Using input device: " + str(used_device)) self._jr.device_error.add_callback( lambda msg: print("Input device error: " + str(msg))) self._jr.start_input(used_device.name) self._jr.set_input_map(used_device.name, "xbox360_mode1") # Controller callback self._jr.input_updated.add_callback( self._cf.commander.send_setpoint) else: print("No input device detected") self.serialthread = threading.Thread(target=self._serial_listener) self.serialthread.start() def stop(self): self.running = False self.serialthread.join() self._cf.close_link() def _console_char(self, pk): self._console = self._console + str(pk) if "\n" in self._console: print("Console: " + str(self._console), end="") self._console = "" def _disconnected(self, link_uri): print("Disconnected...") if self.running: self.stop() def _connection_failed(self, link_uri, msg): print("Conection failed...") if self.running: self.stop() def _serial_listener(self): while self.running and self.is_connected: data = self._serial.read_until(size=30) if (data): pk = CRTPPacket() pk.port = self._port pk.data = data pk.size = len(data) self._cf.send_packet(pk) #print("Got data from agent: len {}, data {}".format(pk.size, pk.data)) def _data_received(self, pk): if pk and pk.port == self._port: self._serial.write(pk.data) self._serial.flush()