diff --git a/OBSW_Config.py b/OBSW_Config.py
index 24b7e34200ff8d78edaca0d3394ae03880dc2927..6c5c4ddb240d6be637b68b3c49f9e3bd05947230 100644
--- a/OBSW_Config.py
+++ b/OBSW_Config.py
@@ -21,6 +21,7 @@ displayMode = "long"
 comIF = 0
 # COM Port for serial communication
 comPort = 'COM0'
+serialTimeout = 0.003
 # Time related
 tmTimeout = 10
 tcSendTimeoutFactor = 2.0
diff --git a/OBSW_UdpClient.py b/OBSW_UdpClient.py
index 0c650aba05ce14e664df8cd7a93edd1fe6c2d477..4037c4466761e6c7939e0b847cab626dd97b4166 100644
--- a/OBSW_UdpClient.py
+++ b/OBSW_UdpClient.py
@@ -98,7 +98,8 @@ def main():
     else:
         comPort = g.comPort
         baudRate = 115200
-        communicationInterface = SerialComIF(tmtcPrinter, comPort, baudRate, 0.007)
+        g.serialTimeout = 0.03
+        communicationInterface = SerialComIF(tmtcPrinter, comPort, baudRate, g.serialTimeout)
     connectToBoard()
 
     if g.modeId == "ListenerMode":
@@ -126,7 +127,8 @@ def main():
         SenderAndReceiver.sendQueueTcAndReceiveTmSequentially()
 
     elif g.modeId == "OBSWUnitTest":
-        communicationInterface.serial.close()
+        if g.comIF == 1:
+            communicationInterface.serial.close()
         # Set up test suite and run it with runner
         # Verbosity specifies detail level
         suite = unittest.TestLoader().loadTestsFromModule(OBSW_UnitTest)
diff --git a/comIF/OBSW_ComInterface.py b/comIF/OBSW_ComInterface.py
index 161e579b65ddeb322fc602a30d510545f8174d02..cc519f99e4a79a2d80f42e0ea44a9bd1c2de425e 100644
--- a/comIF/OBSW_ComInterface.py
+++ b/comIF/OBSW_ComInterface.py
@@ -22,7 +22,8 @@ class CommunicationInterface:
     # Receive telemetry and process it
     @abstractmethod
     def receiveTelemetry(self, parameters=0):
-        pass
+        packetList = []
+        return packetList
 
     # Poll the interface  for data
     @abstractmethod
diff --git a/comIF/OBSW_Ethernet_ComIF.py b/comIF/OBSW_Ethernet_ComIF.py
index a65c2416378a9f38e06b79de7e47db4af1fb8609..dacb009b0ffd86449a1367ae6ad86bfe1701af4d 100644
--- a/comIF/OBSW_Ethernet_ComIF.py
+++ b/comIF/OBSW_Ethernet_ComIF.py
@@ -52,7 +52,8 @@ class EthernetComIF(CommunicationInterface):
         data = self.recvSocket.recvfrom(1024)[0]
         packet = PUSTelemetryFactory(data)
         self.tmtcPrinter.printTelemetry(packet)
-        return packet
+        packetList = [packet]
+        return packetList
 
     def receiveTelemetryAndStoreIntoQueue(self, tmQueue):
         packet = self.receiveTelemetry()
diff --git a/comIF/OBSW_Serial_ComIF.py b/comIF/OBSW_Serial_ComIF.py
index 7f88b4befc7050347fbf4f8eb9ab7b46960dd771..722394d9ac336cbd18b688992d1bfd3cdf3b9130 100644
--- a/comIF/OBSW_Serial_ComIF.py
+++ b/comIF/OBSW_Serial_ComIF.py
@@ -18,15 +18,18 @@ class SerialComIF(CommunicationInterface):
         self.serial.write(tcPacket)
 
     def receiveTelemetry(self, parameters=0):
-        packet = self.pollInterface()
-        return packet
+        packetList = self.pollInterface()
+        return packetList
 
     def pollInterface(self, parameter=0):
         if self.dataAvailable():
-            data = self.pollPusPacket()
-            packet = PUSTelemetryFactory(data)
-            self.tmtcPrinter.printTelemetry(packet)
-            return packet
+            pusDataList, numberOfPackets = self.pollPusPackets()
+            packetList = []
+            for counter in range(0, numberOfPackets):
+                packet = PUSTelemetryFactory(pusDataList[counter])
+                self.tmtcPrinter.printTelemetry(packet)
+                packetList.append(packet)
+            return packetList
 
     def dataAvailable(self, timeout=0):
         if self.serial.in_waiting > 0:
