Example #1
0
def init():
    logging.basicConfig(level=logging.WARNING)
    # init drivers
    init_drivers(enable_debug_driver=False)

    # intit & start Controllerinput-Thread
    GamepadController()
Example #2
0
def init():
    logging.basicConfig(level=logging.WARNING)
    # init drivers
    init_drivers(enable_debug_driver=False)
    
    # intit & start Controllerinput-Thread
    GamepadController()
Example #3
0
    def __init__(self, ble=False, sim_vel=np.array([0., 0., 0.])):
        print("[CrazyFlie] Attaching CrazyFlie Plug with Keyboard handle")

        if ble:
            link = BLEDriver()
            inter = link.scan_interface()[0][0]
            # inter = "e0:c3:b3:86:a6:13"
            link.connect(inter)
            self._cf = Crazyflie(link=link, rw_cache="./")
            self.rate = 0.05
            print("[CrazyFlie] Connected to Crazyflie on bluetooth {}.".format(
                inter))
        else:
            crtp.init_drivers()
            self.inter = crtp.scan_interfaces()
            self._cf = Crazyflie(rw_cache="./")
            self._cf.open_link(self.inter[0][0])
            time.sleep(1)  # wait for a while to let crazyflie fetch the TOC
            self.rate = 0.01
            print("[CrazyFlie] Connected to Crazyflie on radio {}.".format(
                self.inter))

        self.roll = 0
        self.pitch = 0
        self.yaw = 0
        self.hover_thrust = 36330
        self.tilt_angle = 10

        self.x_dot = 0.0
        self.y_dot = 0.0
        self.z_dot = 0.0
        self.yaw_rate = 0.0
        self.step = 0.1
        self.max_xyz_speed = 0.4
        self.max_yaw_rate = 90.

        self.x, self.y, self.z = 0, 0, 0.5

        self.running = True

        self.roll_calib_offset = 0
        self.pitch_calib_offset = 0

        signal.signal(signal.SIGINT, self.interrupt_handle)
        # tty.setcbreak(sys.stdin)
        self.keyboard_handle = Listener(on_press=self.on_press,
                                        on_release=self.on_release)
        self.keyboard_handle.start()

        # unlocking thrust protection
        self._cf.commander.send_setpoint(0, 0, 0, 0)
        self.sim_vel = sim_vel

        # if self.sim_vel is not None:
        self.hover_thread_flag = True
        self.hover_thread = threading.Thread(target=self.hover_threaded)
Example #4
0
def main():
    if not start_only_voice_control:
        uri = args['uri']
        logging.basicConfig()
        crtp.init_drivers(enable_debug_driver=False)

        # the command queue for the crazyflie
        crazyflieCommandQueue = Queue()

        # set up the crazyflie
        cf = crazyflie.Crazyflie(rw_cache='./cache')
        control = ControllerThread(cf, crazyflieCommandQueue)
        control.start()

        # start the web interface to the crazyflie
        server = Process(target=run_server,
                         args=("0.0.0.0", control_port, crazyflieCommandQueue))
        server.start()

        # start the path planning server
        pathPlanner = Process(target=run_path_planner,
                              args=("0.0.0.0", planning_port,
                                    crazyflieCommandQueue, room_config))
        pathPlanner.start()

        # connect to the crazyflie
        if uri is None:
            print('Scanning for Crazyflies...')
            available = crtp.scan_interfaces()
            if available:
                print('Found Crazyflies:')
                for i in available:
                    print('-', i[0])
                uri = available[0][0]
            else:
                print('No Crazyflies found!')
                sys.exit(1)

        print('Connecting to', uri)
        cf.open_link(uri)

        handle_keyboard_input(control, server)

        cf.close_link()

    if start_voice_control or start_only_voice_control:
        start_command_loop(f'{control_url}:{control_port}', voice_api)
