From 18bef65b395a9516a0e3c448a69b37c2033943a3 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Thu, 2 Jan 2020 22:58:46 +0100
Subject: [PATCH] Squashed commit of the following:

commit b4795b4ae8c7ba7c9292c0c2c8f595c63896798f
Author: Robin.Mueller <robin.mueller.m@gmail.com>
Date:   Thu Jan 2 21:24:24 2020 +0100

    listener mode notification print

commit db2c26db2aeb5e109c3c5e114a6fd578bfa9fb3e
Author: Robin.Mueller <robin.mueller.m@gmail.com>
Date:   Thu Jan 2 00:05:37 2020 +0100

    tmtc adapted for serial send and receive

commit faf01c5ab04df97b168aad84aa128f28cf6379af
Author: Robin.Mueller <robin.mueller.m@gmail.com>
Date:   Mon Dec 30 20:55:36 2019 +0100

    service 3 testing first custom definition added

commit b8052826e58eabd5c95b89f02e34c4db3d336ac7
Author: Robin.Mueller <robin.mueller.m@gmail.com>
Date:   Mon Dec 30 20:21:22 2019 +0100

    tmtc refactoring, service 3 tm and tc handling continued

commit 2774a1c2bc2b669a91d54da6eb1f838b4a9dd447
Author: Robin.Mueller <robin.mueller.m@gmail.com>
Date:   Mon Dec 30 01:12:54 2019 +0100

    hk service print init, hk service test plan added

commit 03596fa7955e2efb3361c45bbfc0dd6b58199983
Author: Robin Mueller <robin.mueller.m@gmail.com>
Date:   Sun Dec 29 02:06:03 2019 +0100

    tmtc script service 3 init
---
 OBSW_Config.py                                |  2 +
 OBSW_UdpClient.py                             |  8 ++-
 comIF/OBSW_ComInterface.py                    |  2 +-
 comIF/OBSW_Ethernet_ComIF.py                  |  5 +-
 comIF/OBSW_Serial_ComIF.py                    | 29 ++++-----
 sendreceive/OBSW_CommandSenderReceiver.py     |  4 +-
 sendreceive/OBSW_SequentialSenderReceiver.py  |  5 +-
 .../OBSW_SingleCommandSenderReceiver.py       |  1 +
 tc/OBSW_TcPacker.py                           |  5 +-
 tc/OBSW_TcService3.py                         | 52 +++++++++++++++
 tm/OBSW_PusPacket.py                          |  8 +++
 tm/OBSW_PusTm.py                              | 34 ++++++++++
 tm/OBSW_TmPacket.py                           | 56 +++++-----------
 tm/OBSW_TmService3.py                         | 64 +++++++++++++++++++
 utility/OBSW_TmTcPrinter.py                   | 18 +++++-
 15 files changed, 217 insertions(+), 76 deletions(-)
 create mode 100644 tc/OBSW_TcService3.py
 create mode 100644 tm/OBSW_PusTm.py
 create mode 100644 tm/OBSW_TmService3.py

diff --git a/OBSW_Config.py b/OBSW_Config.py
index c9712dc..fa09986 100644
--- a/OBSW_Config.py
+++ b/OBSW_Config.py
@@ -17,6 +17,7 @@ modeId = 0
 service = 17
 displayMode = "long"
 
+
 comIF = 0
 # Time related
 tmTimeout = 10
@@ -30,6 +31,7 @@ sockReceive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
 
 # Print Settings
 printToFile = False
+printHkData = False
 printRawTmData = False
 
 
diff --git a/OBSW_UdpClient.py b/OBSW_UdpClient.py
index 3f89685..67fe5a6 100644
--- a/OBSW_UdpClient.py
+++ b/OBSW_UdpClient.py
@@ -96,9 +96,9 @@ def main():
         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)
+        comPort = 'COM9'
+        baudRate = 115200
+        communicationInterface = SerialComIF(tmtcPrinter, comPort, baudRate, 0.007)
     connectToBoard()
 
     if g.modeId == "ListenerMode":