@@ -40,13 +43,32 @@ class SerialComIF(CommunicationInterface):
                 elapsed_time = time.time() - start_time
             return False
 
-    def pollPusPacket(self):
-        pusData = self.serial.read(1024)
-        dataFieldSize = (pusData[4] << 8 | pusData[5]) + 1
-        readSize = len(pusData)
-        if readSize < 6 + dataFieldSize:
-            print("Serial Com IF: Size missmatch when polling PUS packet")
-        return pusData
+    def pollPusPackets(self):
+        pusDataList = []
+        data = self.serial.read(1024)
+        packetSize = (data[4] << 8 | data[5]) + 7
+        readSize = len(data)
+        numberOfPackets = 1
+        if readSize < packetSize:
+            print("Serial Com IF: Size missmatch when polling PUS packet. Packet Size: " +
+                  str(packetSize) + ". Read Size: " + str(readSize) + ". Check timeout too")
+        if readSize > packetSize:
+            endOfBuffer = readSize - 1
+            endIndex = packetSize
+            startIndex = 0
+            pusData = data[startIndex:endIndex]
+            pusDataList.append(pusData)
+            while endIndex < endOfBuffer:
+                startIndex = endIndex
+                endIndex = endIndex + 6
+                nextPacketSize = (data[endIndex-1] << 8 | data[endIndex]) + 7
+                endIndex = startIndex + nextPacketSize
+                numberOfPackets = numberOfPackets + 1
+                pusData = data[startIndex:endIndex]
+                pusDataList.append(pusData)
+        else:
+            pusDataList.append(data)
+        return pusDataList, numberOfPackets
 
     def performListenerMode(self):
         print("Listening for packages ...")
diff --git a/sendreceive/OBSW_MultipleCommandsSenderReceiver.py b/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
index 68ac322660b7ee1dc4fa97d8594c24bded9f6398..c94f44a2c0743efe31258c6b63cee9cb12cfb3a1 100644
--- a/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
+++ b/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
@@ -86,9 +86,10 @@ class MultipleCommandSenderReceiver(SequentialCommandSenderReceiver):
             self.allRepliesReceived = True
 
     def receiveTelemetryAndStoreInformation(self):
-        packet = self.comInterface.receiveTelemetry()
-        tmInfo = packet.packTmInformation()
-        self.tmInfoQueue.put(tmInfo)
+        packetList = self.comInterface.receiveTelemetry()
+        for counter in range(0, len(packetList)):
+            tmInfo = packetList[counter].packTmInformation()
+            self.tmInfoQueue.put(tmInfo)
 
     def handleLastRepliesListening(self, start_time):
         elapsed_time_seconds = 0
diff --git a/sendreceive/OBSW_SingleCommandSenderReceiver.py b/sendreceive/OBSW_SingleCommandSenderReceiver.py
index 4b025b7fcdaf2ae202eda8cb62ac111b07ba93f0..3dc54d7ba2ec7f47aeef83fd6c066284dfa59616 100644
--- a/sendreceive/OBSW_SingleCommandSenderReceiver.py
+++ b/sendreceive/OBSW_SingleCommandSenderReceiver.py
@@ -28,7 +28,6 @@ class SingleCommandSenderReceiver(CommandSenderReceiver):
             # wait until reply is received
             time.sleep(3)
         if self.replyReceived:
-            print("Listening for packages ...")
             self.comInterface.performListenerMode()
 
     # runs in separate thread. sends TC again if no TM is received after timeout