Example #5
0
    def run(self):
        self.cf = crazyflie.Crazyflie(ro_cache='./cache/ro/', rw_cache='./cache/rw/')
        crtp.init_drivers()
        available = crtp.scan_interfaces()
        for i in available:
            print "Connecting"
            self.cf.connectSetupFinished.add_callback(self.connected)
            self.cf.open_link(i[0])
            break

        while (self.values.run):
            sys.stdout.write("\rThrust: %d, Roll: %d, Pitch: %d, Yaw: %d            " % (self.values.thrust, self.values.roll, self.values.pitch, self.values.yaw))
            sys.stdout.flush();
            r = self.values.roll + self.base_roll
            p = self.values.pitch + self.base_pitch
            y = self.values.yaw + self.base_yaw
            t = self.values.thrust
            self.cf.commander.send_setpoint(r, p, y, t)
	def connect_to_cf(self, retry_after=10, max_timeout=20):
		"""
		Initializes radio drivers, looks for available CrazyRadios, and attempts to connect to the first available one.
		Doesn't return anything, but raises exceptions if anything goes wrong.
		:param retry_after: Time in seconds after which we should abort current connection and restart the attempt.
		:param max_timeout: Time in seconds after which we should give up if we haven't been able to establish comm. yet
		"""
		logging.info("Initializing drivers.")
		crtp.init_drivers(enable_debug_driver=False)

		logging.info("Setting up the communication link. Looking for available CrazyRadios in range.")
		available_links = crtp.scan_interfaces()
		if len(available_links) == 0:
			raise Exception("Error, no Crazyflies found. Exiting.")
		else:
			logging.info("CrazyFlies found:")  # For debugging purposes, show info about available links
			for i in available_links:
				logging.info("\t" + i[0])
			link_uri = available_links[0][0]  # Choose first available link

		logging.info("Initializing CrazyFlie (connecting to first available interface: '{}').".format(link_uri))
		self.crazyflie = Crazyflie(ro_cache="cachero", rw_cache="cacherw")  # Create an instance of Crazyflie
		self.crazyflie.connected.add_callback(self.on_cf_radio_connected)  # Set up callback functions for communication feedback
		self.crazyflie.disconnected.add_callback(self.on_cf_radio_disconnected)
		self.crazyflie.connection_failed.add_callback(self.on_cf_radio_conn_failed)
		self.crazyflie.connection_lost.add_callback(self.on_cf_radio_conn_lost)

		cnt = 0  # Initialize a time counter
		while self.cf_radio_connecting and cnt < max_timeout:
			if cnt % retry_after == 0:
				if cnt > 0:  # Only show warning after first failed attempt
					logging.warning("Unable to establish communication with Crazyflie ({}) after {}s. Retrying...".format(link_uri, retry_after))
					self.crazyflie.close_link()  # Closing the link will call on_disconnect, which will set cf_radio_connecting to False
					self.cf_radio_connecting = True  # Reset cf_radio_connecting back to True
				self.crazyflie.open_link(link_uri)  # Connect/Reconnect to the CrazyRadio through the selected interface
			time.sleep(1)  # Sleep for a second (give time for callback functions to detect whether we are connected)
			cnt += 1  # Increase the "waiting seconds" counter

		if cnt >= max_timeout:
			self.crazyflie.close_link()
			raise Exception("Unable to establish communication with CrazyFlie after {}s. Given up :(".format(max_timeout))
		elif not self.cf_radio_connected:
			raise Exception("Something failed while attempting to connect to the CrazyFlie, exiting.")
    while True:

        sys.stdout.write("\r" + str(leap_controller.read_input()))
        sys.stdout.flush()

    try:
        sys.stdin.readline()
    except KeyboardInterrupt:
        pass
    finally:
        pass
        # controller.remove_listener(listener)

if __name__ == '__main__':

    crtp.init_drivers(enable_debug_driver=False)
    available = crtp.scan_interfaces()

    for i in available:
        print(i[0])

    if len(available) > 0:
        print("available", available[0][0])
        leap_controller = LeapController(available[0][0])
    else:
        print('No Crazyflies found, cannot run example')



Example #8
0
 def initializeDrivers(self):
     '''
     Initialize the low-level drivers (don't list the debug drivers)
     '''
     radioChannel.init_drivers(enable_debug_driver=False)
Example #9
0
        self.show()

    def SetTableItemText(self, row, col, text):
        self._table.item(row, col).setText(text)


if __name__ == '__main__':
    logging.basicConfig(
        level=logging.INFO,
        format='%(asctime)s %(levelname).1s %(module)-12.12s %(message)s')
    #filename='log.txt')

    app = QtGui.QApplication(sys.argv)
    window = CfMonitorWindow()

    crtp.init_drivers()

    cfmonitors = []
    for i, uri in enumerate(LINK_URIS):
        cfmonitors.append(CfMonitor(i, uri, window))

    video_controller = video_pid_controller.VideoPIDController(cfmonitors[0])
    pressure_thrust_controller = (
        pressure_thrust_controller.PressureThrustController(cfmonitors[0]))
    compass_yaw_controller = (compass_yaw_controller.CompassYawController(
        cfmonitors[0]))

    def SetButtonPressed():
        compass_yaw_controller.SetTarget()

    joy_controller = joystick_controller.JoystickController(
Example #10
0
            control.enable()
        elif ch == 'q':
            if not control.enabled:
                print('Uppercase Q quits the program')
            control.disable()
        elif ch == 'Q':
            control.disable()
            print('Bye!')
            break
        else:
            print('Unhandled key', ch, 'was pressed')


if __name__ == "__main__":
    logging.basicConfig()
    crtp.init_drivers(enable_debug_driver=False)
    cf = crazyflie.Crazyflie(rw_cache='./cache')
    control = ControllerThread(cf)
    control.start()

    if URI is None:
        print('Scanning for Crazyflies...')
        available = crtp.scan_interfaces()
        if available:
            print('Found Crazyflies:')
            for i in available:
                print('-', i[0])
            URI = available[0][0]
        else:
            print('No Crazyflies found!')
            sys.exit(1)
Example #11
0
    def __init__(self):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo()
        self.ui.labelTest = {"MS5611": self.ui.label_baroTest, "MPU6050": self.ui.label_MPUTest, "HMC5883L": self.ui.label_magTest}

        self.state = STATE.DISCONNECTED


        # ROS
        self.ros = ROSNode()



        # FLIE
        self.flie = FlieControl()
        self.ui.checkBox_pktHZ.toggled.connect(lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.value() if on else 0 ))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(self.ui.spinBox_pktHZ.value()) # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode( self.ui.checkBox_xmode.isChecked())


        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)





        # init previous settings
        self.readSettings()



        # Updateing the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000/self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(lambda x: self.guiUpdateTimer.setInterval(1000/x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(lambda x: self.ros.sig_joydata.connect(self.setInputJoy) if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)





        # Set up URI scanner
        self.scanner = ScannerThread()
        self.scanner.start()


        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=False)) # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)


        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(lambda found: self.ui.label_baroFound.setText("Yes" if found else "No"))
        self.paramManager.sig_magFound.connect(lambda found: self.ui.label_magFound.setText("Yes" if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect( lambda v : self.setToUpdate("vbat",self.ui.progressbar_bat.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn,hz))
        self.flie.outKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut,hz))


        # Connections GUI to GUI


        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)

        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()
