diff --git a/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc b/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc index 46a66108eef65d96e5e41d73bf7defe483a5523d..e6d5daf61a108acc7e4a3a17e2ac232b2a13e297 100644 Binary files a/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc and b/cflib_groundstation/__pycache__/crazyflie_connection.cpython-38.pyc differ diff --git a/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc b/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc index 704af640fbe957aa1ab2b02b8e7b8d6685ee8597..8992ae1d5dc36d7cf4ca436430a295654b520bb4 100644 Binary files a/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc and b/cflib_groundstation/__pycache__/groundstation_socket.cpython-38.pyc differ diff --git a/cflib_groundstation/groundstation_socket.py b/cflib_groundstation/groundstation_socket.py index 803726dc388e6779d0ff385704b625e8c89b6632..edbf47da776f90b9860b55489044f33768d393b4 100644 --- a/cflib_groundstation/groundstation_socket.py +++ b/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) + + diff --git a/cflib_groundstation/main.py b/cflib_groundstation/main.py index 76e489c3ccd8114f6f0e443c8ad899dad27fe59c..4f6d633d7bfbfd1668c30fdda3d454b7d6bb4e71 100644 --- a/cflib_groundstation/main.py +++ b/cflib_groundstation/main.py @@ -1,7 +1,7 @@ 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) +