@@ -162,6 +162,7 @@ def parseInputArguments():
     argParser.add_argument('-o', '--tcTimeoutFactor', type=float, help='TC Timeout Factor. Default: 2.0', default=2.0)
     argParser.add_argument('-r', '--rawDataPrint', help='Supply -r to print all raw data directly', action='store_true')
     argParser.add_argument('-d', '--shortDisplayMode', help='Supply -d to print short output', action='store_true')
+    argParser.add_argument('-k', '--hk', help='Supply -k or --hk to print HK data', action='store_true')
     if len(sys.argv) == 1:
         print("No Input Arguments specified, setting default values.")
         argParser.print_help()
@@ -199,6 +200,7 @@ def setGlobals(args):
     g.sockReceive = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     g.sockSend = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
     g.modeId = modeId
+    g.printHkData = args.hk
     g.tmTimeout = args.tmTimeout
     g.printRawTmData = args.rawDataPrint
     g.displayMode = displayMode
diff --git a/comIF/OBSW_ComInterface.py b/comIF/OBSW_ComInterface.py
index a113c44..161e579 100644
--- a/comIF/OBSW_ComInterface.py
+++ b/comIF/OBSW_ComInterface.py
@@ -21,7 +21,7 @@ class CommunicationInterface:
 
     # Receive telemetry and process it
     @abstractmethod
-    def receiveTelemetry(self, parameters):
+    def receiveTelemetry(self, parameters=0):
         pass
 
     # Poll the interface  for data
diff --git a/comIF/OBSW_Ethernet_ComIF.py b/comIF/OBSW_Ethernet_ComIF.py
index b921e96..a65c241 100644
--- a/comIF/OBSW_Ethernet_ComIF.py
+++ b/comIF/OBSW_Ethernet_ComIF.py
@@ -29,8 +29,8 @@ class EthernetComIF(CommunicationInterface):
 
     def performListenerMode(self):
         pollTimeout = 10
+        print("Listening for packages ...")
         while True:
-            print("Listening for packages ...")
             self.pollInterface(pollTimeout)
 
     def dataAvailable(self, timeout):
@@ -43,9 +43,10 @@ class EthernetComIF(CommunicationInterface):
     def pollInterface(self, pollTimeout):
         ready = self.dataAvailable(pollTimeout)
         if ready is False:
-            pass
+            return False
         elif ready:
             self.receiveTelemetry()
+            return True
 
     def receiveTelemetry(self, parameters=0):
         data = self.recvSocket.recvfrom(1024)[0]
diff --git a/comIF/OBSW_Serial_ComIF.py b/comIF/OBSW_Serial_ComIF.py
index 949976e..b862bac 100644
--- a/comIF/OBSW_Serial_ComIF.py
+++ b/comIF/OBSW_Serial_ComIF.py
@@ -12,8 +12,6 @@ class SerialComIF(CommunicationInterface):
         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()
 
@@ -21,7 +19,7 @@ class SerialComIF(CommunicationInterface):
         self.tmtcPrinter.printTelecommand(tcPacket, tcPacketInfo)
         self.serial.write(tcPacket)
 
-    def receiveTelemetry(self, data):
+    def receiveTelemetry(self, parameters=0):
         packet = self.pollInterface()
         return packet
 
@@ -29,36 +27,31 @@ class SerialComIF(CommunicationInterface):
         if self.dataAvailable():
             data = self.pollPusPacket()
             packet = PUSTelemetryFactory(data)
+            self.tmtcPrinter.printTelemetry(packet)
             return packet
 
     def dataAvailable(self, timeout=0):
-        if timeout > 0:
+        if self.serial.in_waiting > 0:
+            return True
+        elif 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:
+        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 data
+        return pusData
 
     def performListenerMode(self):
+        print("Listening for packages ...")
         while True:
             self.pollInterface()
 
diff --git a/sendreceive/OBSW_CommandSenderReceiver.py b/sendreceive/OBSW_CommandSenderReceiver.py
index 41afdaa..8352a3b 100644
--- a/sendreceive/OBSW_CommandSenderReceiver.py
+++ b/sendreceive/OBSW_CommandSenderReceiver.py
@@ -88,9 +88,7 @@ class CommandSenderReceiver:
             while tmReady:
                 self.comInterface.receiveTelemetry()
                 tmReady = self.comInterface.dataAvailable(self.tmTimeout / 1.5)