diff --git a/tc/OBSW_TcService200.py b/tc/OBSW_TcService200.py
index 995fcd3a198e063839ccf24d94bb0c575a52ecc3..ba4a2dc9b23daeed3c453909a76d7996a6f315c1 100644
--- a/tc/OBSW_TcService200.py
+++ b/tc/OBSW_TcService200.py
@@ -1,6 +1,6 @@
 # -*- coding: utf-8 -*-
 """
-Program: OBSW_UnitTest.py
+Program: OBSW_TcService200.py
 Date: 01.11.2019
 Description: PUS Custom Service 200: Mode Commanding
 
@@ -16,21 +16,26 @@ import struct
 
 
 def packService200TestInto(tcQueue):
+    tcQueue.put(("print", "Testing Service 200"))
     # Object ID: Dummy Device
     objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
     # Set On Mode
+    tcQueue.put(("print", "\r\nTesting Service 200: Set Mode On"))
     modeData = packModeData(objectId, 1, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=2000, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # Set Normal mode
+    tcQueue.put(("print", "\r\nTesting Service 200: Set Mode Normal"))
     modeData = packModeData(objectId, 2, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=2010, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # Set Raw Mode
+    tcQueue.put(("print", "\r\nTesting Service 200: Set Mode Raw"))
     modeData = packModeData(objectId, 3, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=2020, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # Set Off Mode
+    tcQueue.put(("print", "\r\nTesting Service 200: Set Mode Off"))
     modeData = packModeData(objectId, 0, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=2030, data=modeData)
     tcQueue.put(command.packCommandTuple())
diff --git a/test/OBSW_UnitTest.py b/test/OBSW_UnitTest.py
index bb698d7fe4997b68018fe589bd647e152b5d4d27..582e12835ecf3def94afc7579750aef72334731c 100644
--- a/test/OBSW_UnitTest.py
+++ b/test/OBSW_UnitTest.py
@@ -62,7 +62,7 @@ class TestService(unittest.TestCase):
                                                    g.sockSend, g.sockReceive, g.sendAddress)
         else:
             baudRate = 115200
-            cls.communicationInterface = SerialComIF(cls.tmtcPrinter, g.comPort, baudRate, 0.004)
+            cls.communicationInterface = SerialComIF(cls.tmtcPrinter, g.comPort, baudRate, g.serialTimeout)
         # connectToBoard()
 
     def performTestingAndGenerateAssertionDict(self):
@@ -180,7 +180,7 @@ class TestService5(TestService):
         super().setUpClass()
         print("Testing Service 5")
         cls.waitIntervals = [1, 2, 3]
-        cls.waitTime = [1.2, 1.5, 1.2]
+        cls.waitTime = [1.2, 1.5, 1.5]
         packService5TestInto(cls.testQueue)
 
     def test_Service5(self):
diff --git a/tm/OBSW_TmPacket.py b/tm/OBSW_TmPacket.py
index 98333c41dd631372256b80ea72567d9133ec431f..e4f02de115a314640eb1d9177d59792db7ec3f9e 100644
--- a/tm/OBSW_TmPacket.py
+++ b/tm/OBSW_TmPacket.py
@@ -197,12 +197,18 @@ class Service200TM(PUSTelemetry):
         super().__init__(byteArray)
         self.isCantReachModeReply = False
         self.isModeReply = False
+        self.printPacketInfo("Mode Reply")
         self.objectId = struct.unpack('>I', self.byteArrayData[0:4])[0]
         if self.dataFieldHeader.subtype == 7:
+            self.appendPacketInfo(": Can't reach mode")
             self.isCantReachModeReply = True
             self.returnValue = self.byteArrayData[4] << 8 | self.byteArrayData[5]
         elif self.dataFieldHeader.subtype == 6 or self.dataFieldHeader.subtype == 8:
             self.isModeReply = True
+            if self.dataFieldHeader.subtype == 8:
+                self.appendPacketInfo(": Wrong Mode")
+            elif self.dataFieldHeader.subtype == 6:
+                self.appendPacketInfo(": Mode reached")
             self.mode = struct.unpack('>I', self.byteArrayData[4:8])[0]
             self.submode = self.byteArrayData[8]
 
diff --git a/utility/OBSW_TmTcPrinter.py b/utility/OBSW_TmTcPrinter.py
index 0157ea6bb5874bb2f0d80ff275b39dd38a0f41ae..a4abbc265ae9718177d234e33b53ba8dc35af3fa 100644
--- a/utility/OBSW_TmTcPrinter.py
+++ b/utility/OBSW_TmTcPrinter.py
@@ -115,6 +115,10 @@ class TmtcPrinter:
         print(self.printBuffer)
         self.addPrintBufferToFileBuffer()
 
+    def printData(self, byteArray):
+        string = self.returnDataString(byteArray)
+        print(string)
+
     @staticmethod
     def returnDataString(byteArray):
         strToPrint = "["