"""
Setpoint handler holds classes used to send setpoints to the crazyflie proto
connection in an organized fashion. It is used by the setpoint menu found on
the controls tab.
"""
from enum import Enum
from PyQt5.QtCore import QTimer
from uCartCommander import Commander
from threading import Semaphore
class FlightMode(Enum):
""" Available flight modes found in crazyflie firmware. """
TYPE_STOP: int = 0
TYPE_VELOCITY_WORLD: int = 1
TYPE_ZDISTANCE: int = 2
altHoldType: int = 4
TYPE_HOVER: int = 5
FULL_STATE_TYPE: int = 6
TYPE_POSITION: int = 7
# ------ Custom modes added to firmware by uCart -----------
ATTITUDE_RATE_TYPE: int = 8
ATTITUDE_TYPE: int = 9
MIXED_ATTITUDE_TYPE: int = 10
class Setpoint:
""" Used in order to hold setpoint easily. """
def __init__(self):
self.yaw: float = 0
self.pitch: float = 0
self.roll: float = 0
self.thrust: int = 0
class SetpointHandler:
"""
Setpoint handler is used to send setpoints to the crazyflie proto
connection in an organized manner. It holds direct access to the
crazyflie's commander, which means that whenever connecting to a
crazyflie, that commander must be configured in the setpoint handler.
This commander is intended to by the custom uCartCommander which is a
modified low level setpoint commander. Currently, no modified version of
the high level commander exists.
"""
def __init__(self, commander: Commander):
"""
Initialize timer,
:param commander: uCartCommander taken directly from the synchronous
crazyflie.
"""
self.setpoint = Setpoint()
self.commander = commander
self.setpoint_semaphore = Semaphore(1)
self._flight_mode = FlightMode.TYPE_STOP
# Send setpoints to crazyflie every 20 ms.
self.timer = QTimer()
self.timer.timeout.connect(self.update)
self.timer.start(20)
def update(self):
""" If the flight mode is not stopped, send the current setpoint. """
if self._flight_mode != FlightMode.TYPE_STOP:
self.sendSetpoint()
def getFlightMode(self):
""" Returns current flight mode. """
return self._flight_mode
def setAttitudeMode(self):
""" Safely set to attitude mode. Uses semaphore in case a callback
happens in the middle of the function. """
self.setpoint_semaphore.acquire()
self._flight_mode = FlightMode.ATTITUDE_TYPE
self.setpoint_semaphore.release()
def setMixedAttitudeMode(self):
""" Safely set to mixed attitude mode. Uses semaphore in case a
callback happens in the middle of the function. """
self.setpoint_semaphore.acquire()
self._flight_mode = FlightMode.MIXED_ATTITUDE_TYPE
self.setpoint_semaphore.release()
def setRateMode(self):
""" Safely set to rate mode. Uses semaphore in case a callback
happens in the middle of the function. """
self.setpoint_semaphore.acquire()
self._flight_mode = FlightMode.ATTITUDE_RATE_TYPE
self.setpoint_semaphore.release()
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()
def stopFlying(self):
""" Tells the crazyflie to stop flying. """
self.setpoint_semaphore.acquire()
self._flight_mode = FlightMode.TYPE_STOP
self.commander.send_notify_setpoint_stop(0)
self.setpoint_semaphore.release()
print("Stop flying")
def setSetpoint(self, yaw: float, pitch: float, roll: float, thrust: float):
""" Safely sets the crazyflie setpoint. Utilizes semaphore to avoid
reading and writing at the same time due to callbacks. """
self.setpoint_semaphore.acquire()
self.setpoint.yaw = yaw
self.setpoint.pitch = pitch
self.setpoint.roll = roll
self.setpoint.thrust = thrust
self.sendSetpointUnsafe()
self.setpoint_semaphore.release()
def sendSetpoint(self):
""" Uses commander to send setpoints to crazyflie depending upon the
current flight mode. """
self.setpoint_semaphore.acquire()
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}")
self.commander.send_attitude_setpoint(
self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
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)
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)