-                if tmReady is None:
-                    return False
-                else:
+                if not tmReady:
                     return True
 
 
diff --git a/sendreceive/OBSW_SequentialSenderReceiver.py b/sendreceive/OBSW_SequentialSenderReceiver.py
index 65025e5..d160c41 100644
--- a/sendreceive/OBSW_SequentialSenderReceiver.py
+++ b/sendreceive/OBSW_SequentialSenderReceiver.py
@@ -94,9 +94,8 @@ class SequentialCommandSenderReceiver(CommandSenderReceiver):
     def handleReplyListening(self):
         if self.firstReplyReceived:
             tmReady = self.comInterface.dataAvailable(2.0)
-            if tmReady is not None:
-                if tmReady[0]:
-                    self.handleTelemetrySequence()
+            if tmReady:
+                self.handleTelemetrySequence()
         else:
             self.handleFirstReplyListening()
 
diff --git a/sendreceive/OBSW_SingleCommandSenderReceiver.py b/sendreceive/OBSW_SingleCommandSenderReceiver.py
index 3dc54d7..4b025b7 100644
--- a/sendreceive/OBSW_SingleCommandSenderReceiver.py
+++ b/sendreceive/OBSW_SingleCommandSenderReceiver.py
@@ -28,6 +28,7 @@ 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_TcPacker.py b/tc/OBSW_TcPacker.py
index a6625d1..34ca200 100644
--- a/tc/OBSW_TcPacker.py
+++ b/tc/OBSW_TcPacker.py
@@ -15,6 +15,7 @@ import os
 
 from tc.OBSW_TcPacket import *
 from tc.OBSW_TcService2 import packService2TestInto
+from tc.OBSW_TcService3 import packService3TestInto
 from tc.OBSW_TcService8 import packService8TestInto
 from tc.OBSW_TcService200 import packModeData, packService200TestInto
 
@@ -54,10 +55,6 @@ def serviceTestSelect(service, serviceQueue):
         exit()
 
 
-def packService3TestInto(tcQueue):
-    tcQueue.put(("print", "Testing Service 3"))
-
-
 def packService5TestInto(tcQueue):
     tcQueue.put(("print", "Testing Service 5"))
     # disable events
diff --git a/tc/OBSW_TcService3.py b/tc/OBSW_TcService3.py
new file mode 100644
index 0000000..1e96a21
--- /dev/null
+++ b/tc/OBSW_TcService3.py
@@ -0,0 +1,52 @@
+# -*- 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 PUSTelecommand
+
+
+def packService3TestInto(tcQueue):
+    tcQueue.put(("print", "Testing Service 3"))
+    # adding custom defintion to hk using test pool variables
+    tcQueue.put(("print", "\r\nTesting Service 3: Adding custom definition"))
+    sid = bytearray([0x00, 0x00, 0x42, 0x00])
+    collectionInterval = struct.pack('>f', 3)
+    numberOfParameters = struct.pack('B', 5)
+    p1 = bytearray([0x01, 0x01, 0x01, 0x01])
+    p2 = bytearray([0x02, 0x02, 0x02, 0x02])
+    p3 = bytearray([0x03, 0x03, 0x03, 0x03])
+    p4 = bytearray([0x04, 0x04, 0x04, 0x04])
+    p5 = bytearray([0x05, 0x05, 0x05, 0x05])
+    hkDefinition = sid + collectionInterval + numberOfParameters + p1 + p2 + p3 + p4 + p5
+    command = PUSTelecommand(service=3, subservice=1, SSC=3010, data=hkDefinition)
+    tcQueue.put(command.packCommandTuple())
+    # adding custom definition to diagnostics using test pool variables
+    command = PUSTelecommand(service=3, subservice=2, SSC=3010, data=hkDefinition)
+    tcQueue.put(command.packCommandTuple())
+    # enable custom hk definition
+    # enable custom diag definition
+    # disable custom hk definition
+    # disable custom diag definition
+    # report custom hk definition
+    # report custom diag definition
+    # generate one custom hk definition
+    # generate one custom diag definition
+    # modify custom hk definition interval
+    # modify custom diag definition interval
+    # report custom hk definition
+    # report custom diag definition
+    # append parameter to custom hk definiton
+    # append parameter to custom diag definition
+    # report custom hk definition
+    # report custom diag definition
+    # delete custom hk definition
+    # delete custom diag definition
+    # do some basic testing on predefined structs too
+    # e.g. add one variable, change interval, report them....
+    tcQueue.put(("export", "log/tmtc_log_service3.txt"))
+    return tcQueue
diff --git a/tm/OBSW_PusPacket.py b/tm/OBSW_PusPacket.py
index d664adc..55d2fe3 100644
--- a/tm/OBSW_PusPacket.py
+++ b/tm/OBSW_PusPacket.py
@@ -134,6 +134,14 @@ class OBSWPusPacket:
     def getSSC(self):
         return self.PUSHeader.sourceSequenceCount
 
