diff --git a/.gitignore b/.gitignore
index 6a2160e6a4af03358cfeebef7471e2753e105836..83e1518665dce88453afcc1a0d3f025daa293703 100644
--- a/.gitignore
+++ b/.gitignore
@@ -11,16 +11,17 @@ _dependencies
 
 .idea
 .idea/*
+__pycache__
 
 .project
 .cproject 
 
-__pycache__
-
 *.lnk
 *.tmp
 *.open
 *.ini
 
 generators/*.csv
-/AT91/
+
+*tmtc*.txt
+log
\ No newline at end of file
diff --git a/OBSW_Config.py b/OBSW_Config.py
index 7dfe133b9cbf5e65e1cd12f5e6e090cd42d405cf..c9712dc2326ea5d1e0614b6396802a8bb6faaaf2 100644
--- a/OBSW_Config.py
+++ b/OBSW_Config.py
@@ -17,13 +17,14 @@ modeId = 0
 service = 17
 displayMode = "long"
 
+comIF = 0
 # Time related
 tmTimeout = 10
 tcSendTimeoutFactor = 2.0
 
 # Ethernet connection settings
 recAddress = 0
-sendAddress = 0
+sendAddress = (0, 0)
 sockSend = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 sockReceive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 
@@ -31,3 +32,8 @@ sockReceive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 printToFile = False
 printRawTmData = False
 
+
+def keyboardInterruptHandler():
+    print("Disconnect registered")
+    disconnect = bytearray([0, 0, 0, 0, 0])
+    sockSend.sendto(disconnect, sendAddress)
diff --git a/OBSW_UdpClient.py b/OBSW_UdpClient.py
index 80e05e9909d8b802fc99ef19346d7ed9520d4f4a..5e6862d960b6b24a962fde38d21fb66731a4c333 100644
--- a/OBSW_UdpClient.py
+++ b/OBSW_UdpClient.py
@@ -54,6 +54,7 @@
 
 import sys
 import atexit
+import signal
 import queue
 import socket
 import unittest
@@ -65,6 +66,9 @@ from tc.OBSW_TcPacker import PUSTelecommand, createTotalTcQueue, serviceTestSele
 from sendreceive.OBSW_CommandSenderReceiver import CommandSenderReceiver, connectToBoard
 from sendreceive.OBSW_SingleCommandSenderReceiver import SingleCommandSenderReceiver
 from sendreceive.OBSW_SequentialSenderReceiver import SequentialCommandSenderReceiver
+from utility.OBSW_TmTcPrinter import TmtcPrinter
+from comIF.OBSW_Ethernet_ComIF import EthernetComIF
+from comIF.OBSW_Serial_ComIF import SerialComIF
 
 # Mode options, set by args parser
 modeList = {
@@ -77,35 +81,48 @@ modeList = {
 }
 
 
+# noinspection PyTypeChecker
 def main():
     args = parseInputArguments()
     setGlobals(args)
+    if g.modeId == "GUIMode":
+        print("GUI not implemented yet")
+        exit()
+
     setUpSocket()
+    atexit.register(g.keyboardInterruptHandler)
     print("Attempting to connect")
+    tmtcPrinter = TmtcPrinter(g.displayMode, g.printToFile, True)
+    if g.comIF == 0:
+        communicationInterface = EthernetComIF(tmtcPrinter, g.tmTimeout, g.tcSendTimeoutFactor,
+                                               g.sockSend, g.sockReceive, g.sendAddress)
+    else:
+        comPort = 'COM8'
+        baudRate = 9600
+        communicationInterface = SerialComIF(tmtcPrinter, comPort, baudRate, 0.2)
     connectToBoard()
 
-    if g.modeId == "GUIMode":
-        pass
-    elif g.modeId == "ListenerMode":
-        Receiver = CommandSenderReceiver(g.displayMode, g.tmTimeout, g.tcSendTimeoutFactor, g.printToFile)
-        Receiver.performListenerMode()
+    if g.modeId == "ListenerMode":
+        Receiver = CommandSenderReceiver(communicationInterface, tmtcPrinter, g.tmTimeout, g.tcSendTimeoutFactor,
+                                         g.printToFile)
+        Receiver.comInterface.performListenerMode()
 
     elif g.modeId == "SingleCommandMode":
         pusPacketTuple = commandPreparation()
-        SenderAndReceiver = SingleCommandSenderReceiver(g.displayMode, pusPacketTuple, g.tmTimeout,
-                                                        g.tcSendTimeoutFactor, g.printToFile)
+        SenderAndReceiver = SingleCommandSenderReceiver(communicationInterface, tmtcPrinter, pusPacketTuple,
+                                                        g.tmTimeout, g.tcSendTimeoutFactor, g.printToFile)
         SenderAndReceiver.sendSingleTcAndReceiveTm()
 
     elif g.modeId == "ServiceTestMode":
         serviceQueue = queue.Queue()
-        SenderAndReceiver = SequentialCommandSenderReceiver(g.displayMode, g.tmTimeout,
+        SenderAndReceiver = SequentialCommandSenderReceiver(communicationInterface, tmtcPrinter, g.tmTimeout,
                                                             serviceTestSelect(g.service, serviceQueue),
                                                             g.tcSendTimeoutFactor, g.printToFile)
         SenderAndReceiver.sendQueueTcAndReceiveTmSequentially()
 
     elif g.modeId == "SoftwareTestMode":
         allTcQueue = createTotalTcQueue()
-        SenderAndReceiver = SequentialCommandSenderReceiver(g.displayMode, g.tmTimeout,
+        SenderAndReceiver = SequentialCommandSenderReceiver(communicationInterface, tmtcPrinter, g.tmTimeout,
                                                             allTcQueue, g.tcSendTimeoutFactor, g.printToFile)
         SenderAndReceiver.sendQueueTcAndReceiveTmSequentially()
 
@@ -135,8 +152,10 @@ def parseInputArguments():
                            '0: GUI Mode, 1:Listener Mode, '
                            '2: Single Command Mode, 3: Service Test Mode, '
                            '4: Software Test Mode, 5: Unit Test Mode ', default=0)
-    argParser.add_argument('-c', '--clientIP', help='Client(Computer) IP. Default:\'\'', default='')
-    argParser.add_argument('-b', '--boardIP', help='Board IP. Default: 169.254.1.38', default='169.254.1.38')
+    argParser.add_argument('-c', '--comIF', type=int, help='Communication Interface. 0 for Ethernet, 1 for Serial',
+                           default=0)
+    argParser.add_argument('--clientIP', help='Client(Computer) IP. Default:\'\'', default='')
+    argParser.add_argument('--boardIP', help='Board IP. Default: 169.254.1.38', default='169.254.1.38')
     argParser.add_argument('-s', '--service',  help='Service to test. Default: 17', default=17)
     argParser.add_argument('-t', '--tmTimeout', type=float, help='TM Timeout. Default: 10)', default=10.0)
     argParser.add_argument('-p', '--printFile', help='Supply -p to print output to file. Default: False',
@@ -177,6 +196,7 @@ def setGlobals(args):
         service = args.service
     g.recAddress = recAddress
     g.sendAddress = sendAddress
+    g.comIF = args.comIF
     g.sockReceive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     g.sockSend = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     g.modeId = modeId
@@ -192,13 +212,18 @@ def setUpSocket():
     g.sockReceive.setblocking(False)
 
 
-def keyboardInterruptHandler():
-    print("The End !")
-    disconnect = bytearray([0, 0, 0, 0, 0])
-    g.sockSend.sendto(disconnect, g.sendAddress)
+class GracefulKiller:
+    kill_now = False
+
+    def __init__(self):
+        signal.signal(signal.SIGINT, self.exit_gracefully)
+        signal.signal(signal.SIGTERM, self.exit_gracefully)
+
+    def exit_gracefully(self):
+        self.kill_now = True
+        print("I was killed")
 
 
 if __name__ == "__main__":
-    atexit.register(keyboardInterruptHandler)
     main()
 
diff --git a/comIF/OBSW_ComInterface.py b/comIF/OBSW_ComInterface.py
new file mode 100644
index 0000000000000000000000000000000000000000..a113c443a0ae01cbb327704752eaaa795a00bf1d
--- /dev/null
+++ b/comIF/OBSW_ComInterface.py
@@ -0,0 +1,51 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_ComInterface.py
+Date: 01.11.2019
+Description: Generic Communication Interface. Defines the syntax of the communication functions.
+             Abstract methods must be implemented by child class (e.g. Ethernet Com IF)
+
+@author: R. Mueller
+"""
+from abc import abstractmethod
+
+
+class CommunicationInterface:
+    def __init__(self, tmtcPrinter):
+        self.tmtcPrinter = tmtcPrinter
+
+    # Send Telecommand
+    @abstractmethod
+    def sendTelecommand(self, tcPacket, tcPacketInfo=""):
+        pass
+
+    # Receive telemetry and process it
+    @abstractmethod
+    def receiveTelemetry(self, parameters):
+        pass
+
+    # Poll the interface  for data
+    @abstractmethod
+    def pollInterface(self, parameters):
+        pass
+
+    # Check whether data is available
+    @abstractmethod
+    def dataAvailable(self, parameters):
+        pass
+
+    # Listen for packets
+    @abstractmethod
+    def performListenerMode(self):
+        pass
+
+    # Receive Telemetry and store it into a queue
+    def receiveTelemetryAndStoreIntoQueue(self, tmQueue):
+        pass
+
+    # Receive Telemetry and store a tuple consisting of TM information and TM packet into queue
+    def receiveTelemetryAndStoreTuple(self, tmTupleQueue):
+        pass
+
+
+
diff --git a/comIF/OBSW_Ethernet_ComIF.py b/comIF/OBSW_Ethernet_ComIF.py
new file mode 100644
index 0000000000000000000000000000000000000000..b2095bb6e473cac17eb838b1006496a72880c28f
--- /dev/null
+++ b/comIF/OBSW_Ethernet_ComIF.py
@@ -0,0 +1,70 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_UnitTest.py
+Date: 01.11.2019
+Description: Ethernet Communication Interface
+
+@author: R. Mueller
+"""
+
+
+from comIF.OBSW_ComInterface import CommunicationInterface
+from tm.OBSW_TmPacket import PUSTelemetryFactory
+import select
+
+
+class EthernetComIF(CommunicationInterface):
+
+    def __init__(self, tmtcPrinter, tmTimeout, tcTimeoutFactor, sendSocket, recvSocket, sendAddress):
+        super().__init__(tmtcPrinter)
+        self.tmTimeout = tmTimeout
+        self.tcTimeoutFactor = tcTimeoutFactor
+        self.sendSocket = sendSocket
+        self.recvSocket = recvSocket
+        self.sendAddress = sendAddress
+
+    def sendTelecommand(self, tcPacket, tcPacketInfo=""):
+        self.tmtcPrinter.printTelecommand(tcPacket, tcPacketInfo)
+        self.sendSocket.sendto(tcPacket, self.sendAddress)
+
+    def performListenerMode(self):
+        pollTimeout = 10
+        while True:
+            print("Listening for packages ...")
+            self.pollInterface(pollTimeout)
+
+    def dataAvailable(self, timeout):
+        ready = select.select([self.recvSocket], [], [], timeout)
+        if ready is None:
+            return False
+        elif ready[0]:
+            return ready
+
+    def pollInterface(self, pollTimeout):
+        ready = self.dataAvailable(pollTimeout)
+        if ready is False:
+            pass
+        elif ready[0]:
+            packet = self.receiveTelemetry()
+            self.tmtcPrinter.printTelemetry(packet)
+
+    def receiveTelemetry(self, parameters=0):
+        data = self.recvSocket.recvfrom(1024)[0]
+        packet = PUSTelemetryFactory(data)
+        self.tmtcPrinter.printTelemetry(packet)
+        return packet
+
+    def receiveTelemetryAndStoreIntoQueue(self, tmQueue):
+        packet = self.receiveTelemetry()
+        tmQueue.put(packet)
+        return tmQueue
+
+    def receiveTelemetryAndStoreTuple(self, tmTupleQueue):
+        data = self.recvSocket.recvfrom(1024)[0]
+        tmInfo = PUSTelemetryFactory(data).packTmInformation()
+        packet = PUSTelemetryFactory(data)
+        self.tmtcPrinter.printTelemetry(packet)
+        tmTuple = (packet, tmInfo)
+        tmTupleQueue.put(tmTuple)
+        return tmTuple
+
diff --git a/comIF/OBSW_Serial_ComIF.py b/comIF/OBSW_Serial_ComIF.py
new file mode 100644
index 0000000000000000000000000000000000000000..949976eef23025746e2e82ff1e6f577dcd8b0746
--- /dev/null
+++ b/comIF/OBSW_Serial_ComIF.py
@@ -0,0 +1,72 @@
+import time
+
+from comIF.OBSW_ComInterface import CommunicationInterface
+import serial
+
+from tm.OBSW_TmPacket import PUSTelemetryFactory
+
+
+class SerialComIF(CommunicationInterface):
+    def __init__(self, tmtcPrinter, comPort, baudRate, serialTimeout):
+        super().__init__(tmtcPrinter)
+        self.comPort = comPort
+        self.baudRate = baudRate
+        self.tmtcPrinter = tmtcPrinter
+        # timeout in seconds
+        serialTimeout = 0.2
+        self.serial = serial.Serial(port=comPort, baudrate=self.baudRate, timeout=serialTimeout)
+        # self.serial.open()
+
+    def sendTelecommand(self, tcPacket, tcPacketInfo=""):
+        self.tmtcPrinter.printTelecommand(tcPacket, tcPacketInfo)
+        self.serial.write(tcPacket)
+
+    def receiveTelemetry(self, data):
+        packet = self.pollInterface()
+        return packet
+
+    def pollInterface(self, parameter=0):
+        if self.dataAvailable():
+            data = self.pollPusPacket()
+            packet = PUSTelemetryFactory(data)
+            return packet
+
+    def dataAvailable(self, timeout=0):
+        if timeout > 0:
+            start_time = time.time()
+            elapsed_time = 0
+            while elapsed_time < timeout:
+                if self.serial.in_waiting > 0:
+                    print("data available!")
+                    print(self.serial.in_waiting)
+                    return True
+                elapsed_time = time.time() - start_time
+            return False
+        elif self.serial.in_waiting > 0:
+            print("data available!")
+            print(self.serial.in_waiting)
+            return True
+
+    def pollPusPacket(self):
+        data = bytearray()
+        pusHeader = self.serial.read(6)
+        dataFieldSize = data[5] << 8 | data[6] - 1
+        pusData = self.serial.read(dataFieldSize)
+        data = data + pusHeader
+        data = data + pusData
+        if len(data) < 6 + dataFieldSize:
+            print("Serial Com IF: Size missmatch when polling PUS packet")
+        return data
+
+    def performListenerMode(self):
+        while True:
+            self.pollInterface()
+
+    def receiveTelemetryAndStoreIntoQueue(self, tmQueue):
+        packet = self.pollInterface()
+        tmQueue.put(packet)
+
+    def receiveTelemetryAndStoreTuple(self, tmTupleQueue):
+        packet = self.pollInterface()
+        tmInfo = packet.packTmInformation()
+        tmTupleQueue.put(packet, tmInfo)
diff --git a/comIF/__init__.py b/comIF/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/gui/OBSW_TmtcGUI.py b/gui/OBSW_TmtcGUI.py
index 09599a96f96d213b85d7e79c1171c60430b43d7b..a64eca3168473920b38cd7a439772df54c0a1ace 100644
--- a/gui/OBSW_TmtcGUI.py
+++ b/gui/OBSW_TmtcGUI.py
@@ -14,7 +14,6 @@
     R. Mueller
 """
 from tkinter import *
-import OBSW_Config as g
 
 # A first simple version has drop down menus to chose all necessary options
 # which are normally handled by the args parser.
diff --git a/sendreceive/OBSW_CommandSenderReceiver.py b/sendreceive/OBSW_CommandSenderReceiver.py
index 7c8259b3abde80d4bbacb47cb314489a0b297130..41afdaaeeeeb918b1a06a2ee5e78e85d4edcf3f5 100644
--- a/sendreceive/OBSW_CommandSenderReceiver.py
+++ b/sendreceive/OBSW_CommandSenderReceiver.py
@@ -13,95 +13,85 @@ This is still experimental.
 @author: R. Mueller
 """
 import OBSW_Config as g
-import select
 import time
-from utility.OBSW_TmTcPrinter import TmtcPrinter
-from tm.OBSW_TmPacket import PUSTelemetryFactory
 
 
 # Generic TMTC SendReceive class which is implemented
 # by specific implementations (e.g. SingleCommandSenderReceiver)
 class CommandSenderReceiver:
-    def __init__(self, displayMode, tmTimeout, tcSendTimeoutFactor, doPrintToFile):
-        self.displayMode = displayMode
-        # TODO: Make this variable modifiable !
-        self.timeoutInSeconds = tmTimeout
+    def __init__(self, comInterface, tmtcPrinter, tmTimeout, tcSendTimeoutFactor, doPrintToFile):
+        self.tmTimeout = tmTimeout
+
+        self.comInterface = comInterface
+        self.tmtcPrinter = tmtcPrinter
+
         self.replyReceived = False
         self.tmReady = False
         self.pusPacketInfo = []
         self.pusPacket = []
-        self.tmtcPrinter = TmtcPrinter(self.displayMode, doPrintToFile)
+
         self.start_time = 0
         self.elapsed_time = 0
         self.timeoutCounter = 0
         self.tcSendTimeoutFactor = tcSendTimeoutFactor
         self.doPrintToFile = doPrintToFile
+        self.queueEntryIsTelecommand = True
 
     # checks for replies. if no reply is received, send telecommand again
     def checkForFirstReply(self):
-        self.tmReady = select.select([g.sockReceive], [], [], self.timeoutInSeconds * self.tcSendTimeoutFactor)
-        if self.tmReady[0]:
-            self.checkForOneTelemetrySequence()
+        success = self.checkForOneTelemetrySequence()
+        if success:
             self.replyReceived = True
         else:
             if len(self.pusPacket) == 0:
-                print("No command has been sent yet")
+                print("Command Sender Receiver: No command has been sent yet")
             else:
-                self.sendTelecommand()
-
-    # check for one sequence of replies for a telecommand (e.g. TM[1,1] , TM[1,7] ...)
-    def checkForOneTelemetrySequence(self):
-        while self.tmReady[0]:
-            packet = self.receiveTelemetry()
-            self.tmtcPrinter.printTelemetry(packet)
-            self.tmReady = select.select([g.sockReceive], [], [], self.timeoutInSeconds / 1.5)
-
-    def sendTelecommand(self):
-        self.tmtcPrinter.displaySentCommand(self.pusPacketInfo, self.pusPacket, self.displayMode)
-        g.sockSend.sendto(self.pusPacket, g.sendAddress)
-
-    def performListenerMode(self):
-        timeoutInSeconds = 10
-        while True:
-            print("Listening for packages ...")
-            ready = select.select([g.sockReceive], [], [], timeoutInSeconds)
-            if ready[0]:
-                packet = self.receiveTelemetry()
-                self.tmtcPrinter.printTelemetry(packet)
+                self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
+
+    # Check for special queue entries.
+    def checkQueueEntry(self, tcQueueEntry):
+        (self.pusPacketInfo, self.pusPacket) = tcQueueEntry
+        self.queueEntryIsTelecommand = False
+        if self.pusPacketInfo == "wait":
+            waitTime = self.pusPacket
+            time.sleep(waitTime)
+        elif self.pusPacketInfo == "print":
+            printString = self.pusPacket
+            self.tmtcPrinter.printString(printString)
+        elif self.pusPacketInfo == "export":
+            exportName = self.pusPacket
+            if self.doPrintToFile:
+                self.tmtcPrinter.printToFile(exportName, True)
+        else:
+            self.queueEntryIsTelecommand = True
 
     def checkForTimeout(self):
         if self.timeoutCounter == 5:
-            print("No response from command !")
+            print("Command Sender Receiver: No response from command !")
             exit()
         if self.start_time != 0:
             self.elapsed_time = time.time() - self.start_time
-        if self.elapsed_time > self.timeoutInSeconds * self.tcSendTimeoutFactor:
-            print("Timeout ! Sending Telecommand again")
-            self.sendTelecommand()
+        if self.elapsed_time > self.tmTimeout * self.tcSendTimeoutFactor:
+            print("Command Sender Receiver: Timeout, sending Telecommand again")
+            self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
             self.timeoutCounter = self.timeoutCounter + 1
             self.start_time = time.time()
         time.sleep(3)
 
-    @staticmethod
-    def receiveTelemetry():
-        data = g.sockReceive.recvfrom(1024)[0]
-        packet = PUSTelemetryFactory(data)
-        return packet
-
-    @staticmethod
-    def receiveTelemetryDeserializeAndStore(tmQueue):
-        data = g.sockReceive.recvfrom(1024)[0]
-        packet = PUSTelemetryFactory(data)
-        tmQueue.put(packet)
-        return tmQueue
-
-    @staticmethod
-    def receiveTelemetryAndStoreTuple(tmTupleQueue):
-        data = g.sockReceive.recvfrom(1024)[0]
-        tmInfo = PUSTelemetryFactory(data).packTmInformation()
-        tmTuple = (data, tmInfo)
-        tmTupleQueue.put(tmTuple)
-        return tmTuple
+    # check for one sequence of replies for a telecommand (e.g. TM[1,1] , TM[1,7] ...)
+    # returns True on success
+    def checkForOneTelemetrySequence(self):
+        tmReady = self.comInterface.dataAvailable(self.tmTimeout * self.tcSendTimeoutFactor)
+        if tmReady is False:
+            return False
+        else:
+            while tmReady:
+                self.comInterface.receiveTelemetry()
+                tmReady = self.comInterface.dataAvailable(self.tmTimeout / 1.5)
+                if tmReady is None:
+                    return False
+                else:
+                    return True
 
 
 def connectToBoard():
diff --git a/sendreceive/OBSW_MultipleCommandsSenderReceiver.py b/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
index 55c62d0f03d1240802d73f3e4d7fb6cbc724f973..7e4f38299ccca385675e73cfb38cb6d718d68967 100644
--- a/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
+++ b/sendreceive/OBSW_MultipleCommandsSenderReceiver.py
@@ -17,88 +17,86 @@ import select
 # Difference to seqential sender: This class can send TCs in bursts. Wait intervals can be specified with
 # wait time between the send bursts. This is generally done in the separate test classes in UnitTest
 class MultipleCommandSenderReceiver(SequentialCommandSenderReceiver):
-    def __init__(self, displayMode, tcQueue, tmTimeout, waitIntervals,
-                 waitTime, printTm, printTc, tcTimeoutFactor, doPrintToFile):
-        super().__init__(displayMode, tmTimeout, tcQueue, tcTimeoutFactor, doPrintToFile)
+    def __init__(self, comInterface, tmtcPrinter, tcQueue, tmTimeout, waitIntervals,
+                 waitTime, printTm, tcTimeoutFactor, doPrintToFile):
+        super().__init__(comInterface, tmtcPrinter, tmTimeout, tcQueue, tcTimeoutFactor, doPrintToFile)
         self.waitIntervals = waitIntervals
         self.waitTime = waitTime
         self.printTm = printTm
-        self.printTc = printTc
         self.tmInfoQueue = queue.Queue()
         self.tcInfoQueue = queue.Queue()
         self.pusPacketInfo = []
         self.pusPacket = []
+        self.waitCounter = 0
 
     def sendTcQueueAndReturnTcInfo(self):
         receiverThread = threading.Thread(target=self.checkForMultipleReplies)
         receiverThread.start()
-        self.sendAllQueue()
+        time.sleep(0.5)
         try:
-            self.handleTcSending()
+            self.sendAllQueue()
+            while not self.allRepliesReceived:
+                time.sleep(0.5)
+            # self.handleTcResending() Turned off for now, not needed
+            if self.doPrintToFile:
+                self.tmtcPrinter.printToFile()
         except (KeyboardInterrupt, SystemExit):
             super().handleInterrupt(receiverThread)
         return self.tcInfoQueue, self.tmInfoQueue
 
-    def handleTcSending(self):
+    def handleTcResending(self):
         while not self.allRepliesReceived:
             if self.tcQueue.empty():
                 if self.start_time == 0:
-                    print("TC queue empty")
                     self.start_time = time.time()
             self.checkForTimeout()
-        if self.doPrintToFile:
-            self.tmtcPrinter.printToFile()
 
     def sendAllQueue(self):
-        waitCounter = 0
         while not self.tcQueue.empty():
             self.sendAndPrintTc()
-            waitCounter = self.handleWaiting(waitCounter)
 
     def sendAndPrintTc(self):
-        (self.pusPacketInfo, self.pusPacket) = self.tcQueue.get()
-        if self.printTc:
-            self.tmtcPrinter.displaySentCommand(self.pusPacketInfo, self.pusPacket, self.displayMode)
-        self.tcInfoQueue.put(self.pusPacketInfo)
-        g.sockSend.sendto(self.pusPacket, g.sendAddress)
+        self.checkQueueEntry(self.tcQueue.get())
+        if self.queueEntryIsTelecommand:
+            self.tcInfoQueue.put(self.pusPacketInfo)
+            self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
+            self.handleWaiting()
 
-    def handleWaiting(self, waitCounter):
-        time.sleep(0.5)
-        waitCounter = waitCounter + 1
-        if waitCounter in self.waitIntervals:
-            time.sleep(self.waitTime)
-        return waitCounter
+    def handleWaiting(self):
+        self.waitCounter = self.waitCounter + 1
+        if self.waitCounter in self.waitIntervals:
+            if isinstance(self.waitTime, list):
+                time.sleep(self.waitTime[self.waitIntervals.index(self.waitCounter)])
+            else:
+                time.sleep(self.waitTime)
 
     def checkForMultipleReplies(self):
         super().checkForMultipleReplies()
 
     def handleReplyListening(self):
-        super().handleReplyListening()
-
-    def handleTelemetrySequence(self):
-        if self.run_event.is_set():
-            self.checkForOneTelemetrySequence()
-            if self.tcQueue.empty():
-                print("Setting flag all reply received in handleTelemetrySequence")
-                self.allRepliesReceived = True
-
-    def handleFirstReplyListening(self):
-        super().handleFirstReplyListening()
-
-    # check for one sequence of replies for a telecommand (e.g. TM[1,1] , TM[1,7] ...)
-    def checkForOneTelemetrySequence(self):
-        while self.tmReady[0]:
+        self.tmReady = select.select([g.sockReceive], [], [], 2.0)
+        if self.tmReady[0]:
             self.receiveTelemetryAndStoreInformation()
-            self.tmReady = select.select([g.sockReceive], [], [], self.timeoutInSeconds / 1.5)
-        print("I checked the sequence")
         if self.tcQueue.empty():
-            print("After checking, the TC queue is empty")
+            print("TC queue empty. Listening for a few more seconds ... ")
+            start_time = time.time()
+            self.handleLastRepliesListening(start_time)
             self.allRepliesReceived = True
 
     def receiveTelemetryAndStoreInformation(self):
-        packet = self.receiveTelemetry()
-        if self.printTm:
-            self.tmtcPrinter.printTelemetry(packet)
+        packet = self.comInterface.receiveTelemetry()
         tmInfo = packet.packTmInformation()
         self.tmInfoQueue.put(tmInfo)
 
+    def handleLastRepliesListening(self, start_time):
+        elapsed_time_seconds = 0
+        while elapsed_time_seconds < self.tmTimeout:
+            elapsed_time_seconds = time.time() - start_time
+            self.tmReady = select.select([g.sockReceive], [], [], 2.0)
+            if self.tmReady[0]:
+                self.receiveTelemetryAndStoreInformation()
+
+
+
+
+
diff --git a/sendreceive/OBSW_SequentialSenderReceiver.py b/sendreceive/OBSW_SequentialSenderReceiver.py
index 0ec83305fbc44d57616c98e29e723afcdb479413..65025e5df3284df5b88d6ecf2b5ca007e68690b8 100644
--- a/sendreceive/OBSW_SequentialSenderReceiver.py
+++ b/sendreceive/OBSW_SequentialSenderReceiver.py
@@ -8,17 +8,15 @@
 @brief
     Used to send multiple TCs in sequence and listen for replies after each sent tc
 """
-from sendreceive.OBSW_CommandSenderReceiver import CommandSenderReceiver, connectToBoard
+from sendreceive.OBSW_CommandSenderReceiver import CommandSenderReceiver
 import threading
-import OBSW_Config as g
 import time
-import select
 
 
 # Specific implementation of CommandSenderReceiver to send multiple telecommands in sequence
 class SequentialCommandSenderReceiver(CommandSenderReceiver):
-    def __init__(self, displayMode, tmTimeout, tcQueue, tcTimeoutFactor, doPrintToFile):
-        super().__init__(displayMode, tmTimeout, tcTimeoutFactor, doPrintToFile)
+    def __init__(self, comInterface, tmtcPrinter, tmTimeout, tcQueue, tcTimeoutFactor, doPrintToFile):
+        super().__init__(comInterface, tmtcPrinter, tmTimeout, tcTimeoutFactor, doPrintToFile)
         self.tcQueue = tcQueue
         self.firstReplyReceived = False
         self.allRepliesReceived = False
@@ -49,7 +47,7 @@ class SequentialCommandSenderReceiver(CommandSenderReceiver):
         if self.doPrintToFile:
             print("Exporting output to log file")
             self.tmtcPrinter.printToFile()
-        self.performListenerMode()
+        self.comInterface.performListenerMode()
 
     def handleInterrupt(self, receiverThread):
         print("Keyboard Interrupt detected")
@@ -59,27 +57,32 @@ class SequentialCommandSenderReceiver(CommandSenderReceiver):
     def performNextTcSend(self):
         # this flag is set in the separate receiver thread too
         if self.replyReceived:
-            self.start_time = time.time()
             self.sendNextTelecommand()
+            self.replyReceived = False
         # just calculate elapsed time if start time has already been set (= command has been sent)
         else:
             self.checkForTimeout()
 
     def sendAndReceiveFirstPacket(self):
-        (self.pusPacketInfo, self.pusPacket) = self.tcQueue.get()
-        self.sendTelecommand()
+        self.checkQueueEntry(self.tcQueue.get())
+        if self.queueEntryIsTelecommand:
+            self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
+        else:
+            self.sendAndReceiveFirstPacket()
 
     def sendNextTelecommand(self):
-        (self.pusPacketInfo, self.pusPacket) = self.tcQueue.get()
-        if self.pusPacketInfo == "wait":
-            waitTime = self.pusPacket
-            time.sleep(waitTime)
+        self.checkQueueEntry(self.tcQueue.get())
+        if self.queueEntryIsTelecommand:
+            self.start_time = time.time()
+            self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
+        elif self.tcQueue.empty():
+            # Special case: Last queue entry is not a Telecommand
+            self.allRepliesReceived = True
         else:
-            self.tmtcPrinter.displaySentCommand(self.pusPacketInfo, self.pusPacket, self.displayMode)
-            self.replyReceived = False
-            g.sockSend.sendto(self.pusPacket, g.sendAddress)
+            self.replyReceived = True
+            self.performNextTcSend()
 
-    # this function runs is a separate thread andchecks for replies. If the first reply has not been received,
+    # this function runs is a separate thread and checks for replies. If the first reply has not been received,
     # it attempts to send the telecommand again.
     # if the tc queue is empty and the last telemetry sequence has been received,
     # a flag is set to transition into listener mode
@@ -87,20 +90,22 @@ class SequentialCommandSenderReceiver(CommandSenderReceiver):
         while not self.allRepliesReceived and self.run_event.is_set():
             # listen for duration timeoutInSeconds for replies
             self.handleReplyListening()
-            self.tmReady = select.select([g.sockReceive], [], [], 5.0)
 
     def handleReplyListening(self):
         if self.firstReplyReceived:
-            if self.tmReady[0]:
-                self.handleTelemetrySequence()
+            tmReady = self.comInterface.dataAvailable(2.0)
+            if tmReady is not None:
+                if tmReady[0]:
+                    self.handleTelemetrySequence()
         else:
             self.handleFirstReplyListening()
 
     def handleTelemetrySequence(self):
         if self.run_event.is_set():
-            self.checkForOneTelemetrySequence()
-            # set this flag so the other thread can send the next telecommand
-            self.replyReceived = True
+            success = self.checkForOneTelemetrySequence()
+            if success:
+                # set this flag so the other thread can send the next telecommand
+                self.replyReceived = True
             if self.tcQueue.empty():
                 self.allRepliesReceived = True
 
diff --git a/sendreceive/OBSW_SingleCommandSenderReceiver.py b/sendreceive/OBSW_SingleCommandSenderReceiver.py
index be0950b0644b9a071357cd33b2e700fc7a26ef4c..3dc54d7ba2ec7f47aeef83fd6c066284dfa59616 100644
--- a/sendreceive/OBSW_SingleCommandSenderReceiver.py
+++ b/sendreceive/OBSW_SingleCommandSenderReceiver.py
@@ -9,28 +9,26 @@
     Used to send single tcs and listen for replies after that
 """
 from sendreceive.OBSW_CommandSenderReceiver import CommandSenderReceiver
-import OBSW_Config as g
 import threading
 import time
 
 
 # Specific implementation of CommandSenderReceiver to send a single telecommand
 class SingleCommandSenderReceiver(CommandSenderReceiver):
-    def __init__(self,displayMode, pusPacketTuple, tmTimeout, tcTimeoutFactor, doPrintToFile):
-        super().__init__(displayMode, tmTimeout, tcTimeoutFactor, doPrintToFile)
+    def __init__(self, comInterface, displayMode, pusPacketTuple, tmTimeout, tcTimeoutFactor, doPrintToFile):
+        super().__init__(comInterface, displayMode, tmTimeout, tcTimeoutFactor, doPrintToFile)
         self.pusPacketTuple = pusPacketTuple
         (self.pusPacketInfo, self.pusPacket) = self.pusPacketTuple
 
     def sendSingleTcAndReceiveTm(self):
-        self.tmtcPrinter.displaySentCommand(self.pusPacketInfo, self.pusPacket, self.displayMode)
         print("Starting listener thread")
         threading.Thread(target=self.receiveReply).start()
-        g.sockSend.sendto(self.pusPacket, g.sendAddress)
+        self.comInterface.sendTelecommand(self.pusPacket, self.pusPacketInfo)
         while not self.replyReceived:
             # wait until reply is received
             time.sleep(3)
         if self.replyReceived:
-            self.performListenerMode()
+            self.comInterface.performListenerMode()
 
     # runs in separate thread. sends TC again if no TM is received after timeout
     def receiveReply(self):
diff --git a/tc/OBSW_TcPacker.py b/tc/OBSW_TcPacker.py
index 8c49f4f54f13e9d8834312816c6ae55aee69b2e7..6a11214f25d7cc63ae1d31c2d627704d27f6b681 100644
--- a/tc/OBSW_TcPacker.py
+++ b/tc/OBSW_TcPacker.py
@@ -1,6 +1,23 @@
 # -*- coding: utf-8 -*-
+"""
+Program: OBSW_UnitTest.py
+Date: 01.11.2019
+Description: Packs the TC queue for specific service or device testing
+
+Manual:
+Contains a service select factory which returns specific service or device tests.
+Also contains an all service and all device tc queue packer.
+Run script with -s <Service or Device> -m 3 with optional -p parameter for file output inside the log folder
+
+@author: R. Mueller
+"""
+import os
+
 from tc.OBSW_TcPacket import *
-import struct
+from tc.OBSW_TcService2 import packService2TestInto
+from tc.OBSW_TcService8 import packService8TestInto
+from tc.OBSW_TcService200 import packModeData, packService200TestInto
+
 from datetime import datetime
 import queue
 
@@ -35,56 +52,31 @@ def serviceTestSelect(service, serviceQueue):
         exit()
 
 
-def packService2TestInto(tcQueue, builderQueue=0):
-    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])  # dummy device
-    # don't forget to set object mode raw (service 200) before calling this !
-    # Set Raw Mode
-    modeData = packModeData(objectId, 3, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=2020, data=modeData)
-    tcQueue.put(command.packCommandTuple())
-    # toggle wiretapping raw
-    wiretappingToggleData = packWiretappingMode(objectId, 1)
-    toggleWiretappingOnCommand = PUSTelecommand(service=2, subservice=129, SSC=200, data=wiretappingToggleData)
-    tcQueue.put(toggleWiretappingOnCommand.packCommandTuple())
-    # send raw command, data should be returned via TM[2,130] and TC[2,131]
-    rawCommand = struct.pack(">I", 666)
-    rawData = objectId + rawCommand
-    rawCommand = PUSTelecommand(service=2, subservice=128, SSC=201, data=rawData)
-    tcQueue.put(rawCommand.packCommandTuple())
-    # toggle wiretapping off
-    wiretappingToggleData = packWiretappingMode(objectId, 0)
-    toggleWiretappingOffCommand = PUSTelecommand(service=2, subservice=129, SSC=204, data=wiretappingToggleData)
-    tcQueue.put(toggleWiretappingOffCommand.packCommandTuple())
-    # send raw command which should be returned via TM[2,130]
-    command = PUSTelecommand(service=2, subservice=128, SSC=205, data=rawData)
-    tcQueue.put(command.packCommandTuple())
-    return tcQueue
-
-
-# wiretappingMode = 0: MODE_OFF, wiretappingMode = 1: MODE_RAW
-def packWiretappingMode(objectId, wiretappingMode_):
-    wiretappingMode = struct.pack(">B", wiretappingMode_)  # MODE_OFF : 0x00, MODE_RAW: 0x01
-    wiretappingToggleData = objectId + wiretappingMode
-    return wiretappingToggleData
-
-
 def packService5TestInto(tcQueue):
+    tcQueue.put(("print", "Testing Service 5"))
     # disable events
+    tcQueue.put(("print", "\r\nTesting Service 5: Disable event"))
     command = PUSTelecommand(service=5, subservice=6, SSC=500)
     tcQueue.put(command.packCommandTuple())
     # trigger event
+    tcQueue.put(("print", "\r\nTesting Service 5: Trigger event"))
     command = PUSTelecommand(service=17, subservice=128, SSC=510)
     tcQueue.put(command.packCommandTuple())
     # enable event
+    tcQueue.put(("print", "\r\nTesting Service 5: Enable event"))
     command = PUSTelecommand(service=5, subservice=5, SSC=520)
     tcQueue.put(command.packCommandTuple())
     # trigger event
+    tcQueue.put(("print", "\r\nTesting Service 5: Trigger another event"))
     command = PUSTelecommand(service=17, subservice=128, SSC=530)
     tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    tcQueue.put(("export", "log/tmtc_log_service5.txt"))
     return tcQueue
 
 
 def packService9TestInto(tcQueue):
+    tcQueue.put(("print", "Testing Service 9"))
     timeTestASCIICodeA = '2019-08-30T20:50:33.892429Z' + '\0'
     timeTestASCIICodeB = '2019-270T05:50:33.002000Z' + '\0'
     timeTestCurrentTime = datetime.now().isoformat() + "Z" + '\0'
@@ -93,121 +85,86 @@ def packService9TestInto(tcQueue):
     timeTest3 = timeTestCurrentTime.encode('ascii')
     print("Time Code 1 :" + str(timeTest1))
     print("Time Code 2 :" + str(timeTest2))
-    print("Time Code 3 :" + str(timeTest3))
+    print("Time Code 3 :" + str(timeTest3) + "\r")
     # time setting
+    tcQueue.put(("print", "\r\nTesting Service 9: Testing timecode A"))
     command = PUSTelecommand(service=9, subservice=128, SSC=900, data=timeTest1)
     tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r\nTesting Service 9: Testing timecode B"))
     command = PUSTelecommand(service=9, subservice=128, SSC=910, data=timeTest2)
     tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r\nTesting Service 9: Testing timecode Current Time"))
     command = PUSTelecommand(service=9, subservice=128, SSC=920, data=timeTest3)
     tcQueue.put(command.packCommandTuple())
     # TODO: Add other time formats here
-    return tcQueue
-
-
-def packService8TestInto(tcQueue):
-    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
-    # set mode normal
-    modeData = packModeData(objectId, 2, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=3, data=modeData)
-    tcQueue.put(command.packCommandTuple())
-    # Direct command which triggers completion reply
-    actionId = struct.pack(">I", 666)
-    directCommand = objectId + actionId
-    command = PUSTelecommand(service=8, subservice=128, SSC=810, data=directCommand)
-    tcQueue.put(command.packCommandTuple())
-    # Direct command which triggers data reply
-    actionId = bytearray([0xC0, 0xC0, 0xBA, 0xBE])
-    commandParam1 = bytearray([0xBA, 0xB0])
-    commandParam2 = bytearray([0x00, 0x00, 0x00, 0x52, 0x4F, 0x42, 0x49, 0x4E])
-    directCommand = objectId + actionId + commandParam1 + commandParam2
-    command = PUSTelecommand(service=8, subservice=128, SSC=820, data=directCommand)
-    tcQueue.put(command.packCommandTuple())
-    # Direct command which triggers an additional step reply and one completion reply
-    actionId = bytearray([0xBA, 0xDE, 0xAF, 0xFE])
-    directCommand = objectId + actionId
-    command = PUSTelecommand(service=8, subservice=128, SSC=830, data=directCommand)
-    tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    tcQueue.put(("export", "log/tmtc_log_service9.txt"))
     return tcQueue
 
 
 def packService17TestInto(tcQueue):
+    tcQueue.put(("print", "Testing Service 17"))
     # ping test
+    tcQueue.put(("print", "\n\rTesting Service 17: Ping Test"))
     command = PUSTelecommand(service=17, subservice=1, SSC=1700)
     tcQueue.put(command.packCommandTuple())
     # enable event
+    tcQueue.put(("print", "\n\rTesting Service 17: Enable Event"))
     command = PUSTelecommand(service=5, subservice=5, SSC=52)
     tcQueue.put(command.packCommandTuple())
     # test event
+    tcQueue.put(("print", "\n\rTesting Service 17: Trigger event"))
     command = PUSTelecommand(service=17, subservice=128, SSC=1701)
     tcQueue.put(command.packCommandTuple())
-    return tcQueue
-
-
-def packService200TestInto(tcQueue):
-    # Object ID: Dummy Device
-    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
-    # Set On Mode
-    modeData = packModeData(objectId, 1, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=2000, data=modeData)
-    tcQueue.put(command.packCommandTuple())
-    # Set Normal mode
-    modeData = packModeData(objectId, 2, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=2010, data=modeData)
-    tcQueue.put(command.packCommandTuple())
-    # Set Raw Mode
-    modeData = packModeData(objectId, 3, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=2020, data=modeData)
-    tcQueue.put(command.packCommandTuple())
-    # Set Off Mode
-    modeData = packModeData(objectId, 0, 0)
-    command = PUSTelecommand(service=200, subservice=1, SSC=2030, data=modeData)
-    tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    tcQueue.put(("export", "log/tmtc_log_service17.txt"))
     return tcQueue
 
 
 def packDummyDeviceTestInto(tcQueue):
+    tcQueue.put(("print", "Testing Dummy Device"))
     # Object ID: Dummy Device
     objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
     # Set On Mode
+    tcQueue.put(("print", "\n\rTesting Service Dummy: Set On"))
     modeData = packModeData(objectId, 1, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=1, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # Test Service 2 commands
-    packService2TestInto(tcQueue)
+    tcQueue.put(("print", "\n\rTesting Service Dummy: Service 2"))
+    packService2TestInto(tcQueue, True)
     # Test Service 8
-    packService8TestInto(tcQueue)
+    tcQueue.put(("print", "\n\rTesting Service Dummy: Service 8"))
+    packService8TestInto(tcQueue, True)
+    tcQueue.put(("print", "\r"))
+    tcQueue.put(("export", "log/tmtc_log_service_dummy.txt"))
     return tcQueue
 
 
 def packGpsTestInto(objectId, tcQueue):
+    tcQueue.put(("print", "Testing GPS Device"))
     # Set Mode Off
+    tcQueue.put(("print", "\n\rTesting GPS: Set Off"))
     modeData = packModeData(objectId, 0, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=11, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # Set Mode On
+    tcQueue.put(("print", "\n\rTesting GPS: Set On"))
     modeData = packModeData(objectId, 1, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=12, data=modeData)
     tcQueue.put(command.packCommandTuple())
     # pack wait interval until mode is one
-    tcQueue.put(("wait", 5))
+    tcQueue.put(("wait", 3))
     # Set Mode Off
+    tcQueue.put(("print", "\n\rTesting GPS: Set Off"))
     modeData = packModeData(objectId, 0, 0)
     command = PUSTelecommand(service=200, subservice=1, SSC=13, data=modeData)
     tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    tcQueue.put(("export", "log/tmtc_log_service_gps.txt"))
     return tcQueue
 
 
-# Mode 0: Off, Mode 1: Mode On, Mode 2: Mode Normal, Mode 3: Mode Raw
-def packModeData(objectId, mode_, submode_):
-    # Normal mode
-    mode = struct.pack(">I", mode_)
-    # Submode default
-    submode = struct.pack('B', submode_)
-    modeData = objectId + mode + submode
-    return modeData
-
-
 def packErrorTestingInto(tcQueue):
     # a lot of events
     command = PUSTelecommand(service=17, subservice=129, SSC=2010)
@@ -219,6 +176,8 @@ def packErrorTestingInto(tcQueue):
 
 
 def createTotalTcQueue():
+    if not os.path.exists("log"):
+        os.mkdir("log")
     tcQueue = queue.Queue()
     tcQueue = packService2TestInto(tcQueue)
     tcQueue = packService5TestInto(tcQueue)
diff --git a/tc/OBSW_TcService2.py b/tc/OBSW_TcService2.py
new file mode 100644
index 0000000000000000000000000000000000000000..5eae374c35ae1ed75308bf1bf7ef4dc9849ef107
--- /dev/null
+++ b/tc/OBSW_TcService2.py
@@ -0,0 +1,60 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_UnitTest.py
+Date: 01.11.2019
+Description: PUS Custom Service 8: Device Access, Native low-level commanding
+
+@author: R. Mueller
+"""
+
+import struct
+
+from tc.OBSW_TcPacket import *
+from tc.OBSW_TcService200 import packModeData
+
+
+def packService2TestInto(tcQueue, calledExternally=False):
+    if calledExternally is False:
+        tcQueue.put(("print", "Testing Service 2"))
+    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])  # dummy device
+    # don't forget to set object mode raw (service 200) before calling this !
+    # Set Raw Mode
+    tcQueue.put(("print", "\r\nTesting Service 2: Setting Raw Mode"))
+    modeData = packModeData(objectId, 3, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=2020, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    # toggle wiretapping raw
+    tcQueue.put(("print", "\r\nTesting Service 2: Toggling Wiretapping Raw"))
+    wiretappingToggleData = packWiretappingMode(objectId, 1)
+    toggleWiretappingOnCommand = PUSTelecommand(service=2, subservice=129, SSC=200, data=wiretappingToggleData)
+    tcQueue.put(toggleWiretappingOnCommand.packCommandTuple())
+    # send raw command, data should be returned via TM[2,130] and TC[2,131]
+    tcQueue.put(("print", "\r\nTesting Service 2: Sending Raw Command"))
+    rawCommand = struct.pack(">I", 666)
+    rawData = objectId + rawCommand
+    rawCommand = PUSTelecommand(service=2, subservice=128, SSC=201, data=rawData)
+    tcQueue.put(rawCommand.packCommandTuple())
+    # toggle wiretapping off
+    tcQueue.put(("print", "\r\nTesting Service 2: Toggle Wiretapping Off"))
+    wiretappingToggleData = packWiretappingMode(objectId, 0)
+    toggleWiretappingOffCommand = PUSTelecommand(service=2, subservice=129, SSC=204, data=wiretappingToggleData)
+    tcQueue.put(toggleWiretappingOffCommand.packCommandTuple())
+    # send raw command which should be returned via TM[2,130]
+    tcQueue.put(("print", "\r\nTesting Service 2: Send second raw command"))
+    command = PUSTelecommand(service=2, subservice=128, SSC=205, data=rawData)
+    tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    if calledExternally is False:
+        tcQueue.put(("export", "log/tmtc_log_service2.txt"))
+    return tcQueue
+
+
+# wiretappingMode = 0: MODE_OFF, wiretappingMode = 1: MODE_RAW
+def packWiretappingMode(objectId, wiretappingMode_):
+    wiretappingMode = struct.pack(">B", wiretappingMode_)  # MODE_OFF : 0x00, MODE_RAW: 0x01
+    wiretappingToggleData = objectId + wiretappingMode
+    return wiretappingToggleData
+
+
+def packSpecificService2TestInto(tcQueue, objectIdList, dataList, sscList, printStringList, calledExternally=False):
+    pass
diff --git a/tc/OBSW_TcService200.py b/tc/OBSW_TcService200.py
new file mode 100644
index 0000000000000000000000000000000000000000..995fcd3a198e063839ccf24d94bb0c575a52ecc3
--- /dev/null
+++ b/tc/OBSW_TcService200.py
@@ -0,0 +1,48 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_UnitTest.py
+Date: 01.11.2019
+Description: PUS Custom Service 200: Mode Commanding
+
+Manual:
+Contains a service select factory which returns specific service or device tests.
+Also contains an all service and all device tc queue packer.
+Run script with -s <Service or Device> -m 3 with optional -p parameter for file output inside the log folder
+
+@author: R. Mueller
+"""
+from tc.OBSW_TcPacket import *
+import struct
+
+
+def packService200TestInto(tcQueue):
+    # Object ID: Dummy Device
+    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
+    # Set On Mode
+    modeData = packModeData(objectId, 1, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=2000, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    # Set Normal mode
+    modeData = packModeData(objectId, 2, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=2010, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    # Set Raw Mode
+    modeData = packModeData(objectId, 3, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=2020, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    # Set Off Mode
+    modeData = packModeData(objectId, 0, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=2030, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("export", "log/tmtc_log_service200.txt"))
+    return tcQueue
+
+
+# Mode 0: Off, Mode 1: Mode On, Mode 2: Mode Normal, Mode 3: Mode Raw
+def packModeData(objectId, mode_, submode_):
+    # Normal mode
+    mode = struct.pack(">I", mode_)
+    # Submode default
+    submode = struct.pack('B', submode_)
+    modeData = objectId + mode + submode
+    return modeData
diff --git a/tc/OBSW_TcService8.py b/tc/OBSW_TcService8.py
new file mode 100644
index 0000000000000000000000000000000000000000..6768e4cda2b2b26166019536773d8105be9836fc
--- /dev/null
+++ b/tc/OBSW_TcService8.py
@@ -0,0 +1,53 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_UnitTest.py
+Date: 01.11.2019
+Description: PUS Custom Service 8: Function Management, High-Level Commanding
+
+@author: R. Mueller
+"""
+
+import struct
+
+from tc.OBSW_TcPacket import *
+from tc.OBSW_TcService200 import packModeData
+
+
+def packService8TestInto(tcQueue, calledExternally=False):
+    if calledExternally is False:
+        tcQueue.put(("print", "Testing Service 8"))
+    objectId = bytearray([0x44, 0x00, 0xAF, 0xFE])
+    # set mode normal
+    tcQueue.put(("print", "\r\nTesting Service 8: Set Normal Mode"))
+    modeData = packModeData(objectId, 2, 0)
+    command = PUSTelecommand(service=200, subservice=1, SSC=3, data=modeData)
+    tcQueue.put(command.packCommandTuple())
+    # Direct command which triggers completion reply
+    tcQueue.put(("print", "\r\nTesting Service 8: Trigger Completion Reply"))
+    actionId = struct.pack(">I", 666)
+    directCommand = objectId + actionId
+    command = PUSTelecommand(service=8, subservice=128, SSC=810, data=directCommand)
+    tcQueue.put(command.packCommandTuple())
+    # Direct command which triggers data reply
+    tcQueue.put(("print", "\r\nTesting Service 8: Trigger Data Reply"))
+    actionId = bytearray([0xC0, 0xC0, 0xBA, 0xBE])
+    commandParam1 = bytearray([0xBA, 0xB0])
+    commandParam2 = bytearray([0x00, 0x00, 0x00, 0x52, 0x4F, 0x42, 0x49, 0x4E])
+    directCommand = objectId + actionId + commandParam1 + commandParam2
+    command = PUSTelecommand(service=8, subservice=128, SSC=820, data=directCommand)
+    tcQueue.put(command.packCommandTuple())
+    # Direct command which triggers an additional step reply and one completion reply
+    tcQueue.put(("print", "\r\nTesting Service 8: Trigger Step and Completion Reply"))
+    actionId = bytearray([0xBA, 0xDE, 0xAF, 0xFE])
+    directCommand = objectId + actionId
+    command = PUSTelecommand(service=8, subservice=128, SSC=830, data=directCommand)
+    tcQueue.put(command.packCommandTuple())
+    tcQueue.put(("print", "\r"))
+    if calledExternally is False:
+        tcQueue.put(("export", "log/tmtc_log_service8.txt"))
+    return tcQueue
+
+
+def packSpecificService8TestInto(tcQueue, objectIdList, actionIdList, dataList,
+                                 sscList, printStringList, calledExternally=False):
+    pass
diff --git a/test/OBSW_UnitTest.py b/test/OBSW_UnitTest.py
index 52633cbe65efffad8cc704004af18f2cbdb7f84e..68f7fb51b719ea27d7a7490209c2889f697e7f52 100644
--- a/test/OBSW_UnitTest.py
+++ b/test/OBSW_UnitTest.py
@@ -27,29 +27,40 @@ import queue
 from tc.OBSW_TcPacker import packService17TestInto, packService5TestInto, packDummyDeviceTestInto
 from sendreceive.OBSW_MultipleCommandsSenderReceiver import MultipleCommandSenderReceiver
 from OBSW_UdpClient import connectToBoard
+from utility.OBSW_TmTcPrinter import TmtcPrinter
+from comIF.OBSW_Ethernet_ComIF import EthernetComIF
+import OBSW_Config as g
 
 
 class TestService(unittest.TestCase):
+    testQueue = queue.Queue()
+
     @classmethod
     def setUpClass(cls):
         cls.displayMode = "long"
+
         # default timeout for receiving TM, set in subclass manually
-        cls.tmTimeout = 15
+        cls.tmTimeout = 3
         # wait intervals between tc send bursts.
         # Example: [2,4] sends to send 2 tc from queue and wait, then sends another 2 and wait again
         cls.waitIntervals = []
+        cls.waitTime = 2
         cls.printTm = True
         cls.printTc = True
         # default wait time between tc send bursts
         cls.waitTime = 5.0
         cls.testQueue = queue.Queue()
-        cls.tcTimeoutFactor = 2.0
+        cls.tcTimeoutFactor = 3.0
+        cls.printFile = True
+        cls.tmtcPrinter = TmtcPrinter(cls.displayMode, cls.printFile, True)
+        cls.communicationInterface = EthernetComIF(cls.tmtcPrinter, cls.tmTimeout, cls.tcTimeoutFactor, g.sockSend,
+                                                   g.sockReceive, g.sendAddress)
         connectToBoard()
 
     def performTestingAndGenerateAssertionDict(self):
-        UnitTester = MultipleCommandSenderReceiver(self.displayMode, self.testQueue, self.tmTimeout,
-                                                   self.waitIntervals, self.waitTime, self.printTm,
-                                                   self.printTc, self.tcTimeoutFactor, False)
+        UnitTester = MultipleCommandSenderReceiver(self.communicationInterface, self.tmtcPrinter, self.testQueue,
+                                                   self.tmTimeout, self.waitIntervals, self.waitTime, self.printTm,
+                                                   self.tcTimeoutFactor, self.printFile)
         (tcInfoQueue, tmInfoQueue) = UnitTester.sendTcQueueAndReturnTcInfo()
         assertionDict = self.analyseTmTcInfo(tmInfoQueue, tcInfoQueue)
         return assertionDict
@@ -158,8 +169,8 @@ class TestService5(TestService):
     def setUpClass(cls):
         super().setUpClass()
         print("Testing Service 5")
-        cls.waitIntervals = [2]
-        cls.waitTime = 7
+        cls.waitIntervals = [1, 2, 3]
+        cls.waitTime = [1.2, 1.5, 1.2]
         packService5TestInto(cls.testQueue)
 
     def test_Service5(self):
@@ -203,6 +214,7 @@ class TestService17(TestService):
         print("Testing Service 17")
         cls.waitIntervals = [2]
         cls.waitTime = 2
+        cls.tmTimeout = 2
         packService17TestInto(cls.testQueue)
 
     def test_Service17(self):
diff --git a/tm/OBSW_TmPacket.py b/tm/OBSW_TmPacket.py
index a77235c295dd9d77d266f663c7ecb3c9156fa021..2e86c0740eff0bf458da5a900f37b9e093a516af 100644
--- a/tm/OBSW_TmPacket.py
+++ b/tm/OBSW_TmPacket.py
@@ -20,6 +20,8 @@ def PUSTelemetryFactory(rawPacket):
         return Service1TM(rawPacket)
     elif servicetype == 2:
         return Service2TM(rawPacket)
+    elif servicetype == 3:
+        return Service3TM(rawPacket)
     elif servicetype == 5:
         return Service5TM(rawPacket)
     elif servicetype == 8:
@@ -126,7 +128,20 @@ class Service2TM(PUSTelemetry):
     def printTelemetryColumnHeaders(self, array):
         super().printTelemetryColumnHeaders(array)
         return
-    
+
+
+class Service3TM(PUSTelemetry):
+    def __init__(self, byteArray):
+        super().__init__(byteArray)
+
+    def printTelemetryHeader(self, array):
+        super().printTelemetryHeader(array)
+        return
+
+    def printTelemetryColumnHeaders(self, array):
+        super().printTelemetryColumnHeaders(array)
+        return
+
 
 class Service5TM(PUSTelemetry):
     def __init__(self, byteArray):
diff --git a/utility/OBSW_TmTcPrinter.py b/utility/OBSW_TmTcPrinter.py
index 8e41f0013cda5b975de8ea4581dcb81f814587e1..919ff6fac994ad5bfa2d12226a82e0b836264a11 100644
--- a/utility/OBSW_TmTcPrinter.py
+++ b/utility/OBSW_TmTcPrinter.py
@@ -13,12 +13,13 @@ import OBSW_Config as g
 
 # TODO: Print everything in a file
 class TmtcPrinter:
-    def __init__(self, displayMode, doPrintToFile):
+    def __init__(self, displayMode, doPrintToFile, printTc):
         self.printBuffer = ""
         # global print buffer which will be useful to print something to file
         self.fileBuffer = ""
         self.displayMode = displayMode
         self.doPrintToFile = doPrintToFile
+        self.printTc = printTc
 
     def printTelemetry(self, packet):
         if self.displayMode == "short":
@@ -74,14 +75,18 @@ class TmtcPrinter:
             print(self.printBuffer)
 
     # This function handles the printing of Telecommands
-    def displaySentCommand(self, pusPacketInfo, pusPacket, displayMode):
-        if len(pusPacket) == 0:
-            print("Empty packet was sent, configuration error")
-            exit()
-        if displayMode == "short":
-            self.handleShortTcPrint(pusPacketInfo)
-        else:
-            self.handleLongTcPrint(pusPacketInfo)
+    def printTelecommand(self, pusPacket, pusPacketInfo=""):
+        if self.printTc:
+            if len(pusPacket) == 0:
+                print("TMTC Printer: Empty packet was sent, configuration error")
+                exit()
+            if pusPacketInfo == "":
+                print("TMTC Printer: No packet info supplied to print")
+                return
+            if self.displayMode == "short":
+                self.handleShortTcPrint(pusPacketInfo)
+            else:
+                self.handleLongTcPrint(pusPacketInfo)
 
     def handleShortTcPrint(self, pusPacketInfo):
         self.printBuffer = "Sent TC[" + str(pusPacketInfo["service"]) + "," + str(pusPacketInfo["subservice"]) \
@@ -105,12 +110,20 @@ class TmtcPrinter:
         strToPrint += ']'
         return strToPrint
 
+    def printString(self, string):
+        self.printBuffer = string
+        print(self.printBuffer)
+        if self.doPrintToFile:
+            self.addPrintBufferToFileBuffer()
+
     def addPrintBufferToFileBuffer(self):
         if self.doPrintToFile:
             self.fileBuffer = self.fileBuffer + self.printBuffer + "\n"
 
-    def printToFile(self):
-        file = open("tmtc_log.txt", 'w')
+    def printToFile(self, logName="tmtc_log.txt", clearFileBuffer=False):
+        file = open(logName, 'w')
         file.write(self.fileBuffer)
+        if clearFileBuffer:
+            self.fileBuffer = ""
         file.close()