diff --git a/pycrocart/CrazyflieProtoConnection.py b/pycrocart/CrazyflieProtoConnection.py
index f5a400773e4815cf49562526bad88454c2f91f76..1a2475f4718d8abeb7617a23eec9fd1192141043 100644
--- a/pycrocart/CrazyflieProtoConnection.py
+++ b/pycrocart/CrazyflieProtoConnection.py
@@ -44,7 +44,7 @@ class CrazyflieProtoConnection:
self.logging_queue = Queue()
self.scf = None
- self.is_connected = None
+ self.is_connected = False
self.param_callback_count = 0
self.logging_configs = []
@@ -193,11 +193,14 @@ class CrazyflieProtoConnection:
self.scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
self.scf.open_link()
self.scf.wait_for_params()
+ self.is_connected = True
def disconnect(self):
""" Disconnect from crazyflie. """
if self.is_connected:
self.scf.close_link()
+ self.scf = None
+ self.is_connected = False
@staticmethod
def list_available_crazyflies():
diff --git a/pycrocart/LoggingConfigTab.py b/pycrocart/LoggingConfigTab.py
index e3a76e08451e9ab51151362f5bb8f8d7c72be701..09bb9d5b303ab1a252b66e899658b4923ee839a2 100644
--- a/pycrocart/LoggingConfigTab.py
+++ b/pycrocart/LoggingConfigTab.py
@@ -112,9 +112,14 @@ class LoggingConfigTab(QWidget):
""" Whenever the refresh button is clicked, call this function. """
self.cf.clear_logging_configs()
+ self.set_logging_block_from_file()
+
+ def on_connect(self):
self.update_logging_values()
+ self.on_refresh()
- self.set_logging_block_from_file()
+ def on_disconnect(self):
+ self.update_logging_values()
def on_stop(self):
""" Whenever the stop logging button is clicked, call this function.
diff --git a/pycrocart/ParameterTab.py b/pycrocart/ParameterTab.py
index ab80f04feaf1ec89e021d9e2910624065a632715..a76f365829e95cd99d77f1ed4096dca416bf9984 100644
--- a/pycrocart/ParameterTab.py
+++ b/pycrocart/ParameterTab.py
@@ -166,9 +166,6 @@ class ParameterTab(QWidget):
self.progress_bar.setMaximumWidth(200)
layout.addWidget(self.progress_bar, 12, 1, alignment=Qt.AlignHCenter)
- # for connecting during runtime this would need to be moved
- self.on_connect()
-
def on_connect(self):
""" Whenever connecting to the crazyflie, grab the parameter table of
contents. This is how we actually get what parameters and groups are
@@ -176,6 +173,10 @@ class ParameterTab(QWidget):
self.toc = self.cf.get_param_toc()
self.populate_group_menu_options()
+ def on_disconnect(self):
+ self.toc = {}
+ self.populate_group_menu_options()
+
def populate_group_menu_options(self):
""" Remove any current entries from parameter boxes and add the
options for the parameter groups. """
@@ -192,7 +193,8 @@ class ParameterTab(QWidget):
self.get_param_cbox.clear()
group = self.get_param_group_cbox.currentText()
- self.get_param_cbox.addItems(self.toc[group])
+ if group != "": # nothing there when cf is disconnected
+ self.get_param_cbox.addItems(self.toc[group])
def on_set_param_group_changed(self):
""" Whenever a parameter group is selected, populate the options for
@@ -200,7 +202,8 @@ class ParameterTab(QWidget):
self.set_param_cbox.clear()
group = self.set_param_group_cbox.currentText()
- self.set_param_cbox.addItems(self.toc[group])
+ if group != "": # nothing there when cf is disconnected
+ self.set_param_cbox.addItems(self.toc[group])
def get_param(self):
""" Retrieve parameter value from crazyflie proto connection. All
diff --git a/pycrocart/PyCroCart.py b/pycrocart/PyCroCart.py
index 7f2e93b31ab8a326a2bfd7792aed2677413ac269..bfe3e2e09f34165d678905238e16949735ac9d84 100644
--- a/pycrocart/PyCroCart.py
+++ b/pycrocart/PyCroCart.py
@@ -8,7 +8,8 @@ groundstation and launches the gui.
"""
import sys
-from PyQt5.QtWidgets import QApplication, QMainWindow, QTabWidget, QHBoxLayout, QVBoxLayout
+from PyQt5.QtWidgets import QApplication, QMainWindow, QTabWidget, \
+ QHBoxLayout, QVBoxLayout, QComboBox, QPushButton, QWidget, QLabel
import uCartCommander
from CrazyflieProtoConnection import CrazyflieProtoConnection
from ControlTab import ControlTab
@@ -18,6 +19,7 @@ from cfclient.utils.input import JoystickReader
from ParameterTab import ParameterTab
from LoggingConfigTab import LoggingConfigTab
import time
+import os
class PyCroCart(QMainWindow):
@@ -32,44 +34,48 @@ class PyCroCart(QMainWindow):
def __init__(self, cf: CrazyflieProtoConnection,
setpoint_handler: SetpointHandler):
super().__init__()
-
- self.dropdown = QComboBox(self)
- self.dropdown.addItem("radio://45/")
- self.dropdown.addItem("radio://100/")
- self.dropdown.setFixedWidth(150)
+
+ self.cf = cf
+ self.crazyflie_selection_box = QComboBox(self)
+ self.crazyflie_selection_box.setFixedWidth(150)
self.connect = QPushButton("Connect", self)
+ self.connect.clicked.connect(self.on_connect)
self.connect.setFixedWidth(100)
self.scan = QPushButton("Scan", self)
+ self.scan.clicked.connect(self.on_scan)
self.scan.setFixedWidth(100)
+
+ self.error_label = QLabel("")
button_layout = QHBoxLayout()
- button_layout.addWidget(self.dropdown)
+ button_layout.addWidget(self.crazyflie_selection_box)
button_layout.addWidget(self.connect)
button_layout.addWidget(self.scan)
+ button_layout.addWidget(self.error_label)
button_layout.addStretch(1)
-
self.joystick_reader = JoystickReader()
+ self.setpoint_handler = setpoint_handler
self.tabs = QTabWidget()
- self.tab1 = ControlTab(cf.logging_queue, setpoint_handler,
- self.joystick_reader, cf)
- self.tab2 = InputConfigDialogue(self.joystick_reader,
- self.tab1.setpoint_menu.enableGamepad)
- self.tab3 = ParameterTab(cf)
- self.tab4 = LoggingConfigTab(
- cf, self.tab1.logging_menu.update_available_logging_variables)
+ self.control_tab = ControlTab(cf.logging_queue, self.setpoint_handler,
+ self.joystick_reader, cf)
+ self.gamepad_tab = InputConfigDialogue(
+ self.joystick_reader, self.control_tab.setpoint_menu.enableGamepad)
+ self.parameter_tab = ParameterTab(cf)
+ self.logging_config_tab = LoggingConfigTab(
+ cf, self.control_tab.logging_menu.update_available_logging_variables)
self.setGeometry(100, 200, 600, 600)
- self.tabs.addTab(self.tab1, "Controls Window")
- self.tabs.addTab(self.tab2, "Gamepad Configuration")
- self.tabs.addTab(self.tab3, "Parameter Window")
- self.tabs.addTab(self.tab4, "Logging Window")
+ self.tabs.addTab(self.control_tab, "Controls Window")
+ self.tabs.addTab(self.gamepad_tab, "Gamepad Configuration")
+ self.tabs.addTab(self.parameter_tab, "Parameter Window")
+ self.tabs.addTab(self.logging_config_tab, "Logging Window")
- main_layout = QHBoxLayout()
+ main_layout = QVBoxLayout()
main_layout.addLayout(button_layout)
main_layout.addWidget(self.tabs)
@@ -77,7 +83,83 @@ class PyCroCart(QMainWindow):
widget.setLayout(main_layout)
self.setCentralWidget(widget)
- # self.show()
+
+ def on_scan(self):
+ """ When pressing the scan button, connect to the crazyradio,
+ and scan for crazyflies. Populate the crazyflie selection box. """
+
+ cfs = self.cf.list_available_crazyflies()
+ # cfs = ["radio://45/", "radio://100/"] # for development purposes only
+ if cfs:
+ error_text = ""
+ self.error_label.setText(
+ "<span style='color: red;'>" + error_text + "</span>")
+
+ # add crazyflie connections to selection box
+ self.crazyflie_selection_box.clear()
+ self.crazyflie_selection_box.addItems(cfs[0][:len(cfs[0])-1])
+ else:
+ error_text = "Either no crazyradio plugged in, or no crazyflies " \
+ "detected."
+ self.error_label.setText(
+ "<span style='color: red;'>" + error_text + "</span>")
+
+ # clear out selection box
+ self.crazyflie_selection_box.clear()
+
+ def on_connect(self):
+
+ # check that a crazyflie is selected
+ uri = self.crazyflie_selection_box.currentText() + "/E7E7E7E7E7"
+
+ # return an error to the user if otherwise
+ if uri == "/E7E7E7E7E7":
+ error_text = "No crazyflie selected."
+ self.error_label.setText(
+ "<span style='color: red;'>" + error_text + "</span>")
+
+ # attempt to connect to the crazyflie
+ self.cf.connect(uri)
+ self.cf.scf.cf.commander = uCartCommander.Commander(cf1.scf.cf)
+ self.cf.scf.wait_for_params()
+
+ # offer the user a green error label saying connected when connected
+ error_text = "Connected to drone"
+ self.error_label.setText(
+ "<span style='color: green;'>" + error_text + "</span>")
+
+ # change the connect button to a disconnect button, and disconnect this
+ # function from it and connect to the on_disconnect function
+ self.connect.clicked.disconnect(self.on_connect)
+ self.connect.clicked.connect(self.on_disconnect)
+ self.connect.setText("Disconnect")
+
+ # connect the crazyflie commander to the setpoint handler
+ # refresh the logging page so that it displays the toc
+ # refresh the parameter page so that it displays the correct information
+ self.setpoint_handler.setCommander(self.cf.scf.cf.commander)
+ self.logging_config_tab.on_connect()
+ self.parameter_tab.on_connect()
+
+ def on_disconnect(self):
+
+ # terminate the link in crazyflieprotoconnection
+ self.cf.disconnect()
+
+ error_text = ""
+ self.error_label.setText(
+ "<span style='color: green;'>" + error_text + "</span>")
+
+ # change the disconnect button to a connect button, and disconnect this
+ # function from it and connect the on_connect function
+ self.connect.clicked.disconnect(self.on_disconnect)
+ self.connect.clicked.connect(self.on_connect)
+ self.connect.setText("Connect")
+
+ # disconnect the commander from setpointhandler
+ self.setpoint_handler.disconnectCommander()
+ self.logging_config_tab.on_disconnect()
+ self.parameter_tab.on_disconnect()
# Start the application
@@ -86,15 +168,10 @@ if __name__ == '__main__':
app = QApplication(sys.argv)
cf1 = CrazyflieProtoConnection()
- uri = 'radio://0/60/2M/E7E7E7E7E7'
- cf1.connect(uri)
- cf1.scf.cf.commander = uCartCommander.Commander(cf1.scf.cf)
-
- time.sleep(1)
- setpoint_handler1 = SetpointHandler(cf1.scf.cf.commander)
+ setpoint_handler1 = SetpointHandler()
window = PyCroCart(cf1, setpoint_handler1)
- window.tab4.on_refresh()
window.show()
- sys.exit(app.exec_())
+ # Janky but it does work
+ os._exit(app.exec_())
diff --git a/pycrocart/SetpointHandler.py b/pycrocart/SetpointHandler.py
index 7e6e8f8ac5275f982bd1fcdffc1eba91740fbb11..4e37777a2ae01e0ddeb3189c438b7c4256ae6f5a 100644
--- a/pycrocart/SetpointHandler.py
+++ b/pycrocart/SetpointHandler.py
@@ -47,7 +47,7 @@ class SetpointHandler:
"""
- def __init__(self, commander: Commander):
+ def __init__(self):
"""
Initialize timer,
@@ -56,7 +56,7 @@ class SetpointHandler:
"""
self.setpoint = Setpoint()
- self.commander = commander
+ self.commander = None
self.setpoint_semaphore = Semaphore(1)
self._flight_mode = FlightMode.TYPE_STOP
@@ -66,6 +66,17 @@ class SetpointHandler:
self.timer.timeout.connect(self.update)
self.timer.start(20)
+ def setCommander(self, commander: Commander):
+ """ When the crazyflie is not connected, there will be no valid
+ commander. Set it during runtime. Enables the use of if commander:
+ on other functions to check if it is valid. """
+ self.commander = commander
+
+ def disconnectCommander(self):
+ """ Set the commander equal to none so that it won't be called when
+ we are not actually connected to the crazyflie. """
+ self.commander = None
+
def update(self):
""" If the flight mode is not stopped, send the current setpoint. """
if self._flight_mode != FlightMode.TYPE_STOP:
@@ -99,16 +110,17 @@ class SetpointHandler:
def startFlying(self):
""" Sends an all 0's setpoint which is the convention to tell the
crazyflie to listen for setpoints. """
- self.commander.start_flying()
+ if self.commander:
+ self.commander.start_flying()
def stopFlying(self):
""" Tells the crazyflie to stop flying. """
- self.setpoint_semaphore.acquire()
- self._flight_mode = FlightMode.TYPE_STOP
+ if self.commander:
+ self.setpoint_semaphore.acquire()
+ self._flight_mode = FlightMode.TYPE_STOP
- self.commander.send_notify_setpoint_stop(0)
- self.setpoint_semaphore.release()
- print("Stop flying")
+ self.commander.send_notify_setpoint_stop(0)
+ self.setpoint_semaphore.release()
def setSetpoint(self, yaw: float, pitch: float, roll: float, thrust: float):
""" Safely sets the crazyflie setpoint. Utilizes semaphore to avoid
@@ -126,76 +138,78 @@ class SetpointHandler:
def sendSetpoint(self):
""" Uses commander to send setpoints to crazyflie depending upon the
current flight mode. """
- self.setpoint_semaphore.acquire()
+ if self.commander:
+ self.setpoint_semaphore.acquire()
- if self._flight_mode == FlightMode.ATTITUDE_TYPE:
+ if self._flight_mode == FlightMode.ATTITUDE_TYPE:
- # scales thrust from 100 for slider control.
- thrust = self.setpoint.thrust * 65000 / 100
- print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {thrust}")
+ # scales thrust from 100 for slider control.
+ thrust = self.setpoint.thrust * 65000 / 100
+ print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {thrust}")
- self.commander.send_attitude_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- thrust)
+ self.commander.send_attitude_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ thrust)
- elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
+ elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
- thrust = self.setpoint.thrust * 65000 / 100
- print(f"Set attitude rate: {self.setpoint.yaw},"
- f" {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {thrust}")
+ thrust = self.setpoint.thrust * 65000 / 100
+ print(f"Set attitude rate: {self.setpoint.yaw},"
+ f" {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {thrust}")
- self.commander.send_attitude_rate_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- thrust)
+ self.commander.send_attitude_rate_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ thrust)
- elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
+ elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
- # scales thrust from 1000 for more fine-grained gamepad control.
- thrust = self.setpoint.thrust * 65000 / 1000
- print(f"Set mixed attitude: {self.setpoint.yaw},"
- f" {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {thrust}")
+ # scales thrust from 1000 for more fine-grained gamepad control.
+ thrust = self.setpoint.thrust * 65000 / 1000
+ print(f"Set mixed attitude: {self.setpoint.yaw},"
+ f" {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {thrust}")
- self.commander.send_mixed_attitude_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- thrust)
+ self.commander.send_mixed_attitude_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ thrust)
- self.setpoint_semaphore.release()
+ self.setpoint_semaphore.release()
def sendSetpointUnsafe(self):
""" Exactly the same as send setpoint but no semaphore is used. """
print("Unsafe mode activate :)")
- if self._flight_mode == FlightMode.ATTITUDE_TYPE:
- # scales thrust from 100 for slider control.
- thrust = self.setpoint.thrust * 65000 / 100
- print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {self.setpoint.thrust}")
-
- self.commander.send_attitude_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- int(thrust))
-
- elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
-
- thrust = self.setpoint.thrust * 65000 / 100
- print(f"Set attitude rate: {self.setpoint.yaw},"
- f" {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {thrust}")
-
- self.commander.send_attitude_rate_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- thrust)
-
- elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
- # scales thrust from 1000 for more fine-grained gamepad control.
- thrust = self.setpoint.thrust * 65000 / 1000
- print(f"Set mixed attitude: {self.setpoint.yaw},"
- f" {self.setpoint.pitch}, "
- f"{self.setpoint.roll}, {thrust}")
-
- self.commander.send_mixed_attitude_setpoint(
- self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
- thrust)
+ if self.commander:
+ if self._flight_mode == FlightMode.ATTITUDE_TYPE:
+ # scales thrust from 100 for slider control.
+ thrust = self.setpoint.thrust * 65000 / 100
+ print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {self.setpoint.thrust}")
+
+ self.commander.send_attitude_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ int(thrust))
+
+ elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
+
+ thrust = self.setpoint.thrust * 65000 / 100
+ print(f"Set attitude rate: {self.setpoint.yaw},"
+ f" {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {thrust}")
+
+ self.commander.send_attitude_rate_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ thrust)
+
+ elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
+ # scales thrust from 1000 for more fine-grained gamepad control.
+ thrust = self.setpoint.thrust * 65000 / 1000
+ print(f"Set mixed attitude: {self.setpoint.yaw},"
+ f" {self.setpoint.pitch}, "
+ f"{self.setpoint.roll}, {thrust}")
+
+ self.commander.send_mixed_attitude_setpoint(
+ self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+ thrust)
diff --git a/pycrocart/SetpointMenu.py b/pycrocart/SetpointMenu.py
index 0740a18154cf4a13d1d6c19ea7a7a499a8ebab85..f5f56cd52bc4ee12ae8bc56c039355110f46ec2c 100644
--- a/pycrocart/SetpointMenu.py
+++ b/pycrocart/SetpointMenu.py
@@ -163,7 +163,8 @@ class SetpointMenu(QWidget):
# into the right control mode, and also tell the crazyflie we are
# about to start flying.
if self.setpoint_handler.getFlightMode() == FlightMode.TYPE_STOP:
- self.setpoint_handler.commander.start_flying()
+ if self.setpoint_handler.commander: # check if connected
+ self.setpoint_handler.commander.start_flying()
if self.gamepad_button.isChecked():
self.setpoint_handler.setMixedAttitudeMode()