+    def printData(self):
+        strToPrint = "["
+        for byte in self.data:
+            strToPrint += str(hex(byte)) + " , "
+            strToPrint = strToPrint.rstrip(' , ')
+            strToPrint += ']'
+        return strToPrint
+
 # Structure of a PUS Packet :
 # A PUS packet consists of consecutive bits, the allocation and structure is standardised.
 # Extended information can be found in ECSS-E-70-41A  on p.42
diff --git a/tm/OBSW_PusTm.py b/tm/OBSW_PusTm.py
new file mode 100644
index 0000000..703fe8e
--- /dev/null
+++ b/tm/OBSW_PusTm.py
@@ -0,0 +1,34 @@
+from tm.OBSW_PusPacket import OBSWPusPacket
+
+
+class PUSTelemetry(OBSWPusPacket):
+    def __init__(self, byteArray):
+        super().__init__(byteArray)
+        self.byteArrayData = self.data
+        self.printInfo = ""
+
+    def unpackTelemetry(self):
+        pass
+
+    def printPacketInfo(self, printInfo):
+        self.printInfo = printInfo
+
+    def appendPacketInfo(self, printInfo):
+        self.printInfo = self.printInfo + printInfo
+
+    def printTelemetryHeader(self, array):
+        super().printPusPacketHeader(array)
+
+    def printTelemetryColumnHeaders(self, array):
+        super().printPusPacketHeaderColumnHeaders(array)
+
+    def packTmInformation(self):
+        tmInformation = {
+            "service": self.getService(),
+            "subservice": self.getSubservice(),
+            "ssc": self.getSSC(),
+            "data": self.byteArrayData,
+            "crc": self.crc,
+            "valid": self.PUSHeader.valid
+        }
+        return tmInformation
diff --git a/tm/OBSW_TmPacket.py b/tm/OBSW_TmPacket.py
index 2e86c07..98333c4 100644
--- a/tm/OBSW_TmPacket.py
+++ b/tm/OBSW_TmPacket.py
@@ -6,7 +6,8 @@ Description: Deserialize TM byte array into PUS TM Class
 Author: R.Mueller, S. Gaisser
 """
 
-from tm.OBSW_PusPacket import OBSWPusPacket
+from tm.OBSW_PusTm import PUSTelemetry
+from tm.OBSW_TmService3 import Service3TM
 import struct
 # TM Packets use the generic space packet structure provided by OBSWPusPacket to generate individual
 # telemetry packets for all services
@@ -34,32 +35,6 @@ def PUSTelemetryFactory(rawPacket):
         print("The service " + str(servicetype) + " is not implemented in Telemetry Factory")
         return PUSTelemetry(rawPacket)
 
-    
-class PUSTelemetry(OBSWPusPacket):
-    def __init__(self, byteArray):
-        super().__init__(byteArray)
-        self.byteArrayData = self.data
-        
-    def unpackTelemetry(self):
-        pass
-   
-    def printTelemetryHeader(self, array):
-        super().printPusPacketHeader(array)
-
-    def printTelemetryColumnHeaders(self, array):
-        super().printPusPacketHeaderColumnHeaders(array)
-
-    def packTmInformation(self):
-        tmInformation = {
-            "service": self.getService(),
-            "subservice": self.getSubservice(),
-            "ssc": self.getSSC(),
-            "data": self.byteArrayData,
-            "crc": self.crc,
-            "valid": self.PUSHeader.valid
-        }
-        return tmInformation
-
 
 class Service1TM(PUSTelemetry):
     def __init__(self, byteArray):
@@ -69,16 +44,20 @@ class Service1TM(PUSTelemetry):
         # Failure Reports with error code
         self.tcPacketId = self.byteArrayData[0] << 8 | self.byteArrayData[1]
         self.tcSSC = ((self.byteArrayData[2] & 0x3F) << 8) | self.byteArrayData[3]
+        self.printPacketInfo("Success Verification")
         if self.dataFieldHeader.subtype % 2 == 0:
+            self.printPacketInfo("Failure Verficiation")
             self.tcErrorCode = True
             if self.dataFieldHeader.subtype == 6:
                 self.isStep = True
+                self.appendPacketInfo(" : Step Failure")
                 self.stepNumber = struct.unpack('>B', self.byteArrayData[4:5])[0]
                 self.ErrCode = struct.unpack('>H', self.byteArrayData[5:7])[0]
             else:
                 self.ErrCode = struct.unpack('>H', self.byteArrayData[4:6])[0]
         elif self.dataFieldHeader.subtype == 5:
             self.isStep = True
+            self.appendPacketInfo(" : Step Success")
             self.stepNumber = struct.unpack('>B', self.byteArrayData[4:5])[0]
                 
     def printTelemetryHeader(self, array):
@@ -130,22 +109,18 @@ class Service2TM(PUSTelemetry):
         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):
         super().__init__(byteArray)
+        self.printPacketInfo("Event")
+        if self.getSubservice() == 1:
+            self.appendPacketInfo(" Info")
+        elif self.getSubservice() == 2:
+            self.appendPacketInfo(" Error Low Severity")
+        elif self.getSubservice() == 3:
+            self.appendPacketInfo(" Error Med Severity")
+        elif self.getSubservice() == 4:
+            self.appendPacketInfo(" Error High Severity")
         self.eventId = struct.unpack('>H', self.byteArrayData[0:2])[0]
         self.objectId = struct.unpack('>I', self.byteArrayData[2:6])[0]
         self.param1 = struct.unpack('>I', self.byteArrayData[6:10])[0]
@@ -206,6 +181,7 @@ class Service9TM(PUSTelemetry):
 class Service17TM(PUSTelemetry):
     def __init__(self, byteArray):
         super().__init__(byteArray)
+        self.printPacketInfo("Test Reply")
 
     def printTelemetryHeader(self, array):
         super().printTelemetryHeader(array)
diff --git a/tm/OBSW_TmService3.py b/tm/OBSW_TmService3.py
new file mode 100644
index 0000000..a697e3c
--- /dev/null
+++ b/tm/OBSW_TmService3.py
@@ -0,0 +1,64 @@
+# -*- coding: utf-8 -*-
+"""
+Program: OBSW_TmPacket.py
+Date: 30.12.2019
+Description: Deserialize Housekeeping TM
+Author: R.Mueller
+"""
+
+from tm.OBSW_PusTm import PUSTelemetry
+import struct
+
+
+class Service3TM(PUSTelemetry):
+    def __init__(self, byteArray):
+        super().__init__(byteArray)
+        self.sid = struct.unpack('>I', self.byteArrayData[0:4])[0]
+        self.hkHeader = []
+        self.hkContent = []
+        self.printPacketInfo("Housekeeping Packet")
+        if self.getSubservice() == 25:
+            self.paramLength = len(self.byteArrayData) - 4
+        # TODO: This can be automated by using the MIB parser pool names and pool datatypes
+        if self.sid == 0x1f00 or 0x2f00:
+            self.hkHeader = ["Fix Mode", "SV in Fix", "GNSS Week", "Time of Week", "Latitude", "Longitude",
+                             "Mean Sea Altitude", "Position X", "Position Y", "Position Z",
+                             "Velocity X", "Velocity Y", "Velocity Z"]
+            self.fixMode = self.byteArrayData[4]
+            self.svInFix = self.byteArrayData[5]
+            self.gnssWeek = struct.unpack('>H', self.byteArrayData[5:7])[0]
+            self.timeOfWeek = struct.unpack('>I', self.byteArrayData[7:11])[0]
+            self.latitude = struct.unpack('>I', self.byteArrayData[11:15])[0]
+            self.longitude = struct.unpack('>I', self.byteArrayData[15:19])[0]
+            self.msa = struct.unpack('>I', self.byteArrayData[19:23])[0]
+            self.positionX = struct.unpack('>d', self.byteArrayData[23:31])[0]
+            self.positionY = struct.unpack('>d', self.byteArrayData[31:39])[0]
+            self.positionZ = struct.unpack('>d', self.byteArrayData[39:47])[0]
+            self.vx = struct.unpack('>d', self.byteArrayData[47:55])[0]
+            self.vy = struct.unpack('>d', self.byteArrayData[55:63])[0]
+            self.vz = struct.unpack('>d', self.byteArrayData[63:71])[0]
+            self.hkContent.append(self.fixMode)
+            self.hkContent.append(self.svInFix)
+            self.hkContent.append(self.gnssWeek)
+            self.hkContent.append(self.timeOfWeek)
+            self.hkContent.append(self.latitude)
+            self.hkContent.append(self.longitude)
+            self.hkContent.append(self.msa)
+            self.hkContent.append(self.positionX)
+            self.hkContent.append(self.positionY)
+            self.hkContent.append(self.positionZ)
+            self.hkContent.append(self.vx)
+            self.hkContent.append(self.vy)
+            self.hkContent.append(self.vz)
+
+    def printTelemetryHeader(self, array):
+        super().printTelemetryHeader(array)
+        array.append(hex(self.sid))
+        array.append(int(self.paramLength))
+        return
+
+    def printTelemetryColumnHeaders(self, array):
+        super().printTelemetryColumnHeaders(array)
+        array.append("SID")
+        array.append("Number of Parameters")
+        return
diff --git a/utility/OBSW_TmTcPrinter.py b/utility/OBSW_TmTcPrinter.py
index 919ff6f..0157ea6 100644
--- a/utility/OBSW_TmTcPrinter.py
+++ b/utility/OBSW_TmTcPrinter.py
@@ -13,7 +13,7 @@ import OBSW_Config as g
 
 # TODO: Print everything in a file
 class TmtcPrinter:
-    def __init__(self, displayMode, doPrintToFile, printTc):
+    def __init__(self, displayMode="long", doPrintToFile=False, printTc=True):
         self.printBuffer = ""
         # global print buffer which will be useful to print something to file
         self.fileBuffer = ""
@@ -28,6 +28,8 @@ class TmtcPrinter:
             self.handleLongPrint(packet)
         self.handleWiretappingPacket(packet)
         self.handleDataReplyPacket(packet)
+        if packet.getService == 3 and packet.getSubservice == 25 or 26:
+            self.handleHkPrint(packet)
         if g.printRawTmData:
             self.printBuffer = "TM Data:" + "\n" + self.returnDataString(packet.data)
             print(self.printBuffer)
@@ -40,7 +42,7 @@ class TmtcPrinter:
         self.addPrintBufferToFileBuffer()
 
     def handleLongPrint(self, packet):
-        self.printBuffer = "Received Telemetry:"
+        self.printBuffer = "Received Telemetry: " + packet.printInfo
         print(self.printBuffer)
         self.addPrintBufferToFileBuffer()
         self.handleColumnHeaderPrint(packet)
@@ -60,6 +62,18 @@ class TmtcPrinter:
         print(self.printBuffer)
         self.addPrintBufferToFileBuffer()
 
+    def handleHkPrint(self, packet):
+        if g.printHkData:
+            self.printBuffer = "HK Data:"
+            print(self.printBuffer)
+            self.addPrintBufferToFileBuffer()
+            self.printBuffer = str(packet.hkHeader)
+            print(self.printBuffer)
+            self.addPrintBufferToFileBuffer()
+            self.printBuffer = str(packet.hkContent)
+            print(self.printBuffer)
+            self.addPrintBufferToFileBuffer()
+
     def handleWiretappingPacket(self, packet):
         if packet.getService() == 2 and (packet.getSubservice() == 131 or packet.getSubservice() == 130):
             self.printBuffer = "Wiretapping Packet or Raw Reply from TM [" + \
-- 
GitLab