Merge branch 'groundstation_TCP' into 'cflib_groundstation'

Groundstation tcp

See merge request !93

cflib_groundstation/crazyflie_connection.py

+
+
+class CrazyflieConnection():
+    def Debug():
+        raise Exception
+    def PacketLog():
+        raise Exception
+    def GetPacketLogs():
+        raise Exception
+    def Update():
+        raise Exception
+    def BeginUpdate():
+        raise Exception
+    def OverrideOuput(): #TODO
+        raise Exception
+    def GetNodeIds():
+        raise Exception
+    def SetParam(): #TODO
+        raise Exception
+    def GetParam(): #TODO
+        raise Exception
+    def SetSource():
+        raise Exception
+    def GetSource():
+        raise Exception
+    def RespSource():
+        raise Exception
+    def GetOutput():
+        raise Exception
+    def GetNodes():
+        raise Exception
+    def AddNode():
+        raise Exception
+    def GetLogFile(): #TODO
+        raise Exception
+    def LogBlockCommand(): #TODO
+        raise Exception
\ No newline at end of file

cflib_groundstation/groundstation_socket.py

+import socket
+import os
+import collections
+from enum import IntEnum, Enum
+
+# Set the path for the Unix socket
+socket_path = './cflib_groundstation.socket'
+
+class GroundstationSocket():
+    metadata = collections.namedtuple("metadata", ["msg_type", "msg_id", "data_len"])
+    def groundstation_connect(self):
+        # remove the socket file if it already exists
+        try:
+            os.unlink(socket_path)
+        except OSError:
+            if os.path.exists(socket_path):
+                raise
+        
+        # Create the Unix socket server
+        server = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)
+        print("Opened groundstation socket.")
+
+        # Bind the socket to the path
+        server.bind(socket_path)
+
+        # Listen for incoming connections
+        server.listen(1)
+
+        # accept connections
+        print('Server is listening for incoming connections...')
+        self.connection, client_address = server.accept()
+
+        try:
+            print('Connection from', str(self.connection).split(", ")[0][-4:])
+
+            # receive data from the client
+            while True:
+                data = self.connection.recv(1000)
+                if not data:
+                    break
+                #print('Received data:' + data.decode())
+                dataarray = bytearray(data)
+                message = self.decodePacket(data, 256, len(data))
+                if message == None:
+                    print("There was an error decoding the packet")
+                    break
+
+                #TODO: Add the message to the main function queue, which will
+                #which will send the appropriate command to the quad.
+        finally:
+            # close the connection
+            self.connection.close()
+            # remove the socket file
+            os.unlink(socket_path)
+
+    def sendToBackend(self, message):
+        if self.connection != None:
+            print("Can't send message, not connected to Backend")
+            return
+        self.connection.send(message)
+    
+    def decodePacket(self, data, data_size, packet_size):
+        print("decoding")
+        if data[PacketHeader.BEGIN.value] != Message.BEGIN_CHAR.value:
+            return
+        if len(data) < PacketHeader.HDR_SIZE.value + ChecksumFormat.CSUM_SIZE.value:
+            return
+        
+        messagedata = {
+            "msg_type": int.from_bytes(data[PacketHeader.MTYPE_L.value:PacketHeader.MTYPE_H.value], 'big'),
+            "msg_id": int.from_bytes(data[PacketHeader.ID_L.value:PacketHeader.ID_H.value], 'big'),
+            "data_len": int.from_bytes(data[PacketHeader.DLEN_L.value:PacketHeader.DLEN_H.value], 'big'),
+        }
+        if packet_size < PacketHeader.HDR_SIZE.value + messagedata["data_len"] + ChecksumFormat.CSUM_SIZE.value:
+            return
+        if data_size < messagedata["data_len"]:
+            return
+        checkSum = self.packetChecksum(data, PacketHeader.HDR_SIZE.value + messagedata["data_len"] + ChecksumFormat.CSUM_SIZE.value)
+        if checkSum != data[PacketHeader.HDR_SIZE.value + messagedata["data_len"]]:
+            return
+        messagedata["data"] = data[PacketHeader.HDR_SIZE.value:PacketHeader.HDR_SIZE.value + messagedata["data_len"]]
+        print("Data: " + str(messagedata))
+        return messagedata
+
+    def packetChecksum(self, data, packet_size):
+        checkSum = 0
+        for i in range(0, packet_size - ChecksumFormat.CSUM_SIZE.value):
+            checkSum ^= data[i]
+        return checkSum
+
+    def EncodePacket(self, messagedata):
+        print("encoding packet")
+
+
+
+#Enums necessary for packet decoding/encoding
+class PacketHeader(IntEnum):
+    BEGIN = 0
+    MTYPE_L = 1
+    MTYPE_H = 2
+    ID_L = 3
+    ID_H = 4
+    DLEN_L = 5
+    DLEN_H = 6
+    HDR_SIZE = 7
+
+class ChecksumFormat(IntEnum):
+    CSUM_L = 0
+    CSUM_SIZE = 1
+
+class Message(Enum):
+    BEGIN_CHAR = 0xBE
+    END_CHAR = 0xED
+
+# Enumeration of the data types that a callback function may use
+# doubleType should get added here at some point
+class DataType(IntEnum):
+    floatType = 0
+    intType = 0
+    stringType = 2
+"""
+  Message type IDs used to know what kind of message we are dealing with
+  Enumeration used to index the MessageTypes array in commands.c
+
+  DO NOT change items in the middle of this enum or you will break backwards compatibility.
+  Add new message types in the slot between MAX_TYPE_ID and the one before it
+  DO NOT change this enum without also updating the "MessageTypes" array
+  in commands.c to match.
+ """
+class MessageTypeID(Enum):
+    GETPACKETLOGS_ID = 2
+    UPDATE_ID = 3
+    BEGINUPDATE_ID = 4
+    LOG_ID = 5
+    LOG_END_ID = 6
+    SETPARAM_ID = 7
+    GETPARAM_ID = 8
+    RESPPARAM_ID = 9
+    SETSOURCE_ID = 10
+    GETSOURCE_ID = 11
+    RESPSOURCE_ID = 12
+    GETOUTPUT_ID = 13
+    RESPOUTPUT_ID = 14
+    GETNODES_ID = 15
+    RESPNODES_ID = 16
+    ADDNODE_ID = 17
+    RESPADDNODE_ID = 18
+    OUTPUT_OVERRIDE_ID = 19
+    SEND_RT_ID = 20
+    MAX_TYPE_ID = 21
+
+if __name__ == '__main__':
+    GroundstationSocket().groundstation_connect()
+
+

cflib_groundstation/main.py

+
+
+
+class main():
+    print("main")
\ No newline at end of file

groundStation/src/backend/config.c

@@ -23,8 +23,8 @@ trackable_t trackables[] = {
         0,
         //Adapter Variables
         1,
-        "./adapters/crazyflie/", 
-        "./adapters/crazyflie/cf_adapter.socket", 
+        ".,/cflib_groundstation/", 
+        "../cflib_groundstation/cflib_groundstation.socket", 
         NULL,
         //Local Communication Variables
         0, //Set to 1 if Intending to use local Fifo