added output override functionality

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):