adding functionality to send packets back

cflib_groundstation/groundstation_socket.py

@@ -92,6 +92,21 @@ class GroundstationSocket():
 
     def EncodePacket(self, messagedata):
         print("encoding packet")
+        bytedata = bytearray(Message.BEGIN_CHAR.value)
+        bytedata.append(messagedata["msg_type"] & 0xFF)
+        bytedata.append((messagedata["msg_type"] >> 8) & 0xFF)
+        bytedata.append(messagedata["msg_id"])
+        bytedata.append(messagedata["data_len"] & 0xFF)
+        bytedata.append((messagedata["data_len"] >> 8) & 0xFF)
+        bytedata.append(messagedata["data"])
+        bytedata.append(self.packetChecksum(messagedata, PacketHeader.HDR_SIZE.value + messagedata["data_len"] + ChecksumFormat.CSUM_SIZE.value))
+        return bytedata
+
+    def WriteToBackend(self, message):
+        messagedata = self.EncodePacket(message)
+        self.connection.send(messagedata)
+
+
 
 
 

cflib_groundstation/main.py

 from email import message
 from queue import Queue
 from threading import Thread
-from groundstation_socket import GroundstationSocket
+from groundstation_socket import GroundstationSocket, MessageTypeID
 from crazyflie_connection import CrazyflieConnection
 
 
@@ -16,12 +16,48 @@ class main():
         gs = GroundstationSocket()
         self.inThread = Thread(target = gs.groundstation_connect, args = (self.inputQueue,))
         self.inThread.start()
+        cfConnect = CrazyflieConnection()
         while True:
             if self.inputQueue.not_empty:
                 command = self.inputQueue.get()
                 print(command)
                 msg_type = command["msg_type"]
                 print(msg_type)
+                if msg_type == MessageTypeID.GETPACKETLOGS_ID.value:
+                    cfConnect.GetPacketLogs()
+                elif msg_type == MessageTypeID.UPDATE_ID.value:
+                    cfConnect.Update()
+                elif msg_type == MessageTypeID.BEGINUPDATE_ID.value:
+                    cfConnect.BeginUpdate()
+                elif msg_type == MessageTypeID.OUTPUT_OVERRIDE_ID.value:
+                    cfConnect.OverrideOuput()
+                elif msg_type == MessageTypeID.GETNODES_ID.value:
+                    cfConnect.GetNodes()
+                elif msg_type == MessageTypeID.SETPARAM_ID.value:
+                    cfConnect.SetParam()
+                elif msg_type == MessageTypeID.GETPARAM_ID.value:
+                    cfConnect.GetParam()
+                elif msg_type == MessageTypeID.SETSOURCE_ID.value:
+                    cfConnect.SetSource()
+                elif msg_type == MessageTypeID.GETSOURCE_ID.value:
+                    cfConnect.GetSource()
+                elif msg_type == MessageTypeID.RESPSOURCE_ID.value:
+                    cfConnect.RespSource()
+                elif msg_type == MessageTypeID.GETOUTPUT_ID.value:
+                    cfConnect.GetOutput()
+                elif msg_type == MessageTypeID.GETNODES_ID.value:
+                    cfConnect.GetNodes()
+                elif msg_type == MessageTypeID.ADDNODE_ID.value:
+                    cfConnect.AddNode()
+                elif msg_type == MessageTypeID.LOG_ID.value:
+                    cfConnect.GetLogFile()
+                elif msg_type == MessageTypeID.LOG_END_ID.value:
+                    cfConnect.LogBlockCommand()
+            
+            if self.outputQueue.not_empty:
+                message = self.outputQueue.get()
+                gs.WriteToBackend(message)
+