Example #12
0
    def __init__(self, options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {
            "MS5611": self.ui.label_baroTest,
            "MPU6050": self.ui.label_MPUTest,
            "HMC5883L": self.ui.label_magTest
        }

        self.state = STATE.DISCONNECTED

        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)

        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(
            lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.
                                                      value() if on else 0))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(
            self.ui.spinBox_pktHZ.value())  # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(
            self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode(
            self.ui.checkBox_xmode.isChecked())

        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(
            self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda: self.connectPressed(
            self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf,
                                         self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)

        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(
            self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)

        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw * 10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(
            lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw / 10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(
            self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(
            lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.
                                                   value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(
            self.ui.checkBox_yaw.checkState())  # force update
        self.ui.pushButton_north.clicked.connect(
            lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw(
            )))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(
            self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(
            lambda x: self.logManager.setGroundLevel(
                self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(
            self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(
            lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.
                                                          getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)

        # init previous settings
        self.readSettings()

        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(
            lambda x: self.ui.label_up.setText(str(round(x, 2)) + "ms"))
        self.sync.sig_delayDown.connect(
            lambda x: self.ui.label_down.setText(str(round(x, 2)) + "ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(
            self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(
            lambda x: self.ui.label_cputime.setText("%08.03fs" % x))
        self.sync.sig_flieTime.connect(
            lambda x: self.ui.label_flietime.setText("%08.03fs" % x))
        self.sync.sig_diffTime.connect(
            lambda x: self.ui.label_difftime.setText("%08.03fs" % x))

        self.ui.checkBox_rosLog.stateChanged.connect(
            self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(
            lambda x: self.logManager.setPubToRos(
                min(self.ui.groupBox_ros.isChecked(),
                    self.ui.checkBox_rosLog.checkState())))  #

        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000 / self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(
            lambda x: self.guiUpdateTimer.setInterval(1000 / x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(
            lambda x: self.ros.sig_joydata.connect(self.setInputJoy)
            if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)

        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()

        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(
            lambda: self.connectPressed(self.ui.comboBox_connect.currentText(),
                                        auto=False))  # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(
            self.ui.checkBox_kill.checkState())  # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(
            self.ui.checkBox_hover.checkState())  # force update

        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(
            lambda x: self.updateBaroTopics(not x))

        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(
            lambda found: self.ui.label_baroFound.setText("Yes"
                                                          if found else "No"))
        self.paramManager.sig_magFound.connect(
            lambda found: self.ui.label_magFound.setText("Yes"
                                                         if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[
            str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(
            lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect(lambda v: self.setToUpdate(
            "vbat", self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect(lambda v: self.setToUpdate(
            "cpu", self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn, hz))
        self.flie.outKBPS.sig_KBPS.connect(
            lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut, hz))

        # Connections GUI to GUI

        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)

        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()
Example #13
0
    self.show()

  def SetTableItemText(self, row, col, text):
    self._table.item(row, col).setText(text)


if __name__ == '__main__':
  logging.basicConfig(
      level=logging.INFO,
      format='%(asctime)s %(levelname).1s %(module)-12.12s %(message)s')
      #filename='log.txt')
  
  app = QtGui.QApplication(sys.argv)
  window = CfMonitorWindow()

  crtp.init_drivers()

  cfmonitors = []
  for i, uri in enumerate(LINK_URIS):
    cfmonitors.append(CfMonitor(i, uri, window))

  video_controller = video_pid_controller.VideoPIDController(cfmonitors[0])
  pressure_thrust_controller = (
      pressure_thrust_controller.PressureThrustController(cfmonitors[0]))
  compass_yaw_controller = (
      compass_yaw_controller.CompassYawController(cfmonitors[0]))

  def SetButtonPressed():
    compass_yaw_controller.SetTarget()
  joy_controller = joystick_controller.JoystickController(cfmonitors[0],
                                                          SetButtonPressed)
Example #14
0
    def __init__(self,options):
        super(DriverWindow, self).__init__()
        #uic.loadUi('ui/driverGUI.ui', self)
        self.options = options
        self.ui = Ui_MainWindow()
        self.ui.setupUi(self)
        self.ui.tabWidget.setCurrentIndex(0)
        self.resetInfo(initial=True)
        self.ui.labelTest = {"MS5611": self.ui.label_baroTest, "MPU6050": self.ui.label_MPUTest, "HMC5883L": self.ui.label_magTest}

        self.state = STATE.DISCONNECTED


        # ROS
        self.d = Dialog(options)
        self.ros = ROSNode(options, self)


        # FLIE
        self.flie = FlieControl(self)
        self.ui.checkBox_pktHZ.toggled.connect(lambda on: self.flie.setPacketUpdateSpeed(self.ui.spinBox_pktHZ.value() if on else 0 ))
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.inKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.connect(self.flie.outKBPS.setHZ)
        self.ui.spinBox_pktHZ.valueChanged.emit(self.ui.spinBox_pktHZ.value()) # force update
        self.ui.checkBox_kill.toggled.connect(self.flie.setKillswitch)
        self.flie.setKillswitch(self.ui.checkBox_kill.isChecked())
        self.ui.checkBox_xmode.toggled.connect(self.flie.crazyflie.commander.set_client_xmode)
        self.flie.crazyflie.commander.set_client_xmode( self.ui.checkBox_xmode.isChecked())


        # Set up ParamManager
        self.paramManager = ParamManager(self.flie.crazyflie, self)
        self.ui.tab_param.layout().addWidget(self.paramManager)

        # Set up LogManager
        self.logManager = LogManager(self.flie.crazyflie, self)
        self.ui.tab_log.layout().addWidget(self.logManager)
        self.ui.checkBox_logHZ.toggled.connect(self.logManager.setEstimateHzOn)
        self.ui.spinBox_logHZ.valueChanged.connect(self.logManager.setFreqMonitorFreq)
        self.logManager.sig_rosData.connect(self.ros.receiveCrazyflieLog)

        self.autoRetryTimer = QTimer()
        self.autoRetryTimer.setInterval(1500)
        self.autoRetryTimer.timeout.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=True))
        self.autoRetryTimer.setSingleShot(True)

        # Set up TrackManager
        self.trackManager = TrackManager(self.ros.sub_tf, self.ros.pub_tf, self)
        self.ui.tab_tracking.layout().addWidget(self.trackManager)


        # AI
        self.ai = AttitudeIndicator(self.ui.tab_hud)
        self.logManager.sig_rpy.connect(self.ai.setRollPitchYaw)
        self.ui.tab_hud.layout().addWidget(self.ai)
        self.ui.checkBox_AI.stateChanged.connect(self.ai.setUpdatesEnabled)
        self.ui.spinBox_AIHZ.valueChanged.connect(self.ai.setUpdateSpeed)
        self.logManager.sig_hoverTarget.connect(self.ai.setHover)
        self.logManager.sig_baroASL.connect(self.ai.setBaro)
        self.logManager.sig_accZ.connect(self.ai.setAccZ)
        self.logManager.sig_motors.connect(self.ai.setMotors)
        self.logManager.sig_batteryUpdated.connect(self.ai.setBattery)
        self.logManager.sig_batteryState.connect(self.ai.setPower)
        self.logManager.sig_temp.connect(self.ai.setTemp)
        self.logManager.sig_cpuUpdated.connect(self.ai.setCPU)
        self.logManager.sig_pressure.connect(self.ai.setPressure)
        self.logManager.sig_aslLong.connect(self.ai.setAslLong)
        self.paramManager.sig_gyroCalib.connect(self.ai.setCalib)
        self.flie.inKBPS.sig_KBPS.connect(self.ai.setPktsIn)
        self.flie.outKBPS.sig_KBPS.connect(self.ai.setPktsOut)
        self.flie.sig_flieLink.connect(self.ai.setLinkQuality)
        self.flie.sig_stateUpdate.connect(self.ai.setFlieState)
        self.ui.checkBox_reconnect.stateChanged.connect(self.ai.setAutoReconnect)
        self.logManager.sig_hzMeasure.connect(self.ai.updateHz)
        self.logManager.sig_logStatus.connect(self.ai.updateHzTarget)


        # Yaw offset
        self.ui.doubleSpinBox_yaw.valueChanged.connect(lambda yaw: self.ui.horizontalSlider_yaw.setValue(yaw*10))
        self.ui.horizontalSlider_yaw.valueChanged.connect(lambda yaw: self.ui.doubleSpinBox_yaw.setValue(yaw/10))
        self.ui.doubleSpinBox_yaw.valueChanged.connect(self.logManager.setYawOffset)
        self.ui.checkBox_yaw.stateChanged.connect(lambda x: self.logManager.setYawOffset(self.ui.doubleSpinBox_yaw.value() if x else 0))
        self.ui.checkBox_yaw.stateChanged.emit(self.ui.checkBox_yaw.checkState()) # force update
        self.ui.pushButton_north.clicked.connect(lambda: self.ui.doubleSpinBox_yaw.setValue(self.logManager.getYaw()))

        # ASL offset
        self.ui.doubleSpinBox_ground.valueChanged.connect(self.logManager.setGroundLevel)
        self.ui.checkBox_ground.stateChanged.connect(lambda x: self.logManager.setGroundLevel(self.ui.doubleSpinBox_ground.value() if x else 0))
        self.ui.checkBox_ground.stateChanged.emit(self.ui.checkBox_ground.checkState())
        self.ui.pushButton_ground.clicked.connect(lambda: self.ui.doubleSpinBox_ground.setValue(self.logManager.getASL()))
        self.ros.sig_baro.connect(self.logManager.setAslOffset)




        # init previous settings
        self.readSettings()


        # Sync
        self.sync = Sync(self.flie.crazyflie)
        self.sync.sig_delayUp.connect(lambda  x: self.ui.label_up.setText(str(round(x,2))+"ms"))
        self.sync.sig_delayDown.connect(lambda  x: self.ui.label_down.setText(str(round(x,2))+"ms"))

        self.ui.groupBox_sync.toggled.connect(self.sync.enable)
        self.ui.spinBox_syncHz.valueChanged.connect(self.sync.setSyncRate)
        self.ui.spinBox_syncHz.valueChanged.emit(self.ui.spinBox_syncHz.value())
        self.ui.groupBox_sync.toggled.emit(self.ui.groupBox_sync.isChecked())
        self.sync.sig_cpuTime.connect(lambda  x: self.ui.label_cputime.setText("%08.03fs"%x))
        self.sync.sig_flieTime.connect(lambda  x: self.ui.label_flietime.setText("%08.03fs"%x))
        self.sync.sig_diffTime.connect(lambda  x: self.ui.label_difftime.setText("%08.03fs"%x))


        self.ui.checkBox_rosLog.stateChanged.connect(self.logManager.setPubToRos)
        self.ui.groupBox_ros.clicked.connect(lambda x: self.logManager.setPubToRos(min(self.ui.groupBox_ros.isChecked(), self.ui.checkBox_rosLog.checkState()))) #




        # Updating the GUI (if we didnt do this, every change would result in an update...)
        self.guiUpdateQueue = {}
        self.guiUpdateQueueSave = 0
        self.guiUpdateTimer = QTimer()
        self.guiUpdateTimer.setInterval(1000/self.ui.spinBox_guiHZ.value())
        self.ui.spinBox_guiHZ.valueChanged.connect(lambda x: self.guiUpdateTimer.setInterval(1000/x))
        self.guiUpdateTimer.timeout.connect(self.updateGui)
        self.guiUpdateTimer.start()

        # Defaults according to settings within GUI
        self.beepOn = self.ui.checkBox_beep.isChecked()
        self.killOn = self.ui.checkBox_kill.isChecked()
        self.autoReconnectOn = self.ui.checkBox_reconnect.isChecked()
        self.startupConnectOn = self.ui.checkBox_startupConnect.isChecked()
        self.ui.groupBox_input.toggled.connect(lambda x: self.ros.sig_joydata.connect(self.setInputJoy) if x else self.ros.sig_joydata.disconnect(self.setInputJoy))
        if self.ui.groupBox_input.isChecked():
            self.ros.sig_joydata.connect(self.setInputJoy)





        # Set up URI scanner
        self.scanner = ScannerThread(radio=options.radio)
        self.scanner.start()


        # Connections from GUI
        self.ui.pushButton_connect.clicked.connect(lambda : self.connectPressed(self.ui.comboBox_connect.currentText(), auto=False)) # Start button -> connect
        self.ui.comboBox_connect.currentIndexChanged.connect(self.uriSelected)

        self.ui.checkBox_beep.toggled.connect(self.setBeep)
        self.ui.checkBox_kill.toggled.connect(self.setKill)
        self.ui.checkBox_kill.toggled.connect(self.ai.setKillSwitch)
        self.ui.checkBox_kill.toggled.emit(self.ui.checkBox_kill.checkState()) # force update
        self.ui.checkBox_hover.toggled.connect(self.flie.setHoverDisabled)
        self.ui.checkBox_hover.toggled.emit(self.ui.checkBox_hover.checkState()) # force update



        self.ui.checkBox_reconnect.toggled.connect(self.setAutoReconnect)
        self.ui.checkBox_startupConnect.toggled.connect(self.setStartupConnect)
        self.ui.pushButton_genRosMsg.clicked.connect(self.genRosMsg)

        self.ui.pushButton_baro.clicked.connect(self.updateBaroTopics)

        self.ui.groupBox_baro.toggled.connect(lambda x: self.updateBaroTopics(not x))






        # Connections to GUI
        self.flie.sig_packetSpeed.connect(self.updatePacketRate)
        self.flie.sig_flieLink.connect(self.ui.progressbar_link.setValue)
        self.paramManager.sig_baroFound.connect(lambda found: self.ui.label_baroFound.setText("Yes" if found else "No"))
        self.paramManager.sig_magFound.connect(lambda found: self.ui.label_magFound.setText("Yes" if found else "No"))
        self.paramManager.sig_test.connect(lambda name, p: self.ui.labelTest[str(name)].setText("Pass" if p else "FAIL"))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fw.setText(fw))
        self.paramManager.sig_firmware.connect(lambda fw, mod: self.ui.label_fwMod.setText(mod))
        self.logManager.sig_batteryUpdated.connect( lambda v : self.setToUpdate("vbat",self.ui.progressbar_bat.setValue, v))
        self.logManager.sig_cpuUpdated.connect( lambda v : self.setToUpdate("cpu",self.ui.progressbar_cpu.setValue, v))
        self.flie.inKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktIn,hz))
        self.flie.outKBPS.sig_KBPS.connect(lambda hz: self.updatePacketRate(self.ui.progressBar_pktOut,hz))


        # Connections GUI to GUI


        # Connections Within
        self.scanner.sig_foundURI.connect(self.receiveScanURI)
        self.sig_requestScan.connect(self.scanner.scan)
        self.sig_requestConnect.connect(self.flie.requestConnect)
        self.sig_requestDisconnect.connect(self.flie.requestDisconnect)
        self.flie.sig_stateUpdate.connect(self.updateFlieState)
        self.flie.sig_console.connect(self.ui.console.insertPlainText)


        # Show window
        self.show()

        # Initiate an initial Scan
        init_drivers(enable_debug_driver=False)
        self.startScanURI()