output override works

cflib_groundstation/SetpointHandler.py

@@ -32,7 +32,7 @@ class Setpoint:
         self.yaw: float = 0
         self.pitch: float = 0
         self.roll: float = 0
-        self.thrust: int = 0
+        self.thrust: float = 0
 
 
 class SetpointHandler:
@@ -184,7 +184,7 @@ class SetpointHandler:
         if self.commander:
             if self._flight_mode == FlightMode.ATTITUDE_TYPE:
                 # scales thrust from 100 for slider control.
-                thrust = self.setpoint.thrust * 60000 / 100
+                thrust = self.setpoint.thrust #* 60000 / 100
                 print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
                       f"{self.setpoint.roll}, {self.setpoint.thrust}")
 
@@ -193,8 +193,8 @@ class SetpointHandler:
                     int(thrust))
 
             elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
-
-                thrust = self.setpoint.thrust * 60000 / 100
+                print(self.setpoint.thrust)
+                thrust = self.setpoint.thrust #* 60000 / 100
                 print(f"Set attitude rate: {self.setpoint.yaw},"
                       f" {self.setpoint.pitch}, "
                       f"{self.setpoint.roll}, {thrust}")
@@ -205,7 +205,7 @@ class SetpointHandler:
 
             elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
                 # scales thrust from 1000 for more fine-grained gamepad control.
-                thrust = self.setpoint.thrust * 60000 / 1000
+                thrust = self.setpoint.thrust #* 60000 / 1000
                 print(f"Set mixed attitude: {self.setpoint.yaw},"
                       f" {self.setpoint.pitch}, "
                       f"{self.setpoint.roll}, {thrust}")

cflib_groundstation/crazyflie_connection.py

@@ -8,6 +8,7 @@ cflib library.
 from time import time
 from typing import List
 import time
+import struct
 
 import cflib.crtp
 from cflib.crazyflie import Crazyflie
@@ -82,8 +83,6 @@ class CrazyflieConnection:
             self.scf.close_link()
             self.scf = None
             self.is_connected = False
-        # disconnect the commander from setpointhandler
-        self.setpoint_handler.disconnectCommander()
 
     def Debug():
         raise Exception
@@ -97,19 +96,21 @@ class CrazyflieConnection:
         raise Exception
     def OverrideOuput(self, command): 
         """Sends all setpoints for a given amount of time"""
+        print(command["data"])
         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]
+        time = command['data'][1:5] # Currently sent every 20ms by setpoint_handler may change
+        thrust =  command['data'][5:9]
+        pitch =  command['data'][9:13]
+        roll =  command['data'][13:17]
+        yaw =  command['data'][17:21]
 
         # Error Handling
         try:
-            yaw = float(yaw)
-            pitch = float(pitch)
-            roll = float(roll)
-            thrust = int(thrust)
+            yaw = struct.unpack('f', bytes(yaw))[0]
+            pitch = struct.unpack('f', bytes(pitch))[0]
+            roll = struct.unpack('f', bytes(roll))[0]
+            thrust = struct.unpack('f', bytes(thrust))[0]
+            print(thrust)
         except ValueError:
             raise Exception
 
@@ -166,21 +167,26 @@ class CrazyflieConnection:
         """ Retrieve parameter value from crazyflie toc. """
         #Bytes 0 and 1 are node ID, ie group
         #Bytes 2 and 3 are node paramID, ie name
+        print("Getting Param...")
         group = command['data'][0:1]
         name = command['data'][2:3]
 
+
         try:
             if self.scf.is_link_open():
                 val = self.scf.cf.param.values[group][name]
+                print(val)
     
+
         except AttributeError:
             val = -1.234567
             pass
 
+            
         data = bytearray(group)
         data.append(name)
         data.append(val)
-
+        print(data)
         responsedata = {
         "msg_type": (MessageTypeID.RESPPARAM_ID),
         "msg_id": command['msg_id'],

cflib_groundstation/main.py

@@ -17,6 +17,7 @@ class main():
         self.inThread = Thread(target = gs.groundstation_connect, args = (self.inputQueue,))
         self.inThread.start()
         self.cfConnect = CrazyflieConnection()
+        self.cfConnect.connect("radio://0/20/2M/E7E7E7E7E7")
         self.commandThread = Thread(target = self.processCommands)
         self.commandThread.start()
         while True:
@@ -38,28 +39,13 @@ class main():
                 elif msg_type == MessageTypeID.BEGINUPDATE_ID.value:
                     self.cfConnect.BeginUpdate()
                 elif msg_type == MessageTypeID.OUTPUT_OVERRIDE_ID.value:
-                    self.cfConnect.OverrideOuput()
+                    self.cfConnect.OverrideOuput(command)
                 elif msg_type == MessageTypeID.GETNODES_ID.value:
                     self.cfConnect.GetNodes()
                 elif msg_type == MessageTypeID.SETPARAM_ID.value:
                     self.cfConnect.SetParam()
                 elif msg_type == MessageTypeID.GETPARAM_ID.value:
-                    group = command['data'][0:1]
-                    name = command['data'][2:3]
-
-                    val = 10
-                    data = bytearray(group)
-                    data += name
-                    data.append(val)
-
-                    responsedata = {
-                    "msg_type": (MessageTypeID.RESPPARAM_ID.value),
-                    "msg_id": command['msg_id'],
-                    "data_len": 8,
-                    "data": data
-                    }
-                    self.outputQueue.put(responsedata)
-                    #self.cfConnect.GetParam()
+                    self.cfConnect.GetParam(command, self.outputQueue)
                 elif msg_type == MessageTypeID.SETSOURCE_ID.value:
                     self.cfConnect.SetSource()
                 elif msg_type == MessageTypeID.GETSOURCE_ID.value: