from PyQt5.QtWidgets import QWidget, QGridLayout, QLabel, QLineEdit, QSlider, \
QPushButton, QButtonGroup, QCheckBox, QVBoxLayout
from PyQt5.QtCore import Qt, QTimer
from SetpointHandler import SetpointHandler, FlightMode, Setpoint
from cfclient.utils.input import JoystickReader
from GamepadWizardTab import DeviceReader
from cfclient.utils.config_manager import ConfigManager
from threading import Semaphore
class SetpointMenu(QWidget):
""" Menu in control tab where setpoints are entered. """
def __init__(self, setpoint_handler: SetpointHandler,
joystick: JoystickReader, ):
"""
Initialize menu.
:param setpoint_handler: When a setpoint is sent, it is sent to the
setpoint handler which handles the interaction with the
crazyflieProtoConnection.
:param joystick: When the setpoint is from the gamepad, it needs to
be read from somewhere. This is where it comes from.
"""
super().__init__()
self.setpoint_handler = setpoint_handler
self.joystick = joystick
self.joystick_reader = DeviceReader(self.joystick)
layout = QVBoxLayout()
grid_widget = QWidget()
grid_layout = QGridLayout()
grid_widget.setLayout(grid_layout)
layout.addWidget(grid_widget, 1)
self.setLayout(layout)
self.timer = QTimer()
self.setpoint = Setpoint()
self.setpoint_semaphore = Semaphore(1) # Helps for synchronization with
# setpoint handler
# ------------------- Gamepad Selection --------------------------------
self.gamepad_button = QCheckBox("Gamepad Mode")
self.setpoint_button = QCheckBox("Setpoint Mode")
self.setpoint_button.setChecked(True)
self.gamepad_button.toggled.connect(self.toggleSetpointMode)
self.setpoint_button.toggled.connect(self.toggleSetpointMode)
self.gamepad_or_setpoint = QButtonGroup() # Grouping buttons can allow
# you to only be able to select one or the other.
self.gamepad_or_setpoint.addButton(self.gamepad_button, 1)
self.gamepad_or_setpoint.addButton(self.setpoint_button, 2)
grid_layout.addWidget(self.gamepad_button, 0, 0)
grid_layout.addWidget(self.setpoint_button, 0, 1)
# ------------------ Setpoints -----------------------------------------
yaw_label = QLabel("Yaw Setpoint:")
pitch_label = QLabel("Pitch Setpoint:")
roll_label = QLabel("Roll Setpoint:")
self.yaw_box = QLineEdit()
self.yaw_box.setText('0') # Default to 0.
self.pitch_box = QLineEdit()
self.pitch_box.setText('0')
self.roll_box = QLineEdit()
self.roll_box.setText('0')
grid_layout.addWidget(yaw_label, 1, 0)
grid_layout.addWidget(self.yaw_box, 1, 1)
grid_layout.addWidget(pitch_label, 2, 0)
grid_layout.addWidget(self.pitch_box, 2, 1)
grid_layout.addWidget(roll_label, 3, 0)
grid_layout.addWidget(self.roll_box, 3, 1)
# ------------------ Thrust --------------------------------------------
# Add the sliders and thrust label
thrust_label = QLabel("Thrust:")
self.thrust_slider = QSlider(Qt.Horizontal)
# Thrust is normally between 0 and 100. Scaled correctly to crazyflie
# in the setpoint handler. For flypi, you only want it between 24% and
# 34%, and so instead everything is on a 1000 scale, and you are adding
# the gamepad input to 240 up to 340.
self.thrust_slider.setMinimum(0)
self.thrust_slider.setMaximum(100)
grid_layout.addWidget(thrust_label, 4, 0)
grid_layout.addWidget(self.thrust_slider, 4, 1)
# ----------------- Attitude or Rate mode ------------------------------
self.b1 = QCheckBox("Attitude Mode")
self.b2 = QCheckBox("Rate Mode")
self.b2.setChecked(True) # Default to rate mode.
self.b1.toggled.connect(self.toggle_flight_mode_while_running)
self.b1.toggled.connect(self.toggle_flight_mode_while_running)
self.attitude_or_rate = QButtonGroup()
self.attitude_or_rate.addButton(self.b1, 1)
self.attitude_or_rate.addButton(self.b2, 2)
grid_layout.addWidget(self.b1, 5, 0)
grid_layout.addWidget(self.b2, 5, 1)
# ----------------- Send Setpoint / Stop Flying ------------------------
self.valid_label = QLabel("Setpoint Valid")
self.send_setpoint_button = QPushButton("Send Setpoint")
self.send_setpoint_button.clicked.connect(self.send_setpoint)
self.stop_flying_button = QPushButton("Stop Flying")
self.stop_flying_button.clicked.connect(self.stop_flying)
layout.addWidget(self.valid_label, 2)
layout.addWidget(self.send_setpoint_button, 3)
layout.addWidget(self.stop_flying_button, 4)
@staticmethod
def cap_max_value(input_value: float, max_value: float,
min_value: float):
""" Saturate an input value """
if input_value > max_value:
value = max_value
elif input_value < min_value:
value = min_value
else:
value = input_value
return value
def send_setpoint(self):
"""
This is a function on a timer whenever we are attempting to send
setpoints. Grabs the setpoints from the setpoint boxes and thrust
slider. It also checks whether we are in gamepad mixed
attitude control mode, rate mode, or attitude mode.
"""
yaw = self.yaw_box.text()
pitch = self.pitch_box.text()
roll = self.roll_box.text()
thrust = self.thrust_slider.value()
# Have the UI give the user feedback on if the setpoints are invalid.
all_valid = True
try:
yaw = float(yaw)
pitch = float(pitch)
roll = float(roll)
thrust = int(thrust)
self.valid_label.setText("Setpoint Valid")
except ValueError:
all_valid = False
self.valid_label.setText("Setpoint Invalid")
if all_valid:
# if we are only just starting, we need to set the setpoint handler
# 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.gamepad_button.isChecked():
self.setpoint_handler.setMixedAttitudeMode()
else:
if self.b1.isChecked():
self.setpoint_handler.setAttitudeMode()
else:
self.setpoint_handler.setRateMode()
yaw = self.cap_max_value(yaw, 20, -20)
pitch = self.cap_max_value(pitch, 5, -5)
roll = self.cap_max_value(roll, 5, -5)
# If you are a flypi do this
thrust = thrust + 100
thrust = self.cap_max_value(thrust, 340, 240)
# otherwise don't..... but I'm not changing that rn
self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)
def stop_flying(self):
""" Set the setpoints to all 0, which should stop the drone from
flying. """
self.setpoint_handler.stopFlying()
def toggle_flight_mode_while_running(self):
""" If the user tries to change the flight mode, let them. This isn't
available in gamepad mode because the buttons are not enabled and
thus cannot be pressed. """
if self.setpoint_handler.getFlightMode() != FlightMode.TYPE_STOP:
if self.b1.isChecked():
self.setpoint_handler.setAttitudeMode()
else:
self.setpoint_handler.setRateMode()
def getGamepadSetpoint(self, data):
""" This function is called whenever new data from the gamepad comes
in. """
# Using a semaphore in case the setpoint is attempted to be read
# before being completely set.
self.setpoint_semaphore.acquire()
self.setpoint.yaw = data.yaw
self.setpoint.pitch = data.pitch
self.setpoint.roll = data.roll
self.setpoint.thrust = data.thrust
self.setpoint_semaphore.release()
def enableGamepad(self, current_selection_name: str):
""" Called when the gamepad checkbox is checked. """
# Get gamepad configuration from gamepad wizard, and maps it using
# the loaded gamepad map.
loaded_map = ConfigManager().get_config(current_selection_name)
if loaded_map:
self.joystick.set_raw_input_map(loaded_map)
print("Joystick set")
else:
print("error loading config")
self.joystick_reader.start_reading()
# callback to setting setpoints whenever the gamepad has changing
# inputs. Notice that setpoints are SET whenever new data comes in,
# but are not neccesarilly SENT.
self.joystick_reader.mapped_values_signal.connect(
self.getGamepadSetpoint)
def toggleSetpointMode(self):
if self.gamepad_button.isChecked():
# Gray out the setpoint boxes, so you can't change stuff when in
# gamepad mode.
self.yaw_box.setEnabled(False)
self.pitch_box.setEnabled(False)
self.roll_box.setEnabled(False)
self.thrust_slider.setEnabled(False)
self.b1.setEnabled(False)
self.b2.setEnabled(False)
self.send_setpoint_button.setEnabled(False)
self.stop_flying_button.setEnabled(False)
# Send new gamepad setpoint every 100 ms
self.timer.timeout.connect(self.sendGamepadSetpoint)
self.timer.start(100)
else:
self.yaw_box.setEnabled(True)
self.pitch_box.setEnabled(True)
self.roll_box.setEnabled(True)
self.thrust_slider.setEnabled(True)
self.b1.setEnabled(True)
self.b2.setEnabled(True)
self.send_setpoint_button.setEnabled(True)
self.stop_flying_button.setEnabled(True)
self.timer.timeout.disconnect(self.sendGamepadSetpoint)
self.timer.stop()
self.stop_flying()
def sendGamepadSetpoint(self):
# Send the gamepad setpoint, but because that is based on the text
# boxes, set them to correct setpoint.
self.setpoint_semaphore.acquire()
self.yaw_box.setText(str(round(self.setpoint.yaw, 2)))
self.roll_box.setText(str(round(self.setpoint.roll, 2)))
self.pitch_box.setText(str(round(self.setpoint.pitch, 2)))
self.thrust_slider.setValue(int(round(self.setpoint.thrust, 2)))
self.setpoint_semaphore.release()
self.send_setpoint()