From a0e72c2d4707a752582de290cc98e7675ddce7dd Mon Sep 17 00:00:00 2001
From: sfrana <sfrana@iastate.edu>
Date: Thu, 26 Oct 2023 06:11:42 +0200
Subject: [PATCH] added output override functionality
---
cflib_groundstation/crazyflie_connection.py | 55 +++++++++++++++++++--
1 file changed, 51 insertions(+), 4 deletions(-)
diff --git a/cflib_groundstation/crazyflie_connection.py b/cflib_groundstation/crazyflie_connection.py
index d744462a8..7c7b9870d 100644
--- a/cflib_groundstation/crazyflie_connection.py
+++ b/cflib_groundstation/crazyflie_connection.py
@@ -14,8 +14,9 @@ from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from queue import Queue
#import groundstation_socket as gs
+import uCartCommander
from groundstation_socket import MessageTypeID
-
+from SetpointHandler import SetpointHandler, FlightMode
from cflib.crazyflie.log import LogConfig
@@ -44,6 +45,10 @@ class CrazyflieConnection:
self.param_callback_count = 0
self.logging_configs = []
+ self.setpoint_handler = SetpointHandler()
+
+ # self.setpoint_handler.setCommander(self.scf.cf.commander)
+
# self.timer = QTimer()
# self.timer.timeout.connect(self.update_plot)
# self.timer.start(50)
@@ -61,6 +66,15 @@ class CrazyflieConnection:
self.scf.wait_for_params()
self.is_connected = True
+ # sets commander
+ self.scf.cf.commander = uCartCommander.Commander(self.scf.cf)
+
+
+ # 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.scf.cf.commander)
+
def disconnect(self):
""" Disconnect from crazyflie. """
print("Disconnect quad")
@@ -79,11 +93,44 @@ class CrazyflieConnection:
raise Exception
def BeginUpdate():
raise Exception
- def OverrideOuput():
+ def OverrideOuput(self, command):
"""Sends all setpoints for a given amount of time"""
+ mode = command['data'][0]
+ time = command['data'][1:4] # Currently sent every 20ms by setpoint_handler may change
+ thrust = command['data'][5:8]
+ pitch = command['data'][9:12]
+ roll = command['data'][13:16]
+ yaw = command['data'][17:20]
+
+ # Error Handling
+ try:
+ yaw = float(yaw)
+ pitch = float(pitch)
+ roll = float(roll)
+ thrust = int(thrust)
+ except ValueError:
+ raise Exception
+
+ # Check that setpoint_handler has a commander set
+ if self.setpoint_handler.commander:
+ # Lets crazyflie know we are about to start flying
+ self.setpoint_handler.commander.start_flying()
+
+ if mode == 0: # Stop?
+ self.setpoint_handler.stopFlying()
+ elif mode == 1: # Rate
+ self.setpoint_handler.setRateMode()
+ elif mode == 2: # Angle
+ self.setpoint_handler.setAttitudeMode()
+ elif mode == 3: # Mixed
+ self.setpoint_handler.setMixedAttitudeMode()
+ elif mode == 4: # Position
+ raise Exception # Not implemented
+ else :
+ raise Exception
+
+ self.setpoint_handler.setSetpoint(yaw, pitch, roll, thrust)
- #TODO
- raise Exception
def GetNodeIds():
raise Exception
def SetParam(self, command):
--
GitLab