def init():
logging.basicConfig(level=logging.WARNING)
# init drivers
init_drivers(enable_debug_driver=False)
# intit & start Controllerinput-Thread
GamepadController()
def init():
logging.basicConfig(level=logging.WARNING)
# init drivers
init_drivers(enable_debug_driver=False)
# intit & start Controllerinput-Thread
GamepadController()
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)
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)
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')
def initializeDrivers(self):
'''
Initialize the low-level drivers (don't list the debug drivers)
'''
radioChannel.init_drivers(enable_debug_driver=False)
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(
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)
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()
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()
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)
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()