From 537a30a4de78bd0f86a60a4b73cb861cd38c1a7e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 12:34:06 +0200
Subject: [PATCH 01/11] added pus a tc

---
 FSFW.h                          |  7 ++++++
 tmtcpacket/pus/CMakeLists.txt   |  3 ++-
 tmtcpacket/pus/TcPacketBase.h   | 25 ---------------------
 tmtcpacket/pus/TcPacketPusA.cpp |  3 +++
 tmtcpacket/pus/TcPacketPusA.h   | 40 +++++++++++++++++++++++++++++++++
 tmtcpacket/pus/TcPacketPusC.cpp |  4 ++++
 tmtcpacket/pus/TcPacketPusC.h   |  8 +++++++
 7 files changed, 64 insertions(+), 26 deletions(-)
 create mode 100644 FSFW.h
 create mode 100644 tmtcpacket/pus/TcPacketPusA.cpp
 create mode 100644 tmtcpacket/pus/TcPacketPusA.h
 create mode 100644 tmtcpacket/pus/TcPacketPusC.cpp
 create mode 100644 tmtcpacket/pus/TcPacketPusC.h

diff --git a/FSFW.h b/FSFW.h
new file mode 100644
index 000000000..df06ff3df
--- /dev/null
+++ b/FSFW.h
@@ -0,0 +1,7 @@
+#ifndef FSFW_FSFW_H_
+#define FSFW_FSFW_H_
+
+#include "FSFWConfig.h"
+
+
+#endif /* FSFW_FSFW_H_ */
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index cd4f49f17..2d22cb668 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,6 +1,7 @@
 target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
+	PRIVATE
 		TcPacketBase.cpp
+		TcPacketPusA.cpp
 		TcPacketStored.cpp
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 582a22140..451f9c3aa 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -4,31 +4,6 @@
 #include "../../tmtcpacket/SpacePacketBase.h"
 #include <cstddef>
 
-
-/**
- * This struct defines a byte-wise structured PUS TC Data Field Header.
- * Any optional fields in the header must be added or removed here.
- * Currently, the Source Id field is present with one byte.
- * @ingroup tmtcpackets
- */
-struct PUSTcDataFieldHeader {
-	uint8_t version_type_ack;
-	uint8_t service_type;
-	uint8_t service_subtype;
-	uint8_t source_id;
-};
-
-/**
- * This struct defines the data structure of a PUS Telecommand Packet when
- * accessed via a pointer.
- * @ingroup tmtcpackets
- */
-struct TcPacketPointer {
-	CCSDSPrimaryHeader primary;
-	PUSTcDataFieldHeader dataField;
-	uint8_t appData;
-};
-
 /**
  * This class is the basic data handler for any ECSS PUS Telecommand packet.
  *
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/TcPacketPusA.cpp
new file mode 100644
index 000000000..c9af38a63
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusA.cpp
@@ -0,0 +1,3 @@
+#include "TcPacketPusA.h"
+
+
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/TcPacketPusA.h
new file mode 100644
index 000000000..9254ad32f
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusA.h
@@ -0,0 +1,40 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+
+#include "ccsds_header.h"
+#include "TcPacketBase.h"
+
+#include <cstdint>
+
+/**
+ * This struct defines a byte-wise structured PUS TC A Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, the Source Id field is present with one byte.
+ * @ingroup tmtcpackets
+ */
+struct PUSTcDataFieldHeader {
+    uint8_t version_type_ack;
+    uint8_t service_type;
+    uint8_t service_subtype;
+    uint8_t source_id;
+};
+
+/**
+ * This struct defines the data structure of a PUS Telecommand A packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TcPacketPointer {
+    CCSDSPrimaryHeader primary;
+    PUSTcDataFieldHeader dataField;
+    uint8_t appData;
+};
+
+
+class TcPacketPusA: public TcPacketBase {
+public:
+private:
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_ */
diff --git a/tmtcpacket/pus/TcPacketPusC.cpp b/tmtcpacket/pus/TcPacketPusC.cpp
new file mode 100644
index 000000000..1f2946a85
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusC.cpp
@@ -0,0 +1,4 @@
+#include "TcPacketPusC.h"
+
+
+
diff --git a/tmtcpacket/pus/TcPacketPusC.h b/tmtcpacket/pus/TcPacketPusC.h
new file mode 100644
index 000000000..e965e752d
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_ */

From 1b6759020aafce39adfbf4e99870f7e8552378b1 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 12:38:24 +0200
Subject: [PATCH 02/11] added getter function

---
 tmtcpacket/pus/TcPacketBase.cpp | 4 ++++
 tmtcpacket/pus/TcPacketBase.h   | 7 +++++++
 2 files changed, 11 insertions(+)

diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index ca3e2a99a..50130c838 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -23,6 +23,10 @@ uint8_t TcPacketBase::getSubService() {
 	return tcData->dataField.service_subtype;
 }
 
+uint8_t TcPacketBase::getSourceId() {
+    return tcData->dataField.source_id;
+}
+
 uint8_t TcPacketBase::getAcknowledgeFlags() {
 	return tcData->dataField.version_type_ack & 0b00001111;
 }
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 582a22140..f5073eba9 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -108,6 +108,13 @@ public:
 	 * @return	The packet's PUS Service Subtype.
 	 */
 	uint8_t getSubService();
+	/**
+	 * The source ID can be used to have an additional identifier, e.g. for different ground
+	 * station.
+	 * @return
+	 */
+	uint8_t getSourceId();
+
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
 	 *

From d0f37b851b4de0fd71521cec5ff19ded413c2d3e Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 16:29:13 +0200
Subject: [PATCH 03/11] added pus c support for tc

---
 defaultcfg/fsfwconfig/FSFWConfig.h     |  21 +++--
 tcdistribution/CMakeLists.txt          |  11 +--
 tcdistribution/PUSDistributor.cpp      |   7 +-
 tcdistribution/PUSDistributor.h        |   8 +-
 tcdistribution/TcPacketCheck.cpp       |  58 ++++++------
 tcdistribution/TcPacketCheck.h         |  84 ++++++++---------
 tmtcpacket/CMakeLists.txt              |   7 +-
 tmtcpacket/pus/CMakeLists.txt          |  25 ++---
 tmtcpacket/pus/TcPacketBase.cpp        |  82 ++--------------
 tmtcpacket/pus/TcPacketBase.h          |  53 ++++-------
 tmtcpacket/pus/TcPacketPusA.cpp        |  72 ++++++++++++++
 tmtcpacket/pus/TcPacketPusA.h          |  47 +++++++++-
 tmtcpacket/pus/TcPacketStored.cpp      | 124 -------------------------
 tmtcpacket/pus/TcPacketStored.h        | 122 ++----------------------
 tmtcpacket/pus/TcPacketStoredBase.cpp  |  72 ++++++++++++++
 tmtcpacket/pus/TcPacketStoredBase.h    |  90 ++++++++++++++++++
 tmtcpacket/pus/TcPacketStoredIF.h      |  38 ++++++++
 tmtcpacket/pus/TcPacketStoredPusA.cpp  |  77 +++++++++++++++
 tmtcpacket/pus/TcPacketStoredPusA.h    |  53 +++++++++++
 tmtcpacket/pus/TcPacketStoredPusC.cpp  |  10 ++
 tmtcpacket/pus/TmPacketBase.cpp        |  15 +--
 tmtcservices/CommandingServiceBase.cpp |  44 +++++----
 tmtcservices/CommandingServiceBase.h   |   7 +-
 tmtcservices/PusServiceBase.h          |   6 +-
 tmtcservices/VerificationReporter.cpp  |  22 +++--
 25 files changed, 667 insertions(+), 488 deletions(-)
 delete mode 100644 tmtcpacket/pus/TcPacketStored.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredBase.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredBase.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredIF.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusA.cpp
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusA.h
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusC.cpp

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 1abdab4cd..5518e33c1 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -7,29 +7,30 @@
 //! Used to determine whether C++ ostreams are used which can increase
 //! the binary size significantly. If this is disabled,
 //! the C stdio functions can be used alternatively
-#define FSFW_CPP_OSTREAM_ENABLED 	1
+#define FSFW_CPP_OSTREAM_ENABLED 	    1
 
 //! More FSFW related printouts depending on level. Useful for development.
-#define FSFW_VERBOSE_LEVEL			1
+#define FSFW_VERBOSE_LEVEL			    1
 
 //! Can be used to completely disable printouts, even the C stdio ones.
 #if FSFW_CPP_OSTREAM_ENABLED == 0 && FSFW_VERBOSE_LEVEL == 0
-	#define FSFW_DISABLE_PRINTOUT 	0
+	#define FSFW_DISABLE_PRINTOUT 	    0
 #endif
 
-#define FSFW_USE_PUS_C_TELEMETRY    1
+#define FSFW_USE_PUS_C_TELEMETRY        1
+#define FSFW_USE_PUS_C_TELECOMMANDS     1
 
 //! Can be used to disable the ANSI color sequences for C stdio.
-#define FSFW_COLORED_OUTPUT 		1
+#define FSFW_COLORED_OUTPUT 		    1
 
 //! If FSFW_OBJ_EVENT_TRANSLATION is set to one,
 //! additional output which requires the translation files translateObjects
 //! and translateEvents (and their compiled source files)
-#define FSFW_OBJ_EVENT_TRANSLATION	0
+#define FSFW_OBJ_EVENT_TRANSLATION	    0
 
 #if FSFW_OBJ_EVENT_TRANSLATION == 1
 //! Specify whether info events are printed too.
-#define FSFW_DEBUG_INFO				1
+#define FSFW_DEBUG_INFO				    1
 #include "objects/translateObjects.h"
 #include "events/translateEvents.h"
 #else
@@ -37,17 +38,17 @@
 
 //! When using the newlib nano library, C99 support for stdio facilities
 //! will not be provided. This define should be set to 1 if this is the case.
-#define FSFW_NO_C99_IO 	            1
+#define FSFW_NO_C99_IO 	                1
 
 //! Specify whether a special mode store is used for Subsystem components.
-#define FSFW_USE_MODESTORE          0
+#define FSFW_USE_MODESTORE              0
 
 //! Defines if the real time scheduler for linux should be used.
 //! If set to 0, this will also disable priority settings for linux
 //! as most systems will not allow to set nice values without privileges
 //! For embedded linux system set this to 1.
 //! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run
-#define FSFW_USE_REALTIME_FOR_LINUX 1
+#define FSFW_USE_REALTIME_FOR_LINUX     1
 
 namespace fsfwconfig {
 
diff --git a/tcdistribution/CMakeLists.txt b/tcdistribution/CMakeLists.txt
index 17dc186c3..6f2370761 100644
--- a/tcdistribution/CMakeLists.txt
+++ b/tcdistribution/CMakeLists.txt
@@ -1,7 +1,6 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		CCSDSDistributor.cpp
-		PUSDistributor.cpp
-		TcDistributor.cpp
-		TcPacketCheck.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    CCSDSDistributor.cpp
+    PUSDistributor.cpp
+    TcDistributor.cpp
+    TcPacketCheck.cpp
 )
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 0fac9ba09..5c2f734b0 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -3,7 +3,6 @@
 
 #include "../objectmanager/ObjectManager.h"
 #include "../serviceinterface/ServiceInterface.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
 #define PUS_DISTRIBUTOR_DEBUGGING     0
@@ -119,7 +118,11 @@ uint16_t PUSDistributor::getIdentifier() {
 }
 
 ReturnValue_t PUSDistributor::initialize() {
-    currentPacket = new TcPacketStored();
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    currentPacket = new TcPacketStoredPusC();
+#else
+    currentPacket = new TcPacketStoredPusA();
+#endif
     if(currentPacket == nullptr) {
         // Should not happen, memory allocation failed!
         return ObjectManagerIF::CHILD_INIT_FAILED;
diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h
index be3804ef2..0c2884519 100644
--- a/tcdistribution/PUSDistributor.h
+++ b/tcdistribution/PUSDistributor.h
@@ -5,6 +5,7 @@
 #include "TcDistributor.h"
 #include "TcPacketCheck.h"
 
+#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/VerificationReporter.h"
@@ -51,7 +52,12 @@ protected:
 	/**
 	 * The currently handled packet is stored here.
 	 */
-	TcPacketStored* currentPacket = nullptr;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC* currentPacket = nullptr;
+#else
+	TcPacketStoredPusA* currentPacket = nullptr;
+#endif
+
 	/**
 	 * With this variable, the current check status is stored to generate
 	 * acceptance messages later.
diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index 38ed04aaf..e88305cce 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -1,39 +1,45 @@
 #include "TcPacketCheck.h"
 
 #include "../globalfunctions/CRC.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../tmtcpacket/pus/TcPacketBase.h"
+#include "../tmtcpacket/pus/TcPacketStoredBase.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../tmtcservices/VerificationCodes.h"
 
-TcPacketCheck::TcPacketCheck( uint16_t setApid ) : apid(setApid) {
+TcPacketCheck::TcPacketCheck(uint16_t setApid): apid(setApid) {
 }
 
-ReturnValue_t TcPacketCheck::checkPacket( TcPacketStored* currentPacket ) {
-	uint16_t calculated_crc = CRC::crc16ccitt( currentPacket->getWholeData(),
-	        currentPacket->getFullSize() );
-	if ( calculated_crc != 0 ) {
-		return INCORRECT_CHECKSUM;
-	}
-	bool condition = (not currentPacket->hasSecondaryHeader()) or
-			(currentPacket->getPacketVersionNumber() != CCSDS_VERSION_NUMBER) or
-			(not currentPacket->isTelecommand());
-	if ( condition ) {
-		return INCORRECT_PRIMARY_HEADER;
-	}
-	if ( currentPacket->getAPID() != this->apid )
-		return ILLEGAL_APID;
+ReturnValue_t TcPacketCheck::checkPacket(TcPacketStoredBase* currentPacket) {
+    TcPacketBase* tcPacketBase = currentPacket->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return RETURN_FAILED;
+    }
+    uint16_t calculated_crc = CRC::crc16ccitt(tcPacketBase->getWholeData(),
+            tcPacketBase->getFullSize());
+    if (calculated_crc != 0) {
+        return INCORRECT_CHECKSUM;
+    }
+    bool condition = (not tcPacketBase->hasSecondaryHeader()) or
+            (tcPacketBase->getPacketVersionNumber() != CCSDS_VERSION_NUMBER) or
+            (not tcPacketBase->isTelecommand());
+    if (condition) {
+        return INCORRECT_PRIMARY_HEADER;
+    }
+    if (tcPacketBase->getAPID() != this->apid)
+        return ILLEGAL_APID;
 
-	if ( not currentPacket->isSizeCorrect() ) {
-		return INCOMPLETE_PACKET;
-	}
-	condition = (currentPacket->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
-			(currentPacket->getPusVersionNumber() != PUS_VERSION_NUMBER);
-	if ( condition ) {
-		return INCORRECT_SECONDARY_HEADER;
-	}
-	return RETURN_OK;
+    if (not currentPacket->isSizeCorrect()) {
+        return INCOMPLETE_PACKET;
+    }
+    condition = (tcPacketBase->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
+            (tcPacketBase->getPusVersionNumber() != PUS_VERSION_NUMBER);
+    if (condition) {
+        return INCORRECT_SECONDARY_HEADER;
+    }
+    return RETURN_OK;
 }
 
 uint16_t TcPacketCheck::getApid() const {
-	return apid;
+    return apid;
 }
diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h
index 703bb1bb5..f7422c192 100644
--- a/tcdistribution/TcPacketCheck.h
+++ b/tcdistribution/TcPacketCheck.h
@@ -2,9 +2,10 @@
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 
 #include "../returnvalues/HasReturnvaluesIF.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
+class TcPacketStoredBase;
+
 /**
  * This class performs a formal packet check for incoming PUS Telecommand Packets.
  * Currently, it only checks if the APID and CRC are correct.
@@ -12,49 +13,48 @@
  */
 class TcPacketCheck : public HasReturnvaluesIF {
 protected:
-	/**
-	 * Describes the version number a packet must have to pass.
-	 */
-	static constexpr uint8_t CCSDS_VERSION_NUMBER = 0;
-	/**
-	 * Describes the secondary header a packet must have to pass.
-	 */
-	static constexpr uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0;
-	/**
-	 * Describes the TC Packet PUS Version Number a packet must have to pass.
-	 */
-	static constexpr uint8_t PUS_VERSION_NUMBER = 1;
-	/**
-	 * The packet id each correct packet should have.
-	 * It is composed of the APID and some static fields.
-	 */
-	uint16_t apid;
+    /**
+     * Describes the version number a packet must have to pass.
+     */
+    static constexpr uint8_t CCSDS_VERSION_NUMBER = 0;
+    /**
+     * Describes the secondary header a packet must have to pass.
+     */
+    static constexpr uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0;
+    /**
+     * Describes the TC Packet PUS Version Number a packet must have to pass.
+     */
+    static constexpr uint8_t PUS_VERSION_NUMBER = 1;
+    /**
+     * The packet id each correct packet should have.
+     * It is composed of the APID and some static fields.
+     */
+    uint16_t apid;
 public:
-	static const uint8_t INTERFACE_ID = CLASS_ID::TC_PACKET_CHECK;
-	static const ReturnValue_t ILLEGAL_APID = MAKE_RETURN_CODE( 0 );
-	static const ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE( 1 );
-	static const ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE( 2 );
-	static const ReturnValue_t ILLEGAL_PACKET_TYPE = MAKE_RETURN_CODE( 3 );
-	static const ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE( 4 );
-	static const ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE( 5 );
-	static const ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE( 6 );
-	/**
-	 * The constructor only sets the APID attribute.
-	 * @param set_apid The APID to set.
-	 */
-	TcPacketCheck( uint16_t setApid );
-	/**
-	 * This is the actual method to formally check a certain Telecommand Packet.
-	 * The packet's Application Data can not be checked here.
-	 * @param current_packet The packt to check
-	 * @return	- @c RETURN_OK on success.
-	 * 			- @c INCORRECT_CHECKSUM if checksum is invalid.
-	 * 			- @c ILLEGAL_APID if APID does not match.
-	 */
-	ReturnValue_t checkPacket( TcPacketStored* currentPacket );
+    static const uint8_t INTERFACE_ID = CLASS_ID::TC_PACKET_CHECK;
+    static const ReturnValue_t ILLEGAL_APID = MAKE_RETURN_CODE( 0 );
+    static const ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE( 1 );
+    static const ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE( 2 );
+    static const ReturnValue_t ILLEGAL_PACKET_TYPE = MAKE_RETURN_CODE( 3 );
+    static const ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE( 4 );
+    static const ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE( 5 );
+    static const ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE( 6 );
+    /**
+     * The constructor only sets the APID attribute.
+     * @param set_apid The APID to set.
+     */
+    TcPacketCheck(uint16_t setApid);
+    /**
+     * This is the actual method to formally check a certain Telecommand Packet.
+     * The packet's Application Data can not be checked here.
+     * @param current_packet The packt to check
+     * @return	- @c RETURN_OK on success.
+     * 			- @c INCORRECT_CHECKSUM if checksum is invalid.
+     * 			- @c ILLEGAL_APID if APID does not match.
+     */
+    ReturnValue_t checkPacket(TcPacketStoredBase* currentPacket);
 
-	uint16_t getApid() const;
+    uint16_t getApid() const;
 };
 
-
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_ */
diff --git a/tmtcpacket/CMakeLists.txt b/tmtcpacket/CMakeLists.txt
index fe3d2a4d2..fdc884ec5 100644
--- a/tmtcpacket/CMakeLists.txt
+++ b/tmtcpacket/CMakeLists.txt
@@ -1,7 +1,6 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		SpacePacket.cpp
-		SpacePacketBase.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    SpacePacket.cpp
+    SpacePacketBase.cpp
 )
 
 add_subdirectory(packetmatcher)
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 2d22cb668..9340e3b1f 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,13 +1,14 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE
-		TcPacketBase.cpp
-		TcPacketPusA.cpp
-		TcPacketStored.cpp
-		TmPacketBase.cpp
-		TmPacketMinimal.cpp
-		TmPacketStoredPusA.cpp
-		TmPacketStoredPusC.cpp
-		TmPacketPusA.cpp
-		TmPacketPusC.cpp
-		TmPacketStoredBase.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    TcPacketBase.cpp
+    TcPacketPusA.cpp
+    TcPacketStoredBase.cpp
+    TcPacketStoredPusA.cpp
+
+    TmPacketBase.cpp
+    TmPacketMinimal.cpp
+    TmPacketStoredPusA.cpp
+    TmPacketStoredPusC.cpp
+    TmPacketPusA.cpp
+    TmPacketPusC.cpp
+    TmPacketStoredBase.cpp
 )
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index ca3e2a99a..a5b6f9ce0 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -2,87 +2,19 @@
 
 #include "../../globalfunctions/CRC.h"
 #include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 #include <cstring>
 
-TcPacketBase::TcPacketBase(const uint8_t* setData) :
-		SpacePacketBase(setData) {
-	tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
-}
+TcPacketBase::TcPacketBase(const uint8_t* setData): SpacePacketBase(setData) {}
 
-TcPacketBase::~TcPacketBase() {
-	//Nothing to do.
-}
-
-uint8_t TcPacketBase::getService() {
-	return tcData->dataField.service_type;
-}
-
-uint8_t TcPacketBase::getSubService() {
-	return tcData->dataField.service_subtype;
-}
-
-uint8_t TcPacketBase::getAcknowledgeFlags() {
-	return tcData->dataField.version_type_ack & 0b00001111;
-}
-
-const uint8_t* TcPacketBase::getApplicationData() const {
-	return &tcData->appData;
-}
-
-uint16_t TcPacketBase::getApplicationDataSize() {
-	return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
-}
-
-uint16_t TcPacketBase::getErrorControl() {
-	uint16_t size = getApplicationDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tcData->appData;
-	return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
-}
-
-void TcPacketBase::setErrorControl() {
-	uint32_t full_size = getFullSize();
-	uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
-	uint32_t size = getApplicationDataSize();
-	(&tcData->appData)[size] = (crc & 0XFF00) >> 8;	// CRCH
-	(&tcData->appData)[size + 1] = (crc) & 0X00FF; 		// CRCL
-}
-
-void TcPacketBase::setData(const uint8_t* pData) {
-	SpacePacketBase::setData(pData);
-	tcData = (TcPacketPointer*) pData;
-}
-
-uint8_t TcPacketBase::getSecondaryHeaderFlag() {
-	return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
-}
-
-uint8_t TcPacketBase::getPusVersionNumber() {
-	return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
-}
+TcPacketBase::~TcPacketBase() {}
 
 void TcPacketBase::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "TcPacketBase::print: " << std::endl;
+    sif::info << "TcPacketBase::print:" << std::endl;
+#else
+    sif::printInfo("TcPacketBase::print:\n");
 #endif
-	arrayprinter::print(getWholeData(), getFullSize());
-}
-
-void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
-        uint8_t ack, uint8_t service, uint8_t subservice) {
-	initSpacePacketHeader(true, true, apid, sequenceCount);
-	std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
-	setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-	//Data Field Header:
-	//Set CCSDS_secondary_header_flag to 0 and version number to 001
-	tcData->dataField.version_type_ack = 0b00010000;
-	tcData->dataField.version_type_ack |= (ack & 0x0F);
-	tcData->dataField.service_type = service;
-	tcData->dataField.service_subtype = subservice;
-}
-
-size_t TcPacketBase::calculateFullPacketLength(size_t appDataLen) {
-	return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
-			appDataLen + TcPacketBase::CRC_SIZE;
+    arrayprinter::print(getWholeData(), getFullSize());
 }
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index 451f9c3aa..80f9e5055 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -16,9 +16,8 @@
  * @ingroup tmtcpackets
  */
 class TcPacketBase : public SpacePacketBase {
+    friend class TcPacketStoredBase;
 public:
-	static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
-	        sizeof(PUSTcDataFieldHeader) + 2);
 
 	enum AckField {
 		//! No acknowledgements are expected.
@@ -54,7 +53,7 @@ public:
 	 * highest bit of the first byte of the Data Field Header.
 	 * @return	the CCSDS Secondary Header Flag
 	 */
-	uint8_t getSecondaryHeaderFlag();
+	virtual uint8_t getSecondaryHeaderFlag() = 0;
 	/**
 	 * This command returns the TC Packet PUS Version Number.
 	 * The version number of ECSS PUS 2003 is 1.
@@ -62,7 +61,7 @@ public:
 	 * first byte.
 	 * @return
 	 */
-	uint8_t getPusVersionNumber();
+	virtual uint8_t getPusVersionNumber() = 0;
 	/**
 	 * This is a getter for the packet's Ack field, which are the lowest four
 	 * bits of the first byte of the Data Field Header.
@@ -70,19 +69,19 @@ public:
 	 * It is packed in a uint8_t variable.
 	 * @return	The packet's PUS Ack field.
 	 */
-	uint8_t getAcknowledgeFlags();
+	virtual uint8_t getAcknowledgeFlags() = 0;
 	/**
 	 * This is a getter for the packet's PUS Service ID, which is the second
 	 * byte of the Data Field Header.
 	 * @return	The packet's PUS Service ID.
 	 */
-	uint8_t getService();
+	virtual uint8_t getService() const = 0;
 	/**
 	 * This is a getter for the packet's PUS Service Subtype, which is the
 	 * third byte of the Data Field Header.
 	 * @return	The packet's PUS Service Subtype.
 	 */
-	uint8_t getSubService();
+	virtual uint8_t getSubService() = 0;
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
 	 *
@@ -90,7 +89,7 @@ public:
 	 * the packet's application data.
 	 * @return	A pointer to the PUS Application Data.
 	 */
-	const uint8_t* getApplicationData() const;
+	virtual const uint8_t* getApplicationData() const = 0;
 	/**
 	 * This method calculates the size of the PUS Application data field.
 	 *
@@ -99,7 +98,7 @@ public:
 	 * @return	The size of the PUS Application Data (without Error Control
 	 * 		field)
 	 */
-	uint16_t getApplicationDataSize();
+	virtual uint16_t getApplicationDataSize() = 0;
 	/**
 	 * This getter returns the Error Control Field of the packet.
 	 *
@@ -108,44 +107,26 @@ public:
 	 * supposed to be a 16bit-CRC.
 	 * @return	The PUS Error Control
 	 */
-	uint16_t getErrorControl();
+	virtual uint16_t getErrorControl() = 0;
 	/**
 	 * With this method, the Error Control Field is updated to match the
 	 * current content of the packet.
 	 */
-	void setErrorControl();
+	virtual void setErrorControl() = 0;
 
-	/**
-	 * This is a debugging helper method that prints the whole packet content
-	 * to the screen.
-	 */
-	void print();
 	/**
 	 * Calculate full packet length from application data length.
 	 * @param appDataLen
 	 * @return
 	 */
-	static size_t calculateFullPacketLength(size_t appDataLen);
+	virtual size_t calculateFullPacketLength(size_t appDataLen) = 0;
 
+    /**
+     * This is a debugging helper method that prints the whole packet content
+     * to the screen.
+     */
+    void print();
 protected:
-    /**
-     * A pointer to a structure which defines the data structure of
-     * the packet's data.
-     *
-     * To be hardware-safe, all elements are of byte size.
-     */
-    TcPacketPointer* tcData;
-
-    /**
-     * Initializes the Tc Packet header.
-     * @param apid APID used.
-     * @param sequenceCount Sequence Count in the primary header.
-     * @param ack Which acknowledeges are expected from the receiver.
-     * @param service   PUS Service
-     * @param subservice PUS Subservice
-     */
-    void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
-            uint8_t service, uint8_t subservice);
 
     /**
      * With this method, the packet data pointer can be redirected to another
@@ -155,7 +136,7 @@ protected:
      *
      * @param p_data    A pointer to another PUS Telecommand Packet.
      */
-    void setData( const uint8_t* pData );
+    void setData( const uint8_t* pData ) = 0;
 };
 
 
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/TcPacketPusA.cpp
index c9af38a63..0ce767b47 100644
--- a/tmtcpacket/pus/TcPacketPusA.cpp
+++ b/tmtcpacket/pus/TcPacketPusA.cpp
@@ -1,3 +1,75 @@
 #include "TcPacketPusA.h"
+#include "../../globalfunctions/CRC.h"
 
+#include <cstring>
 
+TcPacketPusA::TcPacketPusA(const uint8_t *setData): TcPacketBase(setData) {
+    tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
+}
+
+uint8_t TcPacketPusA::getService() const {
+    return tcData->dataField.service_type;
+}
+
+uint8_t TcPacketPusA::getSubService() {
+    return tcData->dataField.service_subtype;
+}
+
+uint8_t TcPacketPusA::getAcknowledgeFlags() {
+    return tcData->dataField.version_type_ack & 0b00001111;
+}
+
+const uint8_t* TcPacketPusA::getApplicationData() const {
+    return &tcData->appData;
+}
+
+uint16_t TcPacketPusA::getApplicationDataSize() {
+    return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
+}
+
+uint16_t TcPacketPusA::getErrorControl() {
+    uint16_t size = getApplicationDataSize() + CRC_SIZE;
+    uint8_t* p_to_buffer = &tcData->appData;
+    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+}
+
+void TcPacketPusA::setErrorControl() {
+    uint32_t full_size = getFullSize();
+    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
+    uint32_t size = getApplicationDataSize();
+    (&tcData->appData)[size] = (crc & 0XFF00) >> 8; // CRCH
+    (&tcData->appData)[size + 1] = (crc) & 0X00FF;      // CRCL
+}
+
+void TcPacketPusA::setData(const uint8_t* pData) {
+    SpacePacketBase::setData(pData);
+    // This function is const-correct, but it was decided to keep the pointer non-const
+    // for convenience. Therefore, cast aways constness here and then cast to packet type.
+    tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
+}
+
+uint8_t TcPacketPusA::getSecondaryHeaderFlag() {
+    return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
+}
+
+uint8_t TcPacketPusA::getPusVersionNumber() {
+    return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
+}
+
+void TcPacketPusA::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+        uint8_t ack, uint8_t service, uint8_t subservice) {
+    initSpacePacketHeader(true, true, apid, sequenceCount);
+    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
+    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    // Data Field Header:
+    // Set CCSDS_secondary_header_flag to 0 and version number to 001
+    tcData->dataField.version_type_ack = 0b00010000;
+    tcData->dataField.version_type_ack |= (ack & 0x0F);
+    tcData->dataField.service_type = service;
+    tcData->dataField.service_subtype = subservice;
+}
+
+size_t TcPacketPusA::calculateFullPacketLength(size_t appDataLen) {
+    return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
+            appDataLen + TcPacketBase::CRC_SIZE;
+}
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/TcPacketPusA.h
index 9254ad32f..0e61d829f 100644
--- a/tmtcpacket/pus/TcPacketPusA.h
+++ b/tmtcpacket/pus/TcPacketPusA.h
@@ -1,7 +1,7 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 
-#include "ccsds_header.h"
+#include "../ccsds_header.h"
 #include "TcPacketBase.h"
 
 #include <cstdint>
@@ -33,7 +33,50 @@ struct TcPacketPointer {
 
 class TcPacketPusA: public TcPacketBase {
 public:
-private:
+    static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTcDataFieldHeader) + 2);
+
+    /**
+     * Initialize a PUS A telecommand packet which already exists. You can also
+     * create an empty (invalid) object by passing nullptr as the data pointer
+     * @param setData
+     */
+    TcPacketPusA(const uint8_t* setData);
+
+    // Base class overrides
+    virtual uint8_t getSecondaryHeaderFlag() override;
+    virtual uint8_t getPusVersionNumber() override;
+    virtual uint8_t getAcknowledgeFlags() override;
+    virtual uint8_t getService() const override;
+    virtual uint8_t getSubService() override;
+    const uint8_t* getApplicationData() const override;
+    uint16_t getApplicationDataSize() override;
+    uint16_t getErrorControl() override;
+    void setErrorControl() override;
+    size_t calculateFullPacketLength(size_t appDataLen) override;
+
+protected:
+
+    void setData(const uint8_t* pData) override;
+
+    /**
+     * Initializes the Tc Packet header.
+     * @param apid APID used.
+     * @param sequenceCount Sequence Count in the primary header.
+     * @param ack Which acknowledeges are expected from the receiver.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     */
+    void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
+            uint8_t service, uint8_t subservice);
+
+    /**
+     * A pointer to a structure which defines the data structure of
+     * the packet's data.
+     *
+     * To be hardware-safe, all elements are of byte size.
+     */
+    TcPacketPointer* tcData = nullptr;
 };
 
 
diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp
deleted file mode 100644
index 36fa8d11f..000000000
--- a/tmtcpacket/pus/TcPacketStored.cpp
+++ /dev/null
@@ -1,124 +0,0 @@
-#include "TcPacketStored.h"
-
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-
-#include <cstring>
-
-StorageManagerIF* TcPacketStored::store = nullptr;
-
-TcPacketStored::TcPacketStored(store_address_t setAddress) :
-		TcPacketBase(nullptr), storeAddress(setAddress) {
-	setStoreAddress(storeAddress);
-}
-
-TcPacketStored::TcPacketStored(uint16_t apid,  uint8_t service,
-        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
-		size_t size, uint8_t ack) :
-		TcPacketBase(nullptr) {
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not this->checkAndSetStore()) {
-		return;
-	}
-	uint8_t* pData = nullptr;
-	ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
-			(TC_PACKET_MIN_SIZE + size), &pData);
-	if (returnValue != this->store->RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStored: Could not get free element from store!"
-				<< std::endl;
-#endif
-		return;
-	}
-	this->setData(pData);
-	initializeTcPacket(apid, sequenceCount, ack, service, subservice);
-	memcpy(&tcData->appData, data, size);
-	this->setPacketDataLength(
-			size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-	this->setErrorControl();
-}
-
-ReturnValue_t TcPacketStored::getData(const uint8_t ** dataPtr,
-		size_t* dataSize) {
-	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStored: Could not get data!" << std::endl;
-#endif
-	}
-	return result;
-}
-
-TcPacketStored::TcPacketStored(): TcPacketBase(nullptr) {
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	this->checkAndSetStore();
-
-}
-
-ReturnValue_t TcPacketStored::deletePacket() {
-	ReturnValue_t result = this->store->deleteData(this->storeAddress);
-	this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	this->setData(nullptr);
-	return result;
-}
-
-bool TcPacketStored::checkAndSetStore() {
-	if (this->store == nullptr) {
-		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
-		if (this->store == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TcPacketStored::TcPacketStored: TC Store not found!"
-					<< std::endl;
-#endif
-			return false;
-		}
-	}
-	return true;
-}
-
-void TcPacketStored::setStoreAddress(store_address_t setAddress) {
-	this->storeAddress = setAddress;
-	const uint8_t* tempData = nullptr;
-	size_t temp_size;
-	ReturnValue_t status = StorageManagerIF::RETURN_FAILED;
-	if (this->checkAndSetStore()) {
-		status = this->store->getData(this->storeAddress, &tempData,
-				&temp_size);
-	}
-	if (status == StorageManagerIF::RETURN_OK) {
-		this->setData(tempData);
-	} else {
-		this->setData(nullptr);
-		this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	}
-}
-
-store_address_t TcPacketStored::getStoreAddress() {
-	return this->storeAddress;
-}
-
-bool TcPacketStored::isSizeCorrect() {
-	const uint8_t* temp_data = nullptr;
-	size_t temp_size;
-	ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
-			&temp_size);
-	if (status == StorageManagerIF::RETURN_OK) {
-		if (this->getFullSize() == temp_size) {
-			return true;
-		}
-	}
-	return false;
-}
-
-TcPacketStored::TcPacketStored(const uint8_t* data, uint32_t size) :
-		TcPacketBase(data) {
-	if (getFullSize() != size) {
-		return;
-	}
-	if (this->checkAndSetStore()) {
-		ReturnValue_t status = store->addData(&storeAddress, data, size);
-		if (status != HasReturnvaluesIF::RETURN_OK) {
-			this->setData(nullptr);
-		}
-	}
-}
diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h
index 1666107b7..0b6ff0f4c 100644
--- a/tmtcpacket/pus/TcPacketStored.h
+++ b/tmtcpacket/pus/TcPacketStored.h
@@ -1,117 +1,13 @@
-#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
-#define TMTCPACKET_PUS_TCPACKETSTORED_H_
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
 
-#include "TcPacketBase.h"
-#include "../../storagemanager/StorageManagerIF.h"
+#include <FSFWConfig.h>
 
-/**
- *	This class generates a ECSS PUS Telecommand packet within a given
- *	intermediate storage.
- *	As most packets are passed between tasks with the help of a storage
- *	anyway, it seems logical to create a Packet-In-Storage access class
- *	which saves the user almost all storage handling operation.
- *	Packets can both be newly created with the class and be "linked" to
- *	packets in a store with the help of a storeAddress.
- *	@ingroup tmtcpackets
- */
-class TcPacketStored : public TcPacketBase {
-public:
-	/**
-	 * This is a default constructor which does not set the data pointer.
-	 * However, it does try to set the packet store.
-	 */
-	TcPacketStored();
-	/**
-	 * With this constructor, the class instance is linked to an existing
-	 * packet in the packet store.
-	 * The packet content is neither checked nor changed with this call. If
-	 * the packet could not be found, the data pointer is set to NULL.
-	 */
-	TcPacketStored( store_address_t setAddress );
-	/**
-	 * With this constructor, new space is allocated in the packet store and
-	 * a new PUS Telecommand Packet is created there.
-	 * Packet Application Data passed in data is copied into the packet.
-	 * @param apid			Sets the packet's APID field.
-	 * @param service		Sets the packet's Service ID field.
-	 *                      This specifies the destination service.
-	 * @param subservice	Sets the packet's Service Subtype field.
-	 * 		                This specifies the destination sub-service.
-	 * @param sequence_count Sets the packet's Source Sequence Count field.
-	 * @param data		The data to be copied to the Application Data Field.
-	 * @param size		The amount of data to be copied.
-	 * @param ack           Set's the packet's Ack field, which specifies
-	 *                      number of verification packets returned
-     *                      for this command.
-	 */
-	TcPacketStored(uint16_t apid, uint8_t service, uint8_t subservice,
-			uint8_t sequence_count = 0, const uint8_t* data = nullptr,
-			size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
-	/**
-	 * Another constructor to create a TcPacket from a raw packet stream.
-	 * Takes the data and adds it unchecked to the TcStore.
-	 * @param data	Pointer to the complete TC Space Packet.
-	 * @param Size size of the packet.
-	 */
-	TcPacketStored( const uint8_t* data, uint32_t size);
-
-	/**
-	 * Getter function for the raw data.
-	 * @param dataPtr [out] Pointer to the data pointer to set
-	 * @param dataSize [out] Address of size to set.
-	 * @return -@c RETURN_OK if data was retrieved successfully.
-	 */
-	ReturnValue_t getData(const uint8_t ** dataPtr,
-			size_t* dataSize);
-	/**
-	 * This is a getter for the current store address of the packet.
-	 * @return	The current store address. The (raw) value is
-	 * 			@c StorageManagerIF::INVALID_ADDRESS if the packet is not linked.
-	 */
-	store_address_t getStoreAddress();
-	/**
-	 * With this call, the packet is deleted.
-	 * It removes itself from the store and sets its data pointer to NULL.
-	 * @return	returncode from deleting the data.
-	 */
-	ReturnValue_t deletePacket();
-	/**
-	 * With this call, a packet can be linked to another store. This is useful
-	 * if the packet is a class member and used for more than one packet.
-	 * @param setAddress	The new packet id to link to.
-	 */
-	void setStoreAddress( store_address_t setAddress );
-	/**
-	 * This method performs a size check.
-	 * It reads the stored size and compares it with the size entered in the
-	 * packet header. This class is the optimal place for such a check as it
-	 * has access to both the header data and the store.
-	 * @return	true if size is correct, false if packet is not registered in
-	 * 			store or size is incorrect.
-	 */
-	bool isSizeCorrect();
-
-private:
-    /**
-     * This is a pointer to the store all instances of the class use.
-     * If the store is not yet set (i.e. @c store is NULL), every constructor
-     * call tries to set it and throws an error message in case of failures.
-     * The default store is objects::TC_STORE.
-     */
-    static StorageManagerIF* store;
-    /**
-     * The address where the packet data of the object instance is stored.
-     */
-    store_address_t storeAddress;
-    /**
-     * A helper method to check if a store is assigned to the class.
-     * If not, the method tries to retrieve the store from the global
-     * ObjectManager.
-     * @return  @li @c true if the store is linked or could be created.
-     *          @li @c false otherwise.
-     */
-    bool checkAndSetStore();
-};
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+#include "TcPacketStoredPusC.h"
+#else
+#include "TcPacketStoredPusA.h"
+#endif
 
 
-#endif /* TMTCPACKET_PUS_TCPACKETSTORED_H_ */
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredBase.cpp b/tmtcpacket/pus/TcPacketStoredBase.cpp
new file mode 100644
index 000000000..629562c01
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredBase.cpp
@@ -0,0 +1,72 @@
+#include "TcPacketStoredBase.h"
+
+#include "../../objectmanager/ObjectManager.h"
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../objectmanager/frameworkObjects.h"
+
+#include <cstring>
+
+StorageManagerIF* TcPacketStoredBase::store = nullptr;
+
+TcPacketStoredBase::TcPacketStoredBase() {
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    this->checkAndSetStore();
+
+}
+
+TcPacketStoredBase::~TcPacketStoredBase() {
+}
+
+ReturnValue_t TcPacketStoredBase::getData(const uint8_t ** dataPtr,
+		size_t* dataSize) {
+	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
+#else
+		sif::printWarning("TcPacketStoredBase: Could not get data!\n");
+#endif
+	}
+	return result;
+}
+
+
+
+bool TcPacketStoredBase::checkAndSetStore() {
+	if (this->store == nullptr) {
+		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+		if (this->store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+			sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
+					<< std::endl;
+#endif
+			return false;
+		}
+	}
+	return true;
+}
+
+void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
+    this->storeAddress = setAddress;
+    const uint8_t* tempData = nullptr;
+    size_t tempSize;
+    ReturnValue_t status = StorageManagerIF::RETURN_FAILED;
+    if (this->checkAndSetStore()) {
+        status = this->store->getData(this->storeAddress, &tempData, &tempSize);
+    }
+    TcPacketBase* tcPacketBase = this->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return;
+    }
+    if (status == StorageManagerIF::RETURN_OK) {
+        tcPacketBase->setData(tempData);
+    }
+    else {
+        tcPacketBase->setData(nullptr);
+        this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    }
+}
+
+store_address_t TcPacketStoredBase::getStoreAddress() {
+	return this->storeAddress;
+}
diff --git a/tmtcpacket/pus/TcPacketStoredBase.h b/tmtcpacket/pus/TcPacketStoredBase.h
new file mode 100644
index 000000000..811123db4
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredBase.h
@@ -0,0 +1,90 @@
+#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
+#define TMTCPACKET_PUS_TCPACKETSTORED_H_
+
+#include <fsfw/tmtcpacket/pus/TcPacketStoredIF.h>
+#include "../../storagemanager/StorageManagerIF.h"
+
+/**
+ *	This class generates a ECSS PUS Telecommand packet within a given
+ *	intermediate storage.
+ *	As most packets are passed between tasks with the help of a storage
+ *	anyway, it seems logical to create a Packet-In-Storage access class
+ *	which saves the user almost all storage handling operation.
+ *	Packets can both be newly created with the class and be "linked" to
+ *	packets in a store with the help of a storeAddress.
+ *	@ingroup tmtcpackets
+ */
+class TcPacketStoredBase: public TcPacketStoredIF {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer to initialize
+     * with an empty cached store address
+     */
+    TcPacketStoredBase();
+    /**
+     * Constructor to set to an existing store address.
+     * @param setAddress
+     */
+    TcPacketStoredBase(store_address_t setAddress);
+    /**
+     * Another constructor to create a TcPacket from a raw packet stream.
+     * Takes the data and adds it unchecked to the TcStore.
+     * @param data  Pointer to the complete TC Space Packet.
+     * @param Size size of the packet.
+     */
+    TcPacketStoredBase(const uint8_t* data, uint32_t size);
+
+	virtual~ TcPacketStoredBase();
+
+	/**
+	 * Getter function for the raw data.
+	 * @param dataPtr [out] Pointer to the data pointer to set
+	 * @param dataSize [out] Address of size to set.
+	 * @return -@c RETURN_OK if data was retrieved successfully.
+	 */
+	ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
+
+    void setStoreAddress(store_address_t setAddress) override;
+    store_address_t getStoreAddress() override;
+
+	/**
+	 * With this call, the packet is deleted.
+	 * It removes itself from the store and sets its data pointer to NULL.
+	 * @return	returncode from deleting the data.
+	 */
+	virtual ReturnValue_t deletePacket() = 0;
+
+	/**
+	 * This method performs a size check.
+	 * It reads the stored size and compares it with the size entered in the
+	 * packet header. This class is the optimal place for such a check as it
+	 * has access to both the header data and the store.
+	 * @return	true if size is correct, false if packet is not registered in
+	 * 			store or size is incorrect.
+	 */
+	virtual bool isSizeCorrect() = 0;
+
+protected:
+    /**
+     * This is a pointer to the store all instances of the class use.
+     * If the store is not yet set (i.e. @c store is NULL), every constructor
+     * call tries to set it and throws an error message in case of failures.
+     * The default store is objects::TC_STORE.
+     */
+    static StorageManagerIF* store;
+    /**
+     * The address where the packet data of the object instance is stored.
+     */
+    store_address_t storeAddress;
+    /**
+     * A helper method to check if a store is assigned to the class.
+     * If not, the method tries to retrieve the store from the global
+     * ObjectManager.
+     * @return  @li @c true if the store is linked or could be created.
+     *          @li @c false otherwise.
+     */
+    bool checkAndSetStore();
+};
+
+
+#endif /* TMTCPACKET_PUS_TcPacketStoredBase_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredIF.h b/tmtcpacket/pus/TcPacketStoredIF.h
new file mode 100644
index 000000000..8eb23004f
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredIF.h
@@ -0,0 +1,38 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+
+#include "../../tmtcpacket/pus/TcPacketBase.h"
+#include "../../storagemanager/storeAddress.h"
+#include "../../returnvalues/HasReturnvaluesIF.h"
+
+class TcPacketStoredIF {
+public:
+    virtual~TcPacketStoredIF() {};
+
+    /**
+     * With this call, the stored packet can be set to another packet in a store. This is useful
+     * if the packet is a class member and used for more than one packet.
+     * @param setAddress    The new packet id to link to.
+     */
+    virtual void setStoreAddress(store_address_t setAddress) = 0;
+
+    virtual store_address_t getStoreAddress() = 0;
+
+    /**
+     * Getter function for the raw data.
+     * @param dataPtr [out]     Pointer to the data pointer to set
+     * @param dataSize [out]    Address of size to set.
+     * @return -@c RETURN_OK if data was retrieved successfully.
+     */
+    virtual ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) = 0;
+
+    /**
+     * Get packet base pointer which can be used to get access to PUS packet fields
+     * @return
+     */
+    virtual TcPacketBase* getPacketBase() = 0;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.cpp b/tmtcpacket/pus/TcPacketStoredPusA.cpp
new file mode 100644
index 000000000..7c706a1a4
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusA.cpp
@@ -0,0 +1,77 @@
+#include "TcPacketStoredPusA.h"
+
+#include <cstring>
+
+TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
+        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
+        size_t size, uint8_t ack) :
+        TcPacketPusA(nullptr) {
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not this->checkAndSetStore()) {
+        return;
+    }
+    uint8_t* pData = nullptr;
+    ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
+            (TC_PACKET_MIN_SIZE + size), &pData);
+    if (returnValue != this->store->RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "TcPacketStoredBase: Could not get free element from store!"
+                << std::endl;
+#endif
+        return;
+    }
+    this->setData(pData);
+    initializeTcPacket(apid, sequenceCount, ack, service, subservice);
+    std::memcpy(&tcData->appData, data, size);
+    this->setPacketDataLength(
+            size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    this->setErrorControl();
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(): TcPacketStoredBase(), TcPacketPusA(nullptr) {
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(store_address_t setAddress): TcPacketPusA(nullptr) {
+    TcPacketStoredBase::setStoreAddress(setAddress);
+}
+
+TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPacketPusA(data) {
+    if (this->getFullSize() != size) {
+        return;
+    }
+    if (this->checkAndSetStore()) {
+        ReturnValue_t status = store->addData(&storeAddress, data, size);
+        if (status != HasReturnvaluesIF::RETURN_OK) {
+            this->setData(nullptr);
+        }
+        const uint8_t* storePtr = nullptr;
+        // Repoint base data pointer to the data in the store.
+        store->getData(storeAddress, &storePtr, &size);
+        this->setData(storePtr);
+    }
+}
+
+ReturnValue_t TcPacketStoredPusA::deletePacket() {
+    ReturnValue_t result = this->store->deleteData(this->storeAddress);
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    this->setData(nullptr);
+    return result;
+}
+
+TcPacketBase* TcPacketStoredPusA::getPacketBase() {
+    return this;
+}
+
+
+bool TcPacketStoredPusA::isSizeCorrect() {
+    const uint8_t* temp_data = nullptr;
+    size_t temp_size;
+    ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
+            &temp_size);
+    if (status == StorageManagerIF::RETURN_OK) {
+        if (this->getFullSize() == temp_size) {
+            return true;
+        }
+    }
+    return false;
+}
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.h b/tmtcpacket/pus/TcPacketStoredPusA.h
new file mode 100644
index 000000000..fe1f64f7a
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusA.h
@@ -0,0 +1,53 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+
+#include "TcPacketStoredBase.h"
+#include "TcPacketPusA.h"
+
+class TcPacketStoredPusA:
+        public TcPacketStoredBase,
+        public TcPacketPusA {
+public:
+    /**
+     * With this constructor, new space is allocated in the packet store and
+     * a new PUS Telecommand Packet is created there.
+     * Packet Application Data passed in data is copied into the packet.
+     * @param apid          Sets the packet's APID field.
+     * @param service       Sets the packet's Service ID field.
+     *                      This specifies the destination service.
+     * @param subservice    Sets the packet's Service Subtype field.
+     *                      This specifies the destination sub-service.
+     * @param sequence_count Sets the packet's Source Sequence Count field.
+     * @param data      The data to be copied to the Application Data Field.
+     * @param size      The amount of data to be copied.
+     * @param ack           Set's the packet's Ack field, which specifies
+     *                      number of verification packets returned
+     *                      for this command.
+     */
+    TcPacketStoredPusA(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint8_t sequence_count = 0, const uint8_t* data = nullptr,
+            size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
+    /**
+     * Create stored packet with existing data.
+     * @param data
+     * @param size
+     */
+    TcPacketStoredPusA(const uint8_t* data, size_t size);
+    /**
+     * Create stored packet from existing packet in store
+     * @param setAddress
+     */
+    TcPacketStoredPusA(store_address_t setAddress);
+    TcPacketStoredPusA();
+
+    ReturnValue_t deletePacket() override;
+    TcPacketBase* getPacketBase() override;
+
+private:
+
+    bool isSizeCorrect() override;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
new file mode 100644
index 000000000..f568d1390
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusC.cpp
@@ -0,0 +1,10 @@
+/*
+ * TcPacketStoredPusC.cpp
+ *
+ *  Created on: Jun 13, 2021
+ *      Author: rmueller
+ */
+
+
+
+
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index acd69b650..b144db1bd 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -44,13 +44,6 @@ ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
             &tempSize, getTimestampSize());
 }
 
-void TmPacketBase::print() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::debug << "TmPacketBase::print: " << std::endl;
-#endif
-    arrayprinter::print(getWholeData(), getFullSize());
-}
-
 bool TmPacketBase::checkAndSetStamper() {
     if (timeStamper == NULL) {
         timeStamper = ObjectManager::instance()->get<TimeStamperIF>(timeStamperId);
@@ -66,3 +59,11 @@ bool TmPacketBase::checkAndSetStamper() {
     return true;
 }
 
+void TmPacketBase::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::info << "TmPacketBase::print:" << std::endl;
+#else
+    sif::printInfo("TmPacketBase::print:\n");
+#endif
+    arrayprinter::print(getWholeData(), getFullSize());
+}
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 863cba4fb..0ede27dc1 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -246,7 +246,11 @@ void CommandingServiceBase::handleRequestQueue() {
 	TmTcMessage message;
 	ReturnValue_t result;
 	store_address_t address;
-	TcPacketStored packet;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC packet;
+#else
+	TcPacketStoredPusA packet;
+#endif
 	MessageQueueId_t queue;
 	object_id_t objectId;
 	for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
@@ -351,14 +355,18 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 }
 
 
-void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
+void CommandingServiceBase::startExecution(TcPacketStoredBase *storedPacket,
         CommandMapIter iter) {
     ReturnValue_t result = RETURN_OK;
     CommandMessage command;
-    iter->second.subservice = storedPacket->getSubService();
+    TcPacketBase* tcPacketBase = storedPacket->getPacketBase();
+    if(tcPacketBase == nullptr) {
+        return;
+    }
+    iter->second.subservice = tcPacketBase->getSubService();
     result = prepareCommand(&command, iter->second.subservice,
-            storedPacket->getApplicationData(),
-            storedPacket->getApplicationDataSize(), &iter->second.state,
+            tcPacketBase->getApplicationData(),
+            tcPacketBase->getApplicationDataSize(), &iter->second.state,
             iter->second.objectId);
 
     ReturnValue_t sendResult = RETURN_OK;
@@ -371,12 +379,12 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 		if (sendResult == RETURN_OK) {
 			Clock::getUptime(&iter->second.uptimeOfStart);
 			iter->second.step = 0;
-			iter->second.subservice = storedPacket->getSubService();
+			iter->second.subservice = tcPacketBase->getSubService();
 			iter->second.command = command.getCommand();
-			iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-			iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
+			iter->second.tcInfo.ackFlags = tcPacketBase->getAcknowledgeFlags();
+			iter->second.tcInfo.tcPacketId = tcPacketBase->getPacketId();
 			iter->second.tcInfo.tcSequenceControl =
-					storedPacket->getPacketSequenceControl();
+			        tcPacketBase->getPacketSequenceControl();
 			acceptPacket(tc_verification::START_SUCCESS, storedPacket);
 		} else {
 			command.clearCommandMessage();
@@ -392,7 +400,7 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 		}
 		if (sendResult == RETURN_OK) {
 			verificationReporter.sendSuccessReport(tc_verification::START_SUCCESS,
-					storedPacket);
+					storedPacket->getPacketBase());
 			acceptPacket(tc_verification::COMPLETION_SUCCESS, storedPacket);
 			checkAndExecuteFifo(iter);
 		} else {
@@ -409,16 +417,16 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
 }
 
 
-void CommandingServiceBase::rejectPacket(uint8_t report_id,
-		TcPacketStored* packet, ReturnValue_t error_code) {
-	verificationReporter.sendFailureReport(report_id, packet, error_code);
+void CommandingServiceBase::rejectPacket(uint8_t reportId,
+		TcPacketStoredBase* packet, ReturnValue_t errorCode) {
+	verificationReporter.sendFailureReport(reportId, packet->getPacketBase(), errorCode);
 	packet->deletePacket();
 }
 
 
 void CommandingServiceBase::acceptPacket(uint8_t reportId,
-		TcPacketStored* packet) {
-	verificationReporter.sendSuccessReport(reportId, packet);
+		TcPacketStoredBase* packet) {
+	verificationReporter.sendSuccessReport(reportId, packet->getPacketBase());
 	packet->deletePacket();
 }
 
@@ -428,7 +436,11 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
 	if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
 		commandMap.erase(&iter);
 	} else {
-		TcPacketStored newPacket(address);
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+		TcPacketStoredPusC newPacket(address);
+#else
+		TcPacketStoredPusA newPacket(address);
+#endif
 		startExecution(&newPacket, iter);
 	}
 }
diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h
index e10f2ddd2..46191bd5e 100644
--- a/tmtcservices/CommandingServiceBase.h
+++ b/tmtcservices/CommandingServiceBase.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 #define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 
+#include <fsfw/tmtcpacket/pus/TcPacketStoredBase.h>
 #include "AcceptsTelecommandsIF.h"
 #include "VerificationReporter.h"
 
@@ -351,12 +352,12 @@ private:
 	 */
 	void handleRequestQueue();
 
-	void rejectPacket(uint8_t reportId, TcPacketStored* packet,
+	void rejectPacket(uint8_t reportId, TcPacketStoredBase* packet,
 			ReturnValue_t errorCode);
 
-	void acceptPacket(uint8_t reportId, TcPacketStored* packet);
+	void acceptPacket(uint8_t reportId, TcPacketStoredBase* packet);
 
-	void startExecution(TcPacketStored *storedPacket, CommandMapIter iter);
+	void startExecution(TcPacketStoredBase *storedPacket, CommandMapIter iter);
 
 	void handleCommandMessage(CommandMessage* reply);
 	void handleReplyHandlerResult(ReturnValue_t result, CommandMapIter iter,
diff --git a/tmtcservices/PusServiceBase.h b/tmtcservices/PusServiceBase.h
index 4d3d99bc4..f48612b12 100644
--- a/tmtcservices/PusServiceBase.h
+++ b/tmtcservices/PusServiceBase.h
@@ -141,7 +141,11 @@ protected:
 	 * The current Telecommand to be processed.
 	 * It is deleted after handleRequest was executed.
 	 */
-	TcPacketStored currentPacket;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+	TcPacketStoredPusC currentPacket;
+#else
+    TcPacketStoredPusA currentPacket;
+#endif
 
 	static object_id_t packetSource;
 
diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp
index 998cbfb6c..74e0719c8 100644
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@@ -17,14 +17,17 @@ VerificationReporter::VerificationReporter() :
 VerificationReporter::~VerificationReporter() {}
 
 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
-		TcPacketBase* current_packet, uint8_t set_step) {
+		TcPacketBase* currentPacket, uint8_t set_step) {
 	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
+	if(currentPacket == nullptr) {
+	    return;
+	}
 	PusVerificationMessage message(set_report_id,
-			current_packet->getAcknowledgeFlags(),
-			current_packet->getPacketId(),
-			current_packet->getPacketSequenceControl(), 0, set_step);
+			currentPacket->getAcknowledgeFlags(),
+			currentPacket->getPacketId(),
+			currentPacket->getPacketSequenceControl(), 0, set_step);
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
@@ -56,15 +59,18 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 }
 
 void VerificationReporter::sendFailureReport(uint8_t report_id,
-		TcPacketBase* current_packet, ReturnValue_t error_code, uint8_t step,
+		TcPacketBase* currentPacket, ReturnValue_t error_code, uint8_t step,
 		uint32_t parameter1, uint32_t parameter2) {
 	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
+	if(currentPacket == nullptr) {
+	    return;
+	}
 	PusVerificationMessage message(report_id,
-			current_packet->getAcknowledgeFlags(),
-			current_packet->getPacketId(),
-			current_packet->getPacketSequenceControl(), error_code, step,
+			currentPacket->getAcknowledgeFlags(),
+			currentPacket->getPacketId(),
+			currentPacket->getPacketSequenceControl(), error_code, step,
 			parameter1, parameter2);
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 	        &message);

From 8dc66784a8aacff57bf6bb45f8061b1730e4a20d Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 16:59:28 +0200
Subject: [PATCH 04/11] tiny changes

---
 tmtcpacket/pus/TcPacketStoredPusC.cpp | 10 +---------
 tmtcpacket/pus/TcPacketStoredPusC.h   |  8 ++++++++
 2 files changed, 9 insertions(+), 9 deletions(-)
 create mode 100644 tmtcpacket/pus/TcPacketStoredPusC.h

diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
index f568d1390..0ff2376a5 100644
--- a/tmtcpacket/pus/TcPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/TcPacketStoredPusC.cpp
@@ -1,10 +1,2 @@
-/*
- * TcPacketStoredPusC.cpp
- *
- *  Created on: Jun 13, 2021
- *      Author: rmueller
- */
-
-
-
+#include "TcPacketStoredPusC.h"
 
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.h b/tmtcpacket/pus/TcPacketStoredPusC.h
new file mode 100644
index 000000000..e8ced948d
--- /dev/null
+++ b/tmtcpacket/pus/TcPacketStoredPusC.h
@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_ */

From 40a8c9a495f8f0a51f029dbd673defb6f6822d4f Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 13 Jun 2021 17:58:44 +0200
Subject: [PATCH 05/11] srv 20 fixes

---
 pus/Service20ParameterManagement.cpp  | 3 +++
 pus/servicepackets/Service20Packets.h | 7 +++----
 2 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp
index 8ebc6db02..c4e4b5eb1 100644
--- a/pus/Service20ParameterManagement.cpp
+++ b/pus/Service20ParameterManagement.cpp
@@ -133,6 +133,9 @@ ReturnValue_t Service20ParameterManagement::prepareLoadCommand(
     store_address_t storeAddress;
     size_t parameterDataLen = tcDataLen - sizeof(object_id_t) - sizeof(ParameterId_t) -
             sizeof(uint32_t);
+    if(parameterDataLen == 0) {
+        return CommandingServiceBase::INVALID_TC;
+    }
     ReturnValue_t result = IPCStore->getFreeElement(&storeAddress,
             parameterDataLen, &storePointer);
     if(result != HasReturnvaluesIF::RETURN_OK) {
diff --git a/pus/servicepackets/Service20Packets.h b/pus/servicepackets/Service20Packets.h
index 33bd153dd..6c7eb6f5c 100644
--- a/pus/servicepackets/Service20Packets.h
+++ b/pus/servicepackets/Service20Packets.h
@@ -27,12 +27,11 @@ public:
     ParameterCommand(uint8_t* storePointer, size_t parameterDataLen):
         parameterBuffer(storePointer, parameterDataLen) {
 #if FSFW_VERBOSE_LEVEL >= 1
-        if(parameterDataLen < sizeof(object_id_t) + sizeof(ParameterId_t) + 4) {
+        if(parameterDataLen == 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::warning << "ParameterCommand: Parameter data length is less than 12!"
-                    << std::endl;
+            sif::warning << "ParameterCommand: Parameter data length is 0" << std::endl;
 #else
-            sif::printWarning("ParameterCommand: Parameter data length is less than 12!\n");
+            sif::printWarning("ParameterCommand: Parameter data length is 0!\n");
 #endif
         }
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */

From 56d2af9d2578775dfee495aca1016a8610a3125a Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 10:19:01 +0200
Subject: [PATCH 06/11] cleaned up a bit

---
 pus/Service17Test.cpp                         |  2 +-
 pus/Service1TelecommandVerification.cpp       |  2 +-
 pus/Service5EventReporting.cpp                |  2 +-
 tcdistribution/PUSDistributor.cpp             |  6 +--
 tcdistribution/PUSDistributor.h               |  8 +---
 tcdistribution/TcPacketCheck.cpp              |  4 +-
 tmstorage/TmStorePackets.h                    |  2 +-
 tmtcpacket/packetmatcher/ApidMatcher.h        |  2 +-
 tmtcpacket/packetmatcher/PacketMatchTree.h    |  2 +-
 tmtcpacket/packetmatcher/ServiceMatcher.h     |  2 +-
 tmtcpacket/packetmatcher/SubserviceMatcher.h  |  2 +-
 tmtcpacket/pus/CMakeLists.txt                 | 16 +------
 tmtcpacket/pus/TcPacketPusC.cpp               |  4 --
 tmtcpacket/pus/TcPacketPusC.h                 |  8 ----
 tmtcpacket/pus/TcPacketStored.h               | 13 -----
 tmtcpacket/pus/TcPacketStoredPusC.cpp         |  2 -
 tmtcpacket/pus/TcPacketStoredPusC.h           |  8 ----
 tmtcpacket/pus/tc.h                           |  7 +++
 tmtcpacket/pus/tc/CMakeLists.txt              |  6 +++
 tmtcpacket/pus/{ => tc}/TcPacketBase.cpp      |  6 +--
 tmtcpacket/pus/{ => tc}/TcPacketBase.h        |  2 +-
 .../{TcPacketPusA.cpp => tc/TcPacketPus.cpp}  | 48 +++++++++----------
 .../pus/{TcPacketPusA.h => tc/TcPacketPus.h}  | 19 +++++---
 .../pus/{ => tc}/TcPacketStoredBase.cpp       |  6 +--
 tmtcpacket/pus/{ => tc}/TcPacketStoredBase.h  |  4 +-
 tmtcpacket/pus/{ => tc}/TcPacketStoredIF.h    |  6 +--
 .../TcPacketStoredPus.cpp}                    | 18 +++----
 .../TcPacketStoredPus.h}                      | 14 +++---
 tmtcpacket/pus/tm.h                           | 16 +++++++
 tmtcpacket/pus/tm/CMakeLists.txt              |  9 ++++
 tmtcpacket/pus/{ => tm}/TmPacketBase.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketBase.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketMinimal.cpp   |  7 +--
 tmtcpacket/pus/{ => tm}/TmPacketMinimal.h     |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketPusA.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusA.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusC.cpp      | 10 ++--
 tmtcpacket/pus/{ => tm}/TmPacketPusC.h        |  8 ++--
 tmtcpacket/pus/{ => tm}/TmPacketStored.h      |  0
 .../pus/{ => tm}/TmPacketStoredBase.cpp       |  6 +--
 tmtcpacket/pus/{ => tm}/TmPacketStoredBase.h  | 12 ++---
 .../pus/{ => tm}/TmPacketStoredPusA.cpp       |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.h  |  0
 .../pus/{ => tm}/TmPacketStoredPusC.cpp       |  4 +-
 tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.h  |  4 +-
 tmtcservices/CommandingServiceBase.cpp        | 16 ++-----
 tmtcservices/CommandingServiceBase.h          |  2 +-
 tmtcservices/PusServiceBase.h                 |  8 +---
 tmtcservices/PusVerificationReport.h          |  2 +-
 49 files changed, 173 insertions(+), 196 deletions(-)
 delete mode 100644 tmtcpacket/pus/TcPacketPusC.cpp
 delete mode 100644 tmtcpacket/pus/TcPacketPusC.h
 delete mode 100644 tmtcpacket/pus/TcPacketStored.h
 delete mode 100644 tmtcpacket/pus/TcPacketStoredPusC.cpp
 delete mode 100644 tmtcpacket/pus/TcPacketStoredPusC.h
 create mode 100644 tmtcpacket/pus/tc.h
 create mode 100644 tmtcpacket/pus/tc/CMakeLists.txt
 rename tmtcpacket/pus/{ => tc}/TcPacketBase.cpp (72%)
 rename tmtcpacket/pus/{ => tc}/TcPacketBase.h (98%)
 rename tmtcpacket/pus/{TcPacketPusA.cpp => tc/TcPacketPus.cpp} (54%)
 rename tmtcpacket/pus/{TcPacketPusA.h => tc/TcPacketPus.h} (87%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredBase.cpp (92%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredBase.h (96%)
 rename tmtcpacket/pus/{ => tc}/TcPacketStoredIF.h (88%)
 rename tmtcpacket/pus/{TcPacketStoredPusA.cpp => tc/TcPacketStoredPus.cpp} (78%)
 rename tmtcpacket/pus/{TcPacketStoredPusA.h => tc/TcPacketStoredPus.h} (84%)
 create mode 100644 tmtcpacket/pus/tm.h
 create mode 100644 tmtcpacket/pus/tm/CMakeLists.txt
 rename tmtcpacket/pus/{ => tm}/TmPacketBase.cpp (88%)
 rename tmtcpacket/pus/{ => tm}/TmPacketBase.h (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketMinimal.cpp (93%)
 rename tmtcpacket/pus/{ => tm}/TmPacketMinimal.h (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusA.cpp (90%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusA.h (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusC.cpp (90%)
 rename tmtcpacket/pus/{ => tm}/TmPacketPusC.h (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStored.h (100%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredBase.cpp (94%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredBase.h (91%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.cpp (95%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusA.h (100%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.cpp (96%)
 rename tmtcpacket/pus/{ => tm}/TmPacketStoredPusC.h (96%)

diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp
index daed987ae..37258cc16 100644
--- a/pus/Service17Test.cpp
+++ b/pus/Service17Test.cpp
@@ -3,7 +3,7 @@
 
 #include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 
 
 Service17Test::Service17Test(object_id_t objectId,
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index bef7b6b1a..8aec6902c 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -4,7 +4,7 @@
 #include "../ipc/QueueFactory.h"
 #include "../objectmanager/ObjectManager.h"
 #include "../tmtcservices/PusVerificationReport.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../tmtcservices/AcceptsTelemetryIF.h"
 
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 272cc203a..0c139f3aa 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -5,7 +5,7 @@
 #include "../objectmanager/ObjectManager.h"
 #include "../events/EventManagerIF.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tm/TmPacketStored.h"
 
 
 Service5EventReporting::Service5EventReporting(object_id_t objectId,
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 5c2f734b0..955a80932 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -118,11 +118,7 @@ uint16_t PUSDistributor::getIdentifier() {
 }
 
 ReturnValue_t PUSDistributor::initialize() {
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-    currentPacket = new TcPacketStoredPusC();
-#else
-    currentPacket = new TcPacketStoredPusA();
-#endif
+    currentPacket = new TcPacketStoredPus();
     if(currentPacket == nullptr) {
         // Should not happen, memory allocation failed!
         return ObjectManagerIF::CHILD_INIT_FAILED;
diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h
index 0c2884519..c6f863f04 100644
--- a/tcdistribution/PUSDistributor.h
+++ b/tcdistribution/PUSDistributor.h
@@ -5,7 +5,7 @@
 #include "TcDistributor.h"
 #include "TcPacketCheck.h"
 
-#include "../tmtcpacket/pus/TcPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/AcceptsTelecommandsIF.h"
 #include "../tmtcservices/VerificationReporter.h"
@@ -52,11 +52,7 @@ protected:
 	/**
 	 * The currently handled packet is stored here.
 	 */
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC* currentPacket = nullptr;
-#else
-	TcPacketStoredPusA* currentPacket = nullptr;
-#endif
+	TcPacketStoredPus* currentPacket = nullptr;
 
 	/**
 	 * With this variable, the current check status is stored to generate
diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index e88305cce..dce0a4583 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketCheck.h"
 
 #include "../globalfunctions/CRC.h"
-#include "../tmtcpacket/pus/TcPacketBase.h"
-#include "../tmtcpacket/pus/TcPacketStoredBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketStoredBase.h"
 #include "../serviceinterface/ServiceInterface.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../tmtcservices/VerificationCodes.h"
diff --git a/tmstorage/TmStorePackets.h b/tmstorage/TmStorePackets.h
index 3abd0c1ca..53a5d8d6b 100644
--- a/tmstorage/TmStorePackets.h
+++ b/tmstorage/TmStorePackets.h
@@ -5,7 +5,7 @@
 #include "../serialize/SerializeElement.h"
 #include "../serialize/SerialLinkedListAdapter.h"
 #include "../serialize/SerialBufferAdapter.h"
-#include "../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../tmtcpacket/pus/tm/TmPacketMinimal.h"
 #include "../timemanager/TimeStamperIF.h"
 #include "../timemanager/CCSDSTime.h"
 #include "../globalfunctions/timevalOperations.h"
diff --git a/tmtcpacket/packetmatcher/ApidMatcher.h b/tmtcpacket/packetmatcher/ApidMatcher.h
index 4f196ac97..64f73dbf0 100644
--- a/tmtcpacket/packetmatcher/ApidMatcher.h
+++ b/tmtcpacket/packetmatcher/ApidMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../../tmtcpacket/pus/tm/TmPacketMinimal.h"
 
 class ApidMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.h b/tmtcpacket/packetmatcher/PacketMatchTree.h
index 54fc856c7..735f05664 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.h
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.h
@@ -4,7 +4,7 @@
 #include "../../container/PlacementFactory.h"
 #include "../../globalfunctions/matching/MatchTree.h"
 #include "../../storagemanager/LocalPool.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../../tmtcpacket/pus/tm/TmPacketMinimal.h"
 
 class PacketMatchTree: public MatchTree<TmPacketMinimal*>, public HasReturnvaluesIF {
 public:
diff --git a/tmtcpacket/packetmatcher/ServiceMatcher.h b/tmtcpacket/packetmatcher/ServiceMatcher.h
index eba23d755..57f491f3f 100644
--- a/tmtcpacket/packetmatcher/ServiceMatcher.h
+++ b/tmtcpacket/packetmatcher/ServiceMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../pus/tm/TmPacketMinimal.h"
 
 class ServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
diff --git a/tmtcpacket/packetmatcher/SubserviceMatcher.h b/tmtcpacket/packetmatcher/SubserviceMatcher.h
index a9b6def89..80681b5db 100644
--- a/tmtcpacket/packetmatcher/SubserviceMatcher.h
+++ b/tmtcpacket/packetmatcher/SubserviceMatcher.h
@@ -3,7 +3,7 @@
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
-#include "../../tmtcpacket/pus/TmPacketMinimal.h"
+#include "../pus/tm/TmPacketMinimal.h"
 
 class SubServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 public:
diff --git a/tmtcpacket/pus/CMakeLists.txt b/tmtcpacket/pus/CMakeLists.txt
index 9340e3b1f..32c80dd31 100644
--- a/tmtcpacket/pus/CMakeLists.txt
+++ b/tmtcpacket/pus/CMakeLists.txt
@@ -1,14 +1,2 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-    TcPacketBase.cpp
-    TcPacketPusA.cpp
-    TcPacketStoredBase.cpp
-    TcPacketStoredPusA.cpp
-
-    TmPacketBase.cpp
-    TmPacketMinimal.cpp
-    TmPacketStoredPusA.cpp
-    TmPacketStoredPusC.cpp
-    TmPacketPusA.cpp
-    TmPacketPusC.cpp
-    TmPacketStoredBase.cpp
-)
+add_subdirectory(tm)
+add_subdirectory(tc)
diff --git a/tmtcpacket/pus/TcPacketPusC.cpp b/tmtcpacket/pus/TcPacketPusC.cpp
deleted file mode 100644
index 1f2946a85..000000000
--- a/tmtcpacket/pus/TcPacketPusC.cpp
+++ /dev/null
@@ -1,4 +0,0 @@
-#include "TcPacketPusC.h"
-
-
-
diff --git a/tmtcpacket/pus/TcPacketPusC.h b/tmtcpacket/pus/TcPacketPusC.h
deleted file mode 100644
index e965e752d..000000000
--- a/tmtcpacket/pus/TcPacketPusC.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_
-
-
-
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSC_H_ */
diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h
deleted file mode 100644
index 0b6ff0f4c..000000000
--- a/tmtcpacket/pus/TcPacketStored.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_
-
-#include <FSFWConfig.h>
-
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-#include "TcPacketStoredPusC.h"
-#else
-#include "TcPacketStoredPusA.h"
-#endif
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTORED_H_ */
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.cpp b/tmtcpacket/pus/TcPacketStoredPusC.cpp
deleted file mode 100644
index 0ff2376a5..000000000
--- a/tmtcpacket/pus/TcPacketStoredPusC.cpp
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "TcPacketStoredPusC.h"
-
diff --git a/tmtcpacket/pus/TcPacketStoredPusC.h b/tmtcpacket/pus/TcPacketStoredPusC.h
deleted file mode 100644
index e8ced948d..000000000
--- a/tmtcpacket/pus/TcPacketStoredPusC.h
+++ /dev/null
@@ -1,8 +0,0 @@
-#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
-#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_
-
-
-
-
-
-#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSC_H_ */
diff --git a/tmtcpacket/pus/tc.h b/tmtcpacket/pus/tc.h
new file mode 100644
index 000000000..156e49fe1
--- /dev/null
+++ b/tmtcpacket/pus/tc.h
@@ -0,0 +1,7 @@
+#ifndef FSFW_TMTCPACKET_PUS_TC_H_
+#define FSFW_TMTCPACKET_PUS_TC_H_
+
+#include "tc/TcPacketStoredPus.h"
+#include "tc/TcPacketPus.h"
+
+#endif /* FSFW_TMTCPACKET_PUS_TC_H_ */
diff --git a/tmtcpacket/pus/tc/CMakeLists.txt b/tmtcpacket/pus/tc/CMakeLists.txt
new file mode 100644
index 000000000..723b79437
--- /dev/null
+++ b/tmtcpacket/pus/tc/CMakeLists.txt
@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE    
+    TcPacketBase.cpp
+    TcPacketPus.cpp
+    TcPacketStoredBase.cpp
+    TcPacketStoredPus.cpp
+)
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/tc/TcPacketBase.cpp
similarity index 72%
rename from tmtcpacket/pus/TcPacketBase.cpp
rename to tmtcpacket/pus/tc/TcPacketBase.cpp
index a5b6f9ce0..dd576fec8 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketBase.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterface.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../serviceinterface/ServiceInterface.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
similarity index 98%
rename from tmtcpacket/pus/TcPacketBase.h
rename to tmtcpacket/pus/tc/TcPacketBase.h
index 606975e28..f29f9b35d 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -1,7 +1,7 @@
 #ifndef TMTCPACKET_PUS_TCPACKETBASE_H_
 #define TMTCPACKET_PUS_TCPACKETBASE_H_
 
-#include "../../tmtcpacket/SpacePacketBase.h"
+#include "../../../tmtcpacket/SpacePacketBase.h"
 #include <cstddef>
 
 /**
diff --git a/tmtcpacket/pus/TcPacketPusA.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
similarity index 54%
rename from tmtcpacket/pus/TcPacketPusA.cpp
rename to tmtcpacket/pus/tc/TcPacketPus.cpp
index 0ce767b47..7a184f3c6 100644
--- a/tmtcpacket/pus/TcPacketPusA.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -1,39 +1,39 @@
-#include "TcPacketPusA.h"
-#include "../../globalfunctions/CRC.h"
+#include "TcPacketPus.h"
+#include "../../../globalfunctions/CRC.h"
 
 #include <cstring>
 
-TcPacketPusA::TcPacketPusA(const uint8_t *setData): TcPacketBase(setData) {
+TcPacketPus::TcPacketPus(const uint8_t *setData): TcPacketBase(setData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
 }
 
-uint8_t TcPacketPusA::getService() const {
-    return tcData->dataField.service_type;
+uint8_t TcPacketPus::getService() const {
+    return tcData->dataField.serviceType;
 }
 
-uint8_t TcPacketPusA::getSubService() {
-    return tcData->dataField.service_subtype;
+uint8_t TcPacketPus::getSubService() {
+    return tcData->dataField.serviceSubtype;
 }
 
-uint8_t TcPacketPusA::getAcknowledgeFlags() {
-    return tcData->dataField.version_type_ack & 0b00001111;
+uint8_t TcPacketPus::getAcknowledgeFlags() {
+    return tcData->dataField.versionTypeAck & 0b00001111;
 }
 
-const uint8_t* TcPacketPusA::getApplicationData() const {
+const uint8_t* TcPacketPus::getApplicationData() const {
     return &tcData->appData;
 }
 
-uint16_t TcPacketPusA::getApplicationDataSize() {
+uint16_t TcPacketPus::getApplicationDataSize() {
     return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
 }
 
-uint16_t TcPacketPusA::getErrorControl() {
+uint16_t TcPacketPus::getErrorControl() {
     uint16_t size = getApplicationDataSize() + CRC_SIZE;
     uint8_t* p_to_buffer = &tcData->appData;
     return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
 }
 
-void TcPacketPusA::setErrorControl() {
+void TcPacketPus::setErrorControl() {
     uint32_t full_size = getFullSize();
     uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
     uint32_t size = getApplicationDataSize();
@@ -41,35 +41,35 @@ void TcPacketPusA::setErrorControl() {
     (&tcData->appData)[size + 1] = (crc) & 0X00FF;      // CRCL
 }
 
-void TcPacketPusA::setData(const uint8_t* pData) {
+void TcPacketPus::setData(const uint8_t* pData) {
     SpacePacketBase::setData(pData);
     // This function is const-correct, but it was decided to keep the pointer non-const
     // for convenience. Therefore, cast aways constness here and then cast to packet type.
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
 }
 
-uint8_t TcPacketPusA::getSecondaryHeaderFlag() {
-    return (tcData->dataField.version_type_ack & 0b10000000) >> 7;
+uint8_t TcPacketPus::getSecondaryHeaderFlag() {
+    return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
 }
 
-uint8_t TcPacketPusA::getPusVersionNumber() {
-    return (tcData->dataField.version_type_ack & 0b01110000) >> 4;
+uint8_t TcPacketPus::getPusVersionNumber() {
+    return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
 }
 
-void TcPacketPusA::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
         uint8_t ack, uint8_t service, uint8_t subservice) {
     initSpacePacketHeader(true, true, apid, sequenceCount);
     std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
     setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
     // Data Field Header:
     // Set CCSDS_secondary_header_flag to 0 and version number to 001
-    tcData->dataField.version_type_ack = 0b00010000;
-    tcData->dataField.version_type_ack |= (ack & 0x0F);
-    tcData->dataField.service_type = service;
-    tcData->dataField.service_subtype = subservice;
+    tcData->dataField.versionTypeAck = 0b00010000;
+    tcData->dataField.versionTypeAck |= (ack & 0x0F);
+    tcData->dataField.serviceType = service;
+    tcData->dataField.serviceSubtype = subservice;
 }
 
-size_t TcPacketPusA::calculateFullPacketLength(size_t appDataLen) {
+size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) {
     return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
             appDataLen + TcPacketBase::CRC_SIZE;
 }
diff --git a/tmtcpacket/pus/TcPacketPusA.h b/tmtcpacket/pus/tc/TcPacketPus.h
similarity index 87%
rename from tmtcpacket/pus/TcPacketPusA.h
rename to tmtcpacket/pus/tc/TcPacketPus.h
index 0e61d829f..fb8483c52 100644
--- a/tmtcpacket/pus/TcPacketPusA.h
+++ b/tmtcpacket/pus/tc/TcPacketPus.h
@@ -1,7 +1,8 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
 
-#include "../ccsds_header.h"
+#include "../../../FSFW.h"
+#include "../../ccsds_header.h"
 #include "TcPacketBase.h"
 
 #include <cstdint>
@@ -13,10 +14,14 @@
  * @ingroup tmtcpackets
  */
 struct PUSTcDataFieldHeader {
-    uint8_t version_type_ack;
-    uint8_t service_type;
-    uint8_t service_subtype;
-    uint8_t source_id;
+    uint8_t versionTypeAck;
+    uint8_t serviceType;
+    uint8_t serviceSubtype;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    uint16_t sourceId;
+#else
+    uint8_t sourceId;
+#endif
 };
 
 /**
@@ -31,7 +36,7 @@ struct TcPacketPointer {
 };
 
 
-class TcPacketPusA: public TcPacketBase {
+class TcPacketPus: public TcPacketBase {
 public:
     static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
             sizeof(PUSTcDataFieldHeader) + 2);
@@ -41,7 +46,7 @@ public:
      * create an empty (invalid) object by passing nullptr as the data pointer
      * @param setData
      */
-    TcPacketPusA(const uint8_t* setData);
+    TcPacketPus(const uint8_t* setData);
 
     // Base class overrides
     virtual uint8_t getSecondaryHeaderFlag() override;
diff --git a/tmtcpacket/pus/TcPacketStoredBase.cpp b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
similarity index 92%
rename from tmtcpacket/pus/TcPacketStoredBase.cpp
rename to tmtcpacket/pus/tc/TcPacketStoredBase.cpp
index 629562c01..daae35f5a 100644
--- a/tmtcpacket/pus/TcPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
@@ -1,8 +1,8 @@
 #include "TcPacketStoredBase.h"
 
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../objectmanager/frameworkObjects.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../objectmanager/frameworkObjects.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TcPacketStoredBase.h b/tmtcpacket/pus/tc/TcPacketStoredBase.h
similarity index 96%
rename from tmtcpacket/pus/TcPacketStoredBase.h
rename to tmtcpacket/pus/tc/TcPacketStoredBase.h
index 811123db4..b7d735310 100644
--- a/tmtcpacket/pus/TcPacketStoredBase.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.h
@@ -1,8 +1,8 @@
 #ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
 #define TMTCPACKET_PUS_TCPACKETSTORED_H_
 
-#include <fsfw/tmtcpacket/pus/TcPacketStoredIF.h>
-#include "../../storagemanager/StorageManagerIF.h"
+#include "TcPacketStoredIF.h"
+#include "../../../storagemanager/StorageManagerIF.h"
 
 /**
  *	This class generates a ECSS PUS Telecommand packet within a given
diff --git a/tmtcpacket/pus/TcPacketStoredIF.h b/tmtcpacket/pus/tc/TcPacketStoredIF.h
similarity index 88%
rename from tmtcpacket/pus/TcPacketStoredIF.h
rename to tmtcpacket/pus/tc/TcPacketStoredIF.h
index 8eb23004f..3d3567259 100644
--- a/tmtcpacket/pus/TcPacketStoredIF.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredIF.h
@@ -1,9 +1,9 @@
 #ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
 #define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
 
-#include "../../tmtcpacket/pus/TcPacketBase.h"
-#include "../../storagemanager/storeAddress.h"
-#include "../../returnvalues/HasReturnvaluesIF.h"
+#include "TcPacketBase.h"
+#include "../../../storagemanager/storeAddress.h"
+#include "../../../returnvalues/HasReturnvaluesIF.h"
 
 class TcPacketStoredIF {
 public:
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.cpp b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
similarity index 78%
rename from tmtcpacket/pus/TcPacketStoredPusA.cpp
rename to tmtcpacket/pus/tc/TcPacketStoredPus.cpp
index 7c706a1a4..f098ce2b0 100644
--- a/tmtcpacket/pus/TcPacketStoredPusA.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp
@@ -1,11 +1,11 @@
-#include "TcPacketStoredPusA.h"
+#include "TcPacketStoredPus.h"
 
 #include <cstring>
 
-TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
+TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,
         uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
         size_t size, uint8_t ack) :
-        TcPacketPusA(nullptr) {
+        TcPacketPus(nullptr) {
     this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
     if (not this->checkAndSetStore()) {
         return;
@@ -28,14 +28,14 @@ TcPacketStoredPusA::TcPacketStoredPusA(uint16_t apid,  uint8_t service,
     this->setErrorControl();
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(): TcPacketStoredBase(), TcPacketPusA(nullptr) {
+TcPacketStoredPus::TcPacketStoredPus(): TcPacketStoredBase(), TcPacketPus(nullptr) {
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(store_address_t setAddress): TcPacketPusA(nullptr) {
+TcPacketStoredPus::TcPacketStoredPus(store_address_t setAddress): TcPacketPus(nullptr) {
     TcPacketStoredBase::setStoreAddress(setAddress);
 }
 
-TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPacketPusA(data) {
+TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size): TcPacketPus(data) {
     if (this->getFullSize() != size) {
         return;
     }
@@ -51,19 +51,19 @@ TcPacketStoredPusA::TcPacketStoredPusA(const uint8_t* data, size_t size): TcPack
     }
 }
 
-ReturnValue_t TcPacketStoredPusA::deletePacket() {
+ReturnValue_t TcPacketStoredPus::deletePacket() {
     ReturnValue_t result = this->store->deleteData(this->storeAddress);
     this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
     this->setData(nullptr);
     return result;
 }
 
-TcPacketBase* TcPacketStoredPusA::getPacketBase() {
+TcPacketBase* TcPacketStoredPus::getPacketBase() {
     return this;
 }
 
 
-bool TcPacketStoredPusA::isSizeCorrect() {
+bool TcPacketStoredPus::isSizeCorrect() {
     const uint8_t* temp_data = nullptr;
     size_t temp_size;
     ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data,
diff --git a/tmtcpacket/pus/TcPacketStoredPusA.h b/tmtcpacket/pus/tc/TcPacketStoredPus.h
similarity index 84%
rename from tmtcpacket/pus/TcPacketStoredPusA.h
rename to tmtcpacket/pus/tc/TcPacketStoredPus.h
index fe1f64f7a..8b3187331 100644
--- a/tmtcpacket/pus/TcPacketStoredPusA.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredPus.h
@@ -2,11 +2,11 @@
 #define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
 
 #include "TcPacketStoredBase.h"
-#include "TcPacketPusA.h"
+#include "TcPacketPus.h"
 
-class TcPacketStoredPusA:
+class TcPacketStoredPus:
         public TcPacketStoredBase,
-        public TcPacketPusA {
+        public TcPacketPus {
 public:
     /**
      * With this constructor, new space is allocated in the packet store and
@@ -24,7 +24,7 @@ public:
      *                      number of verification packets returned
      *                      for this command.
      */
-    TcPacketStoredPusA(uint16_t apid, uint8_t service, uint8_t subservice,
+    TcPacketStoredPus(uint16_t apid, uint8_t service, uint8_t subservice,
             uint8_t sequence_count = 0, const uint8_t* data = nullptr,
             size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL);
     /**
@@ -32,13 +32,13 @@ public:
      * @param data
      * @param size
      */
-    TcPacketStoredPusA(const uint8_t* data, size_t size);
+    TcPacketStoredPus(const uint8_t* data, size_t size);
     /**
      * Create stored packet from existing packet in store
      * @param setAddress
      */
-    TcPacketStoredPusA(store_address_t setAddress);
-    TcPacketStoredPusA();
+    TcPacketStoredPus(store_address_t setAddress);
+    TcPacketStoredPus();
 
     ReturnValue_t deletePacket() override;
     TcPacketBase* getPacketBase() override;
diff --git a/tmtcpacket/pus/tm.h b/tmtcpacket/pus/tm.h
new file mode 100644
index 000000000..591ada7c3
--- /dev/null
+++ b/tmtcpacket/pus/tm.h
@@ -0,0 +1,16 @@
+#ifndef FSFW_TMTCPACKET_PUS_TM_H_
+#define FSFW_TMTCPACKET_PUS_TM_H_
+
+#include "../../FSFW.h"
+
+#if FSFW_USE_PUS_C_TELEMETRY == 1
+#include "tm/TmPacketPusC.h"
+#include "tm/TmPacketStoredPusC.h"
+#else
+#include "tm/TmPacketPusA.h"
+#include "tm/TmPacketStoredPusA.h"
+#endif
+
+#include "tm/TmPacketMinimal.h"
+
+#endif /* FSFW_TMTCPACKET_PUS_TM_H_ */
diff --git a/tmtcpacket/pus/tm/CMakeLists.txt b/tmtcpacket/pus/tm/CMakeLists.txt
new file mode 100644
index 000000000..ace87820d
--- /dev/null
+++ b/tmtcpacket/pus/tm/CMakeLists.txt
@@ -0,0 +1,9 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE
+    TmPacketStoredPusA.cpp
+    TmPacketStoredPusC.cpp
+    TmPacketPusA.cpp
+    TmPacketPusC.cpp
+    TmPacketStoredBase.cpp
+    TmPacketBase.cpp
+    TmPacketMinimal.cpp
+)
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/tm/TmPacketBase.cpp
similarity index 88%
rename from tmtcpacket/pus/TmPacketBase.cpp
rename to tmtcpacket/pus/tm/TmPacketBase.cpp
index b144db1bd..91ec7d58e 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketBase.cpp
@@ -1,10 +1,10 @@
 #include "TmPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../timemanager/CCSDSTime.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/tm/TmPacketBase.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketBase.h
rename to tmtcpacket/pus/tm/TmPacketBase.h
index 6925e99b9..9f534f294 100644
--- a/tmtcpacket/pus/TmPacketBase.h
+++ b/tmtcpacket/pus/tm/TmPacketBase.h
@@ -1,10 +1,10 @@
 #ifndef TMTCPACKET_PUS_TMPACKETBASE_H_
 #define TMTCPACKET_PUS_TMPACKETBASE_H_
 
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketMinimal.cpp b/tmtcpacket/pus/tm/TmPacketMinimal.cpp
similarity index 93%
rename from tmtcpacket/pus/TmPacketMinimal.cpp
rename to tmtcpacket/pus/tm/TmPacketMinimal.cpp
index 18e9dda1e..3f785cde8 100644
--- a/tmtcpacket/pus/TmPacketMinimal.cpp
+++ b/tmtcpacket/pus/tm/TmPacketMinimal.cpp
@@ -1,7 +1,8 @@
 #include "TmPacketMinimal.h"
-#include <stddef.h>
-#include <time.h>
-#include "PacketTimestampInterpreterIF.h"
+#include "../PacketTimestampInterpreterIF.h"
+
+#include <cstddef>
+#include <ctime>
 
 TmPacketMinimal::TmPacketMinimal(const uint8_t* set_data) : SpacePacketBase( set_data ) {
 	this->tm_data = (TmPacketMinimalPointer*)set_data;
diff --git a/tmtcpacket/pus/TmPacketMinimal.h b/tmtcpacket/pus/tm/TmPacketMinimal.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketMinimal.h
rename to tmtcpacket/pus/tm/TmPacketMinimal.h
index 728acb154..08daa5848 100644
--- a/tmtcpacket/pus/TmPacketMinimal.h
+++ b/tmtcpacket/pus/tm/TmPacketMinimal.h
@@ -2,8 +2,8 @@
 #define FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_
 
 
-#include "../../tmtcpacket/SpacePacketBase.h"
-#include "../../returnvalues/HasReturnvaluesIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../returnvalues/HasReturnvaluesIF.h"
 
 struct timeval;
 class PacketTimestampInterpreterIF;
diff --git a/tmtcpacket/pus/TmPacketPusA.cpp b/tmtcpacket/pus/tm/TmPacketPusA.cpp
similarity index 90%
rename from tmtcpacket/pus/TmPacketPusA.cpp
rename to tmtcpacket/pus/tm/TmPacketPusA.cpp
index d96f6aa79..bdc0a815e 100644
--- a/tmtcpacket/pus/TmPacketPusA.cpp
+++ b/tmtcpacket/pus/tm/TmPacketPusA.cpp
@@ -1,11 +1,11 @@
 #include "TmPacketPusA.h"
 #include "TmPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include "../../timemanager/CCSDSTime.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManagerIF.h"
+#include "../../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketPusA.h b/tmtcpacket/pus/tm/TmPacketPusA.h
similarity index 95%
rename from tmtcpacket/pus/TmPacketPusA.h
rename to tmtcpacket/pus/tm/TmPacketPusA.h
index dd9a5d090..486b68f30 100644
--- a/tmtcpacket/pus/TmPacketPusA.h
+++ b/tmtcpacket/pus/tm/TmPacketPusA.h
@@ -2,10 +2,10 @@
 #define FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
 
 #include "TmPacketBase.h"
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketPusC.cpp b/tmtcpacket/pus/tm/TmPacketPusC.cpp
similarity index 90%
rename from tmtcpacket/pus/TmPacketPusC.cpp
rename to tmtcpacket/pus/tm/TmPacketPusC.cpp
index ca2bccdb7..5090aaebe 100644
--- a/tmtcpacket/pus/TmPacketPusC.cpp
+++ b/tmtcpacket/pus/tm/TmPacketPusC.cpp
@@ -1,11 +1,11 @@
 #include "TmPacketPusC.h"
 #include "TmPacketBase.h"
 
-#include "../../globalfunctions/CRC.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include "../../timemanager/CCSDSTime.h"
+#include "../../../globalfunctions/CRC.h"
+#include "../../../globalfunctions/arrayprinter.h"
+#include "../../../objectmanager/ObjectManagerIF.h"
+#include "../../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketPusC.h b/tmtcpacket/pus/tm/TmPacketPusC.h
similarity index 95%
rename from tmtcpacket/pus/TmPacketPusC.h
rename to tmtcpacket/pus/tm/TmPacketPusC.h
index fdc275486..97e5a9a5a 100644
--- a/tmtcpacket/pus/TmPacketPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketPusC.h
@@ -2,10 +2,10 @@
 #define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
 
 #include "TmPacketBase.h"
-#include "../SpacePacketBase.h"
-#include "../../timemanager/TimeStamperIF.h"
-#include "../../timemanager/Clock.h"
-#include "../../objectmanager/SystemObjectIF.h"
+#include "../../SpacePacketBase.h"
+#include "../../../timemanager/TimeStamperIF.h"
+#include "../../../timemanager/Clock.h"
+#include "../../../objectmanager/SystemObjectIF.h"
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/tm/TmPacketStored.h
similarity index 100%
rename from tmtcpacket/pus/TmPacketStored.h
rename to tmtcpacket/pus/tm/TmPacketStored.h
diff --git a/tmtcpacket/pus/TmPacketStoredBase.cpp b/tmtcpacket/pus/tm/TmPacketStoredBase.cpp
similarity index 94%
rename from tmtcpacket/pus/TmPacketStoredBase.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredBase.cpp
index eeaa938d4..ba8b15d1a 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredBase.cpp
@@ -1,8 +1,8 @@
 #include "TmPacketStoredBase.h"
 
-#include "../../objectmanager/ObjectManager.h"
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../objectmanager/ObjectManager.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredBase.h b/tmtcpacket/pus/tm/TmPacketStoredBase.h
similarity index 91%
rename from tmtcpacket/pus/TmPacketStoredBase.h
rename to tmtcpacket/pus/tm/TmPacketStoredBase.h
index dd7e31eba..1bc092dd7 100644
--- a/tmtcpacket/pus/TmPacketStoredBase.h
+++ b/tmtcpacket/pus/tm/TmPacketStoredBase.h
@@ -1,15 +1,15 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
 
+#include "../../../FSFW.h"
 #include "TmPacketBase.h"
 #include "TmPacketStoredBase.h"
-#include <FSFWConfig.h>
+#include "TmPacketPusA.h"
 
-#include "../../tmtcpacket/pus/TmPacketPusA.h"
-#include "../../serialize/SerializeIF.h"
-#include "../../storagemanager/StorageManagerIF.h"
-#include "../../internalError/InternalErrorReporterIF.h"
-#include "../../ipc/MessageQueueSenderIF.h"
+#include "../../../serialize/SerializeIF.h"
+#include "../../../storagemanager/StorageManagerIF.h"
+#include "../../../internalError/InternalErrorReporterIF.h"
+#include "../../../ipc/MessageQueueSenderIF.h"
 
 /**
  *  This class generates a ECSS PUS Telemetry packet within a given
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.cpp b/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
similarity index 95%
rename from tmtcpacket/pus/TmPacketStoredPusA.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
index 68102b621..02fb77ef5 100644
--- a/tmtcpacket/pus/TmPacketStoredPusA.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp
@@ -1,7 +1,7 @@
 #include "TmPacketStoredPusA.h"
 
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusA.h b/tmtcpacket/pus/tm/TmPacketStoredPusA.h
similarity index 100%
rename from tmtcpacket/pus/TmPacketStoredPusA.h
rename to tmtcpacket/pus/tm/TmPacketStoredPusA.h
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.cpp b/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
similarity index 96%
rename from tmtcpacket/pus/TmPacketStoredPusC.cpp
rename to tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
index 7f7744114..6f8f7fa26 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.cpp
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp
@@ -1,7 +1,7 @@
 #include "TmPacketStoredPusC.h"
 
-#include "../../serviceinterface/ServiceInterface.h"
-#include "../../tmtcservices/TmTcMessage.h"
+#include "../../../serviceinterface/ServiceInterface.h"
+#include "../../../tmtcservices/TmTcMessage.h"
 
 #include <cstring>
 
diff --git a/tmtcpacket/pus/TmPacketStoredPusC.h b/tmtcpacket/pus/tm/TmPacketStoredPusC.h
similarity index 96%
rename from tmtcpacket/pus/TmPacketStoredPusC.h
rename to tmtcpacket/pus/tm/TmPacketStoredPusC.h
index 883dc0ffd..35c66083e 100644
--- a/tmtcpacket/pus/TmPacketStoredPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketStoredPusC.h
@@ -1,8 +1,8 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
 
-#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
-#include <fsfw/tmtcpacket/pus/TmPacketStoredBase.h>
+#include "TmPacketPusC.h"
+#include "TmPacketStoredBase.h"
 
 /**
  *  This class generates a ECSS PUS C Telemetry packet within a given
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 0ede27dc1..b416ffbe1 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -6,8 +6,8 @@
 #include "../tcdistribution/PUSDistributorIF.h"
 #include "../objectmanager/ObjectManager.h"
 #include "../ipc/QueueFactory.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
-#include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
+#include "../tmtcpacket/pus/tm.h"
 #include "../serviceinterface/ServiceInterface.h"
 
 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
@@ -246,11 +246,7 @@ void CommandingServiceBase::handleRequestQueue() {
 	TmTcMessage message;
 	ReturnValue_t result;
 	store_address_t address;
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC packet;
-#else
-	TcPacketStoredPusA packet;
-#endif
+	TcPacketStoredPus packet;
 	MessageQueueId_t queue;
 	object_id_t objectId;
 	for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
@@ -436,11 +432,7 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
 	if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
 		commandMap.erase(&iter);
 	} else {
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-		TcPacketStoredPusC newPacket(address);
-#else
-		TcPacketStoredPusA newPacket(address);
-#endif
+		TcPacketStoredPus newPacket(address);
 		startExecution(&newPacket, iter);
 	}
 }
diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h
index 46191bd5e..4ee4a21a5 100644
--- a/tmtcservices/CommandingServiceBase.h
+++ b/tmtcservices/CommandingServiceBase.h
@@ -1,7 +1,6 @@
 #ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 #define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 
-#include <fsfw/tmtcpacket/pus/TcPacketStoredBase.h>
 #include "AcceptsTelecommandsIF.h"
 #include "VerificationReporter.h"
 
@@ -17,6 +16,7 @@
 #include <FSFWConfig.h>
 
 class TcPacketStored;
+class TcPacketStoredBase;
 
 namespace Factory{
 void setStaticFrameworkObjectIds();
diff --git a/tmtcservices/PusServiceBase.h b/tmtcservices/PusServiceBase.h
index f48612b12..6cde6fb49 100644
--- a/tmtcservices/PusServiceBase.h
+++ b/tmtcservices/PusServiceBase.h
@@ -9,7 +9,7 @@
 #include "../objectmanager/SystemObject.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tasks/ExecutableObjectIF.h"
-#include "../tmtcpacket/pus/TcPacketStored.h"
+#include "../tmtcpacket/pus/tc.h"
 #include "../ipc/MessageQueueIF.h"
 
 namespace Factory{
@@ -141,11 +141,7 @@ protected:
 	 * The current Telecommand to be processed.
 	 * It is deleted after handleRequest was executed.
 	 */
-#if FSFW_USE_PUS_C_TELECOMMANDS == 1
-	TcPacketStoredPusC currentPacket;
-#else
-    TcPacketStoredPusA currentPacket;
-#endif
+	TcPacketStoredPus currentPacket;
 
 	static object_id_t packetSource;
 
diff --git a/tmtcservices/PusVerificationReport.h b/tmtcservices/PusVerificationReport.h
index 9dce95ac7..0e1732efc 100644
--- a/tmtcservices/PusVerificationReport.h
+++ b/tmtcservices/PusVerificationReport.h
@@ -4,7 +4,7 @@
 #include "VerificationCodes.h"
 
 #include "../ipc/MessageQueueMessage.h"
-#include "../tmtcpacket/pus/TcPacketBase.h"
+#include "../tmtcpacket/pus/tc/TcPacketBase.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 
 class PusVerificationMessage: public MessageQueueMessage {

From b1e3a1b2b586809e45b0a741f9e62fc1d2125093 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 11:16:56 +0200
Subject: [PATCH 07/11] const correctness

---
 tmtcpacket/SpacePacketBase.cpp         |  2 +-
 tmtcpacket/SpacePacketBase.h           |  6 ++--
 tmtcpacket/pus/tc/TcPacketBase.h       | 16 +++++-----
 tmtcpacket/pus/tc/TcPacketPus.cpp      | 44 +++++++++++++++-----------
 tmtcpacket/pus/tc/TcPacketPus.h        | 20 ++++++------
 tmtcservices/CommandingServiceBase.cpp |  1 +
 6 files changed, 51 insertions(+), 38 deletions(-)

diff --git a/tmtcpacket/SpacePacketBase.cpp b/tmtcpacket/SpacePacketBase.cpp
index e13af8d06..14198027d 100644
--- a/tmtcpacket/SpacePacketBase.cpp
+++ b/tmtcpacket/SpacePacketBase.cpp
@@ -72,7 +72,7 @@ void SpacePacketBase::setPacketSequenceCount( uint16_t new_count) {
 	this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF );
 }
 
-uint16_t SpacePacketBase::getPacketDataLength( void ) {
+uint16_t SpacePacketBase::getPacketDataLength() const {
 	return ( (this->data->header.packet_length_h) << 8 )
 		+ this->data->header.packet_length_l;
 }
diff --git a/tmtcpacket/SpacePacketBase.h b/tmtcpacket/SpacePacketBase.h
index 19cbd0741..13cb31308 100644
--- a/tmtcpacket/SpacePacketBase.h
+++ b/tmtcpacket/SpacePacketBase.h
@@ -138,9 +138,11 @@ public:
 	 * Returns the packet data length, which is the fifth and sixth byte of the
 	 * CCSDS Primary Header. The packet data length is the size of every kind
 	 * of data \b after the CCSDS Primary Header \b -1.
-	 * @return The CCSDS packet data length.
+	 * @return
+	 * The CCSDS packet data length. uint16_t is sufficient,
+	 * because this is limit in CCSDS standard
 	 */
-	uint16_t getPacketDataLength( void ); //uint16_t is sufficient, because this is limit in CCSDS standard
+	uint16_t getPacketDataLength(void) const;
 	/**
 	 * Sets the packet data length, which is the fifth and sixth byte of the
 	 * CCSDS Primary Header.
diff --git a/tmtcpacket/pus/tc/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
index f29f9b35d..68f088963 100644
--- a/tmtcpacket/pus/tc/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -53,7 +53,7 @@ public:
 	 * highest bit of the first byte of the Data Field Header.
 	 * @return	the CCSDS Secondary Header Flag
 	 */
-	virtual uint8_t getSecondaryHeaderFlag() = 0;
+	virtual uint8_t getSecondaryHeaderFlag() const = 0;
 	/**
 	 * This command returns the TC Packet PUS Version Number.
 	 * The version number of ECSS PUS 2003 is 1.
@@ -61,7 +61,7 @@ public:
 	 * first byte.
 	 * @return
 	 */
-	virtual uint8_t getPusVersionNumber() = 0;
+	virtual uint8_t getPusVersionNumber() const = 0;
 	/**
 	 * This is a getter for the packet's Ack field, which are the lowest four
 	 * bits of the first byte of the Data Field Header.
@@ -69,7 +69,7 @@ public:
 	 * It is packed in a uint8_t variable.
 	 * @return	The packet's PUS Ack field.
 	 */
-	virtual uint8_t getAcknowledgeFlags() = 0;
+	virtual uint8_t getAcknowledgeFlags() const = 0;
 	/**
 	 * This is a getter for the packet's PUS Service ID, which is the second
 	 * byte of the Data Field Header.
@@ -81,13 +81,13 @@ public:
 	 * third byte of the Data Field Header.
 	 * @return	The packet's PUS Service Subtype.
 	 */
-	virtual uint8_t getSubService() = 0;
+	virtual uint8_t getSubService() const = 0;
 	/**
 	 * The source ID can be used to have an additional identifier, e.g. for different ground
 	 * station.
 	 * @return
 	 */
-	uint8_t getSourceId();
+	virtual uint16_t getSourceId() const = 0;
 
 	/**
 	 * This is a getter for a pointer to the packet's Application data.
@@ -105,7 +105,7 @@ public:
 	 * @return	The size of the PUS Application Data (without Error Control
 	 * 		field)
 	 */
-	virtual uint16_t getApplicationDataSize() = 0;
+	virtual uint16_t getApplicationDataSize() const = 0;
 	/**
 	 * This getter returns the Error Control Field of the packet.
 	 *
@@ -114,7 +114,7 @@ public:
 	 * supposed to be a 16bit-CRC.
 	 * @return	The PUS Error Control
 	 */
-	virtual uint16_t getErrorControl() = 0;
+	virtual uint16_t getErrorControl() const = 0;
 	/**
 	 * With this method, the Error Control Field is updated to match the
 	 * current content of the packet.
@@ -126,7 +126,7 @@ public:
 	 * @param appDataLen
 	 * @return
 	 */
-	virtual size_t calculateFullPacketLength(size_t appDataLen) = 0;
+	virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
 
     /**
      * This is a debugging helper method that prints the whole packet content
diff --git a/tmtcpacket/pus/tc/TcPacketPus.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
index 7a184f3c6..1df61c079 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -7,15 +7,28 @@ TcPacketPus::TcPacketPus(const uint8_t *setData): TcPacketBase(setData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(setData));
 }
 
+void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
+        uint8_t ack, uint8_t service, uint8_t subservice) {
+    initSpacePacketHeader(true, true, apid, sequenceCount);
+    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
+    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
+    // Data Field Header:
+    // Set CCSDS_secondary_header_flag to 0 and version number to 001
+    tcData->dataField.versionTypeAck = 0b00010000;
+    tcData->dataField.versionTypeAck |= (ack & 0x0F);
+    tcData->dataField.serviceType = service;
+    tcData->dataField.serviceSubtype = subservice;
+}
+
 uint8_t TcPacketPus::getService() const {
     return tcData->dataField.serviceType;
 }
 
-uint8_t TcPacketPus::getSubService() {
+uint8_t TcPacketPus::getSubService() const {
     return tcData->dataField.serviceSubtype;
 }
 
-uint8_t TcPacketPus::getAcknowledgeFlags() {
+uint8_t TcPacketPus::getAcknowledgeFlags() const {
     return tcData->dataField.versionTypeAck & 0b00001111;
 }
 
@@ -23,11 +36,11 @@ const uint8_t* TcPacketPus::getApplicationData() const {
     return &tcData->appData;
 }
 
-uint16_t TcPacketPus::getApplicationDataSize() {
+uint16_t TcPacketPus::getApplicationDataSize() const {
     return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
 }
 
-uint16_t TcPacketPus::getErrorControl() {
+uint16_t TcPacketPus::getErrorControl() const {
     uint16_t size = getApplicationDataSize() + CRC_SIZE;
     uint8_t* p_to_buffer = &tcData->appData;
     return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
@@ -48,28 +61,23 @@ void TcPacketPus::setData(const uint8_t* pData) {
     tcData = reinterpret_cast<TcPacketPointer*>(const_cast<uint8_t*>(pData));
 }
 
-uint8_t TcPacketPus::getSecondaryHeaderFlag() {
+uint8_t TcPacketPus::getSecondaryHeaderFlag() const {
     return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
 }
 
-uint8_t TcPacketPus::getPusVersionNumber() {
+uint8_t TcPacketPus::getPusVersionNumber() const {
     return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
 }
 
-void TcPacketPus::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
-        uint8_t ack, uint8_t service, uint8_t subservice) {
-    initSpacePacketHeader(true, true, apid, sequenceCount);
-    std::memset(&tcData->dataField, 0, sizeof(tcData->dataField));
-    setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
-    // Data Field Header:
-    // Set CCSDS_secondary_header_flag to 0 and version number to 001
-    tcData->dataField.versionTypeAck = 0b00010000;
-    tcData->dataField.versionTypeAck |= (ack & 0x0F);
-    tcData->dataField.serviceType = service;
-    tcData->dataField.serviceSubtype = subservice;
+uint16_t TcPacketPus::getSourceId() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    return (tcData->dataField.sourceIdH << 8) | tcData->dataField.sourceIdL;
+#else
+    return tcData->dataField.sourceId;
+#endif
 }
 
-size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) {
+size_t TcPacketPus::calculateFullPacketLength(size_t appDataLen) const {
     return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
             appDataLen + TcPacketBase::CRC_SIZE;
 }
diff --git a/tmtcpacket/pus/tc/TcPacketPus.h b/tmtcpacket/pus/tc/TcPacketPus.h
index fb8483c52..b50b868d2 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.h
+++ b/tmtcpacket/pus/tc/TcPacketPus.h
@@ -18,7 +18,8 @@ struct PUSTcDataFieldHeader {
     uint8_t serviceType;
     uint8_t serviceSubtype;
 #if FSFW_USE_PUS_C_TELECOMMANDS == 1
-    uint16_t sourceId;
+    uint8_t sourceIdH;
+    uint8_t sourceIdL;
 #else
     uint8_t sourceId;
 #endif
@@ -49,16 +50,17 @@ public:
     TcPacketPus(const uint8_t* setData);
 
     // Base class overrides
-    virtual uint8_t getSecondaryHeaderFlag() override;
-    virtual uint8_t getPusVersionNumber() override;
-    virtual uint8_t getAcknowledgeFlags() override;
-    virtual uint8_t getService() const override;
-    virtual uint8_t getSubService() override;
+    uint8_t getSecondaryHeaderFlag() const override;
+    uint8_t getPusVersionNumber() const override;
+    uint8_t getAcknowledgeFlags() const override;
+    uint8_t getService() const override;
+    uint8_t getSubService() const override;
+    uint16_t getSourceId() const override;
     const uint8_t* getApplicationData() const override;
-    uint16_t getApplicationDataSize() override;
-    uint16_t getErrorControl() override;
+    uint16_t getApplicationDataSize() const override;
+    uint16_t getErrorControl() const override;
     void setErrorControl() override;
-    size_t calculateFullPacketLength(size_t appDataLen) override;
+    size_t calculateFullPacketLength(size_t appDataLen) const override;
 
 protected:
 
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index b416ffbe1..307a2a982 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -259,6 +259,7 @@ void CommandingServiceBase::handleRequestQueue() {
 			rejectPacket(tc_verification::START_FAILURE, &packet, INVALID_SUBSERVICE);
 			continue;
 		}
+
 		result = getMessageQueueAndObject(packet.getSubService(),
 				packet.getApplicationData(), packet.getApplicationDataSize(),
 				&queue, &objectId);

From 7cf4aa0d5a8cc707721785e9a55f1fd0e2eaf652 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 11:44:39 +0200
Subject: [PATCH 08/11] fixes for pus tc c

---
 tcdistribution/TcPacketCheck.cpp  | 1 +
 tcdistribution/TcPacketCheck.h    | 6 ++++++
 tmtcpacket/pus/tc/TcPacketPus.cpp | 9 +++++++++
 3 files changed, 16 insertions(+)

diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp
index dce0a4583..b3a025a49 100644
--- a/tcdistribution/TcPacketCheck.cpp
+++ b/tcdistribution/TcPacketCheck.cpp
@@ -32,6 +32,7 @@ ReturnValue_t TcPacketCheck::checkPacket(TcPacketStoredBase* currentPacket) {
     if (not currentPacket->isSizeCorrect()) {
         return INCOMPLETE_PACKET;
     }
+
     condition = (tcPacketBase->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) ||
             (tcPacketBase->getPusVersionNumber() != PUS_VERSION_NUMBER);
     if (condition) {
diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h
index f7422c192..7106b7e41 100644
--- a/tcdistribution/TcPacketCheck.h
+++ b/tcdistribution/TcPacketCheck.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_
 
+#include "../FSFW.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
@@ -24,7 +25,12 @@ protected:
     /**
      * Describes the TC Packet PUS Version Number a packet must have to pass.
      */
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    static constexpr uint8_t PUS_VERSION_NUMBER = 2;
+#else
     static constexpr uint8_t PUS_VERSION_NUMBER = 1;
+#endif
+
     /**
      * The packet id each correct packet should have.
      * It is composed of the APID and some static fields.
diff --git a/tmtcpacket/pus/tc/TcPacketPus.cpp b/tmtcpacket/pus/tc/TcPacketPus.cpp
index 1df61c079..334da4e78 100644
--- a/tmtcpacket/pus/tc/TcPacketPus.cpp
+++ b/tmtcpacket/pus/tc/TcPacketPus.cpp
@@ -62,11 +62,20 @@ void TcPacketPus::setData(const uint8_t* pData) {
 }
 
 uint8_t TcPacketPus::getSecondaryHeaderFlag() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    // Does not exist for PUS C
+    return 0;
+#else
     return (tcData->dataField.versionTypeAck & 0b10000000) >> 7;
+#endif
 }
 
 uint8_t TcPacketPus::getPusVersionNumber() const {
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    return (tcData->dataField.versionTypeAck & 0b11110000) >> 4;
+#else
     return (tcData->dataField.versionTypeAck & 0b01110000) >> 4;
+#endif
 }
 
 uint16_t TcPacketPus::getSourceId() const {

From a7068acca76d6e65a7f9f18bb566b2d8a330dcf3 Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 14:38:09 +0200
Subject: [PATCH 09/11] updated generic file system message

---
 memory/GenericFileSystemMessage.cpp | 25 +++++++++++++++++++++++++
 1 file changed, 25 insertions(+)

diff --git a/memory/GenericFileSystemMessage.cpp b/memory/GenericFileSystemMessage.cpp
index e4edc7acf..005f4edeb 100644
--- a/memory/GenericFileSystemMessage.cpp
+++ b/memory/GenericFileSystemMessage.cpp
@@ -1,5 +1,7 @@
 #include "GenericFileSystemMessage.h"
 
+#include "../objectmanager/ObjectManager.h"
+#include "../storagemanager/StorageManagerIF.h"
 
 void GenericFileSystemMessage::setCreateFileCommand(CommandMessage* message,
         store_address_t storeId) {
@@ -132,5 +134,28 @@ bool GenericFileSystemMessage::getReadReply(const CommandMessage *message,
 }
 
 ReturnValue_t GenericFileSystemMessage::clear(CommandMessage* message) {
+	switch(message->getCommand()) {
+	case(CMD_CREATE_FILE):
+	case(CMD_DELETE_FILE):
+	case(CMD_CREATE_DIRECTORY):
+	case(CMD_REPORT_FILE_ATTRIBUTES):
+	case(REPLY_REPORT_FILE_ATTRIBUTES):
+	case(CMD_LOCK_FILE):
+	case(CMD_UNLOCK_FILE):
+	case(CMD_COPY_FILE):
+	case(REPLY_READ_FROM_FILE):
+	case(CMD_READ_FROM_FILE):
+	case(CMD_APPEND_TO_FILE):
+	case(CMD_FINISH_APPEND_TO_FILE):
+	case(REPLY_READ_FINISHED_STOP):
+	case(REPLY_FINISH_APPEND): {
+		store_address_t storeId = GenericFileSystemMessage::getStoreId(message);
+		auto ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+		if(ipcStore == nullptr) {
+			return HasReturnvaluesIF::RETURN_FAILED;
+		}
+		return ipcStore->deleteData(storeId);
+	}
+	}
     return HasReturnvaluesIF::RETURN_OK;
 }

From aa33ff2f486f74797ed20e6b631907af7d250c2c Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Mon, 14 Jun 2021 15:14:57 +0200
Subject: [PATCH 10/11] max tm packet size now configurable

---
 defaultcfg/fsfwconfig/FSFWConfig.h           |   2 +
 tmtcpacket/packetmatcher/ApidMatcher.h       |  54 ++--
 tmtcpacket/packetmatcher/PacketMatchTree.cpp | 319 +++++++++----------
 tmtcpacket/packetmatcher/PacketMatchTree.h   |  46 +--
 tmtcpacket/packetmatcher/ServiceMatcher.h    |  50 +--
 tmtcpacket/packetmatcher/SubserviceMatcher.h |  54 ++--
 tmtcpacket/pus/tc/TcPacketBase.h             | 206 ++++++------
 tmtcpacket/pus/tc/TcPacketStoredBase.cpp     |  34 +-
 tmtcpacket/pus/tc/TcPacketStoredBase.h       |  46 +--
 tmtcpacket/pus/tm/TmPacketBase.cpp           |   4 +-
 tmtcpacket/pus/tm/TmPacketPusA.h             |   5 +-
 tmtcpacket/pus/tm/TmPacketPusC.h             |   3 +-
 12 files changed, 409 insertions(+), 414 deletions(-)

diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 5518e33c1..adf9912f9 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -68,6 +68,8 @@ static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 
+static constexpr size_t FSFW_MAX_TM_PACKET_SIZE = 2048;
+
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/tmtcpacket/packetmatcher/ApidMatcher.h b/tmtcpacket/packetmatcher/ApidMatcher.h
index 64f73dbf0..33db71519 100644
--- a/tmtcpacket/packetmatcher/ApidMatcher.h
+++ b/tmtcpacket/packetmatcher/ApidMatcher.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
@@ -7,32 +7,32 @@
 
 class ApidMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
-	uint16_t apid;
+    uint16_t apid;
 public:
-	ApidMatcher(uint16_t setApid) :
-			apid(setApid) {
-	}
-	ApidMatcher(TmPacketMinimal* test) :
-			apid(test->getAPID()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getAPID() == apid) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&apid, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&apid);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&apid, buffer, size, streamEndianness);
-	}
+    ApidMatcher(uint16_t setApid) :
+        apid(setApid) {
+    }
+    ApidMatcher(TmPacketMinimal* test) :
+        apid(test->getAPID()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getAPID() == apid) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&apid, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&apid);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&apid, buffer, size, streamEndianness);
+    }
 };
 
 
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.cpp b/tmtcpacket/packetmatcher/PacketMatchTree.cpp
index ac72b3e77..05e176ef2 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.cpp
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.cpp
@@ -5,197 +5,194 @@
 
 // This should be configurable..
 const LocalPool::LocalPoolConfig PacketMatchTree::poolConfig = {
-		{10, sizeof(ServiceMatcher)},
-		{20, sizeof(SubServiceMatcher)},
-		{2, sizeof(ApidMatcher)},
-		{40, sizeof(PacketMatchTree::Node)}
+        {10, sizeof(ServiceMatcher)},
+        {20, sizeof(SubServiceMatcher)},
+        {2, sizeof(ApidMatcher)},
+        {40, sizeof(PacketMatchTree::Node)}
 };
 
-PacketMatchTree::PacketMatchTree(Node* root) :
-		MatchTree<TmPacketMinimal*>(root, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(Node* root): MatchTree<TmPacketMinimal*>(root, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
-PacketMatchTree::PacketMatchTree(iterator root) :
-		MatchTree<TmPacketMinimal*>(root.element, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(iterator root): MatchTree<TmPacketMinimal*>(root.element, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
-PacketMatchTree::PacketMatchTree() :
-		MatchTree<TmPacketMinimal*>((Node*) NULL, 2),
-		factoryBackend(0, poolConfig, false, true),
-		factory(&factoryBackend) {
+PacketMatchTree::PacketMatchTree(): MatchTree<TmPacketMinimal*>((Node*) NULL, 2),
+        factoryBackend(0, poolConfig, false, true),
+        factory(&factoryBackend) {
 }
 
 PacketMatchTree::~PacketMatchTree() {
 }
 
 ReturnValue_t PacketMatchTree::addMatch(uint16_t apid, uint8_t type,
-		uint8_t subtype) {
-	//We assume adding APID is always requested.
-	TmPacketMinimal::TmPacketMinimalPointer data;
-	data.data_field.service_type = type;
-	data.data_field.service_subtype = subtype;
-	TmPacketMinimal testPacket((uint8_t*) &data);
-	testPacket.setAPID(apid);
-	iterator lastTest;
-	iterator rollback;
-	ReturnValue_t result = findOrInsertMatch<TmPacketMinimal*, ApidMatcher>(
-			this->begin(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		rollback = lastTest;
-	} else if (result != RETURN_OK) {
-		return result;
-	}
-	if (type == 0) {
-		//Check if lastTest has no children, otherwise, delete them,
-		//as a more general check is requested.
-		if (lastTest.left() != this->end()) {
-			removeElementAndAllChildren(lastTest.left());
-		}
-		return RETURN_OK;
-	}
-	//Type insertion required.
-	result = findOrInsertMatch<TmPacketMinimal*, ServiceMatcher>(
-			lastTest.left(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		if  (rollback == this->end()) {
-			rollback = lastTest;
-		}
-	} else if (result != RETURN_OK) {
-		if (rollback != this->end()) {
-			removeElementAndAllChildren(rollback);
-		}
-		return result;
-	}
-	if (subtype == 0) {
-		if (lastTest.left() != this->end()) {
-			//See above
-			removeElementAndAllChildren(lastTest.left());
-		}
-		return RETURN_OK;
-	}
-	//Subtype insertion required.
-	result = findOrInsertMatch<TmPacketMinimal*, SubServiceMatcher>(
-			lastTest.left(), &testPacket, &lastTest);
-	if (result == NEW_NODE_CREATED) {
-		return RETURN_OK;
-	} else if (result != RETURN_OK) {
-		if (rollback != this->end()) {
-			removeElementAndAllChildren(rollback);
-		}
-		return result;
-	}
-	return RETURN_OK;
+        uint8_t subtype) {
+    //We assume adding APID is always requested.
+    TmPacketMinimal::TmPacketMinimalPointer data;
+    data.data_field.service_type = type;
+    data.data_field.service_subtype = subtype;
+    TmPacketMinimal testPacket((uint8_t*) &data);
+    testPacket.setAPID(apid);
+    iterator lastTest;
+    iterator rollback;
+    ReturnValue_t result = findOrInsertMatch<TmPacketMinimal*, ApidMatcher>(
+            this->begin(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        rollback = lastTest;
+    } else if (result != RETURN_OK) {
+        return result;
+    }
+    if (type == 0) {
+        //Check if lastTest has no children, otherwise, delete them,
+        //as a more general check is requested.
+        if (lastTest.left() != this->end()) {
+            removeElementAndAllChildren(lastTest.left());
+        }
+        return RETURN_OK;
+    }
+    //Type insertion required.
+    result = findOrInsertMatch<TmPacketMinimal*, ServiceMatcher>(
+            lastTest.left(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        if  (rollback == this->end()) {
+            rollback = lastTest;
+        }
+    } else if (result != RETURN_OK) {
+        if (rollback != this->end()) {
+            removeElementAndAllChildren(rollback);
+        }
+        return result;
+    }
+    if (subtype == 0) {
+        if (lastTest.left() != this->end()) {
+            //See above
+            removeElementAndAllChildren(lastTest.left());
+        }
+        return RETURN_OK;
+    }
+    //Subtype insertion required.
+    result = findOrInsertMatch<TmPacketMinimal*, SubServiceMatcher>(
+            lastTest.left(), &testPacket, &lastTest);
+    if (result == NEW_NODE_CREATED) {
+        return RETURN_OK;
+    } else if (result != RETURN_OK) {
+        if (rollback != this->end()) {
+            removeElementAndAllChildren(rollback);
+        }
+        return result;
+    }
+    return RETURN_OK;
 }
 
 template<typename VALUE_T, typename INSERTION_T>
 ReturnValue_t PacketMatchTree::findOrInsertMatch(iterator startAt, VALUE_T test,
-		iterator* lastTest) {
-	bool attachToBranch = AND;
-	iterator iter = startAt;
-	while (iter != this->end()) {
-		bool isMatch = iter->match(test);
-		attachToBranch = OR;
-		*lastTest = iter;
-		if (isMatch) {
-			return RETURN_OK;
-		} else {
-			//Go down OR branch.
-			iter = iter.right();
-		}
-	}
-	//Only reached if nothing was found.
-	SerializeableMatcherIF<VALUE_T>* newContent = factory.generate<INSERTION_T>(
-			test);
-	if (newContent == NULL) {
-		return FULL;
-	}
-	Node* newNode = factory.generate<Node>(newContent);
-	if (newNode == NULL) {
-		//Need to make sure partially generated content is deleted, otherwise, that's a leak.
-		factory.destroy<INSERTION_T>(static_cast<INSERTION_T*>(newContent));
-		return FULL;
-	}
-	*lastTest = insert(attachToBranch, *lastTest, newNode);
-	if (*lastTest == end()) {
-		//This actaully never fails, so creating a dedicated returncode seems an overshoot.
-		return RETURN_FAILED;
-	}
-	return NEW_NODE_CREATED;
+        iterator* lastTest) {
+    bool attachToBranch = AND;
+    iterator iter = startAt;
+    while (iter != this->end()) {
+        bool isMatch = iter->match(test);
+        attachToBranch = OR;
+        *lastTest = iter;
+        if (isMatch) {
+            return RETURN_OK;
+        } else {
+            //Go down OR branch.
+            iter = iter.right();
+        }
+    }
+    //Only reached if nothing was found.
+    SerializeableMatcherIF<VALUE_T>* newContent = factory.generate<INSERTION_T>(
+            test);
+    if (newContent == NULL) {
+        return FULL;
+    }
+    Node* newNode = factory.generate<Node>(newContent);
+    if (newNode == NULL) {
+        //Need to make sure partially generated content is deleted, otherwise, that's a leak.
+        factory.destroy<INSERTION_T>(static_cast<INSERTION_T*>(newContent));
+        return FULL;
+    }
+    *lastTest = insert(attachToBranch, *lastTest, newNode);
+    if (*lastTest == end()) {
+        //This actaully never fails, so creating a dedicated returncode seems an overshoot.
+        return RETURN_FAILED;
+    }
+    return NEW_NODE_CREATED;
 }
 
 ReturnValue_t PacketMatchTree::removeMatch(uint16_t apid, uint8_t type,
-		uint8_t subtype) {
-	TmPacketMinimal::TmPacketMinimalPointer data;
-	data.data_field.service_type = type;
-	data.data_field.service_subtype = subtype;
-	TmPacketMinimal testPacket((uint8_t*) &data);
-	testPacket.setAPID(apid);
-	iterator foundElement = findMatch(begin(), &testPacket);
-	if (foundElement == this->end()) {
-		return NO_MATCH;
-	}
-	if (type == 0) {
-		if (foundElement.left() == end()) {
-			return removeElementAndReconnectChildren(foundElement);
-		} else {
-			return TOO_GENERAL_REQUEST;
-		}
-	}
-	//Go down AND branch. Will abort if empty.
-	foundElement = findMatch(foundElement.left(), &testPacket);
-	if (foundElement == this->end()) {
-		return NO_MATCH;
-	}
-	if (subtype == 0) {
-		if (foundElement.left() == end()) {
-			return removeElementAndReconnectChildren(foundElement);
-		} else {
-			return TOO_GENERAL_REQUEST;
-		}
-	}
-	//Again, go down AND branch.
-	foundElement = findMatch(foundElement.left(), &testPacket);
-	if (foundElement == end()) {
-		return NO_MATCH;
-	}
-	return removeElementAndReconnectChildren(foundElement);
+        uint8_t subtype) {
+    TmPacketMinimal::TmPacketMinimalPointer data;
+    data.data_field.service_type = type;
+    data.data_field.service_subtype = subtype;
+    TmPacketMinimal testPacket((uint8_t*) &data);
+    testPacket.setAPID(apid);
+    iterator foundElement = findMatch(begin(), &testPacket);
+    if (foundElement == this->end()) {
+        return NO_MATCH;
+    }
+    if (type == 0) {
+        if (foundElement.left() == end()) {
+            return removeElementAndReconnectChildren(foundElement);
+        } else {
+            return TOO_GENERAL_REQUEST;
+        }
+    }
+    //Go down AND branch. Will abort if empty.
+    foundElement = findMatch(foundElement.left(), &testPacket);
+    if (foundElement == this->end()) {
+        return NO_MATCH;
+    }
+    if (subtype == 0) {
+        if (foundElement.left() == end()) {
+            return removeElementAndReconnectChildren(foundElement);
+        } else {
+            return TOO_GENERAL_REQUEST;
+        }
+    }
+    //Again, go down AND branch.
+    foundElement = findMatch(foundElement.left(), &testPacket);
+    if (foundElement == end()) {
+        return NO_MATCH;
+    }
+    return removeElementAndReconnectChildren(foundElement);
 }
 
 PacketMatchTree::iterator PacketMatchTree::findMatch(iterator startAt,
-		TmPacketMinimal* test) {
-	iterator iter = startAt;
-	while (iter != end()) {
-		bool isMatch = iter->match(test);
-		if (isMatch) {
-			break;
-		} else {
-			iter = iter.right(); //next OR element
-		}
-	}
-	return iter;
+        TmPacketMinimal* test) {
+    iterator iter = startAt;
+    while (iter != end()) {
+        bool isMatch = iter->match(test);
+        if (isMatch) {
+            break;
+        } else {
+            iter = iter.right(); //next OR element
+        }
+    }
+    return iter;
 }
 
 ReturnValue_t PacketMatchTree::initialize() {
-	return factoryBackend.initialize();
+    return factoryBackend.initialize();
 }
 
 
 ReturnValue_t PacketMatchTree::changeMatch(bool addToMatch, uint16_t apid,
-		uint8_t type, uint8_t subtype) {
-	if (addToMatch) {
-		return addMatch(apid, type, subtype);
-	} else {
-		return removeMatch(apid, type, subtype);
-	}
+        uint8_t type, uint8_t subtype) {
+    if (addToMatch) {
+        return addMatch(apid, type, subtype);
+    } else {
+        return removeMatch(apid, type, subtype);
+    }
 }
 
 ReturnValue_t PacketMatchTree::cleanUpElement(iterator position) {
-	factory.destroy(position.element->value);
-	//Go on anyway, there's nothing we can do.
-	//SHOULDDO: Throw event, or write debug message?
-	return factory.destroy(position.element);
+    factory.destroy(position.element->value);
+    //Go on anyway, there's nothing we can do.
+    //SHOULDDO: Throw event, or write debug message?
+    return factory.destroy(position.element);
 }
diff --git a/tmtcpacket/packetmatcher/PacketMatchTree.h b/tmtcpacket/packetmatcher/PacketMatchTree.h
index 735f05664..a40b5099c 100644
--- a/tmtcpacket/packetmatcher/PacketMatchTree.h
+++ b/tmtcpacket/packetmatcher/PacketMatchTree.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
 
 #include "../../container/PlacementFactory.h"
 #include "../../globalfunctions/matching/MatchTree.h"
@@ -8,29 +8,29 @@
 
 class PacketMatchTree: public MatchTree<TmPacketMinimal*>, public HasReturnvaluesIF {
 public:
-	PacketMatchTree(Node* root);
-	PacketMatchTree(iterator root);
-	PacketMatchTree();
-	virtual ~PacketMatchTree();
-	ReturnValue_t changeMatch(bool addToMatch, uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t addMatch(uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t removeMatch(uint16_t apid, uint8_t type = 0,
-			uint8_t subtype = 0);
-	ReturnValue_t initialize();
+    PacketMatchTree(Node* root);
+    PacketMatchTree(iterator root);
+    PacketMatchTree();
+    virtual ~PacketMatchTree();
+    ReturnValue_t changeMatch(bool addToMatch, uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t addMatch(uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t removeMatch(uint16_t apid, uint8_t type = 0,
+            uint8_t subtype = 0);
+    ReturnValue_t initialize();
 protected:
-	ReturnValue_t cleanUpElement(iterator position);
+    ReturnValue_t cleanUpElement(iterator position);
 private:
-	static const uint8_t N_POOLS = 4;
-	LocalPool factoryBackend;
-	PlacementFactory factory;
-	static const LocalPool::LocalPoolConfig poolConfig;
-	static const uint16_t POOL_SIZES[N_POOLS];
-	static const uint16_t N_ELEMENTS[N_POOLS];
-	template<typename VALUE_T, typename INSERTION_T>
-	ReturnValue_t findOrInsertMatch(iterator startAt, VALUE_T test, iterator* lastTest);
-	iterator findMatch(iterator startAt, TmPacketMinimal* test);
+    static const uint8_t N_POOLS = 4;
+    LocalPool factoryBackend;
+    PlacementFactory factory;
+    static const LocalPool::LocalPoolConfig poolConfig;
+    static const uint16_t POOL_SIZES[N_POOLS];
+    static const uint16_t N_ELEMENTS[N_POOLS];
+    template<typename VALUE_T, typename INSERTION_T>
+    ReturnValue_t findOrInsertMatch(iterator startAt, VALUE_T test, iterator* lastTest);
+    iterator findMatch(iterator startAt, TmPacketMinimal* test);
 };
 
 #endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ */
diff --git a/tmtcpacket/packetmatcher/ServiceMatcher.h b/tmtcpacket/packetmatcher/ServiceMatcher.h
index 57f491f3f..67d09d2b4 100644
--- a/tmtcpacket/packetmatcher/ServiceMatcher.h
+++ b/tmtcpacket/packetmatcher/ServiceMatcher.h
@@ -7,32 +7,32 @@
 
 class ServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 private:
-	uint8_t service;
+    uint8_t service;
 public:
-	ServiceMatcher(uint8_t setService) :
-			service(setService) {
-	}
-	ServiceMatcher(TmPacketMinimal* test) :
-			service(test->getService()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getService() == service) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&service, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&service);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&service, buffer, size, streamEndianness);
-	}
+    ServiceMatcher(uint8_t setService) :
+        service(setService) {
+    }
+    ServiceMatcher(TmPacketMinimal* test) :
+        service(test->getService()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getService() == service) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&service, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&service);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&service, buffer, size, streamEndianness);
+    }
 };
 
 
diff --git a/tmtcpacket/packetmatcher/SubserviceMatcher.h b/tmtcpacket/packetmatcher/SubserviceMatcher.h
index 80681b5db..e570b4520 100644
--- a/tmtcpacket/packetmatcher/SubserviceMatcher.h
+++ b/tmtcpacket/packetmatcher/SubserviceMatcher.h
@@ -1,5 +1,5 @@
-#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
-#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
 
 #include "../../globalfunctions/matching/SerializeableMatcherIF.h"
 #include "../../serialize/SerializeAdapter.h"
@@ -7,32 +7,32 @@
 
 class SubServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
 public:
-	SubServiceMatcher(uint8_t subService) :
-			subService(subService) {
-	}
-	SubServiceMatcher(TmPacketMinimal* test) :
-			subService(test->getSubService()) {
-	}
-	bool match(TmPacketMinimal* packet) {
-		if (packet->getSubService() == subService) {
-			return true;
-		} else {
-			return false;
-		}
-	}
-	ReturnValue_t serialize(uint8_t** buffer, size_t* size,
-			size_t maxSize, Endianness streamEndianness) const {
-		return SerializeAdapter::serialize(&subService, buffer, size, maxSize, streamEndianness);
-	}
-	size_t getSerializedSize() const {
-		return SerializeAdapter::getSerializedSize(&subService);
-	}
-	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-			Endianness streamEndianness) {
-		return SerializeAdapter::deSerialize(&subService, buffer, size, streamEndianness);
-	}
+    SubServiceMatcher(uint8_t subService) :
+        subService(subService) {
+    }
+    SubServiceMatcher(TmPacketMinimal* test) :
+        subService(test->getSubService()) {
+    }
+    bool match(TmPacketMinimal* packet) {
+        if (packet->getSubService() == subService) {
+            return true;
+        } else {
+            return false;
+        }
+    }
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size,
+            size_t maxSize, Endianness streamEndianness) const {
+        return SerializeAdapter::serialize(&subService, buffer, size, maxSize, streamEndianness);
+    }
+    size_t getSerializedSize() const {
+        return SerializeAdapter::getSerializedSize(&subService);
+    }
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+            Endianness streamEndianness) {
+        return SerializeAdapter::deSerialize(&subService, buffer, size, streamEndianness);
+    }
 private:
-	uint8_t subService;
+    uint8_t subService;
 };
 
 
diff --git a/tmtcpacket/pus/tc/TcPacketBase.h b/tmtcpacket/pus/tc/TcPacketBase.h
index 68f088963..e28722469 100644
--- a/tmtcpacket/pus/tc/TcPacketBase.h
+++ b/tmtcpacket/pus/tc/TcPacketBase.h
@@ -19,114 +19,114 @@ class TcPacketBase : public SpacePacketBase {
     friend class TcPacketStoredBase;
 public:
 
-	enum AckField {
-		//! No acknowledgements are expected.
-		ACK_NONE = 0b0000,
-		//! Acknowledgements on acceptance are expected.
-		ACK_ACCEPTANCE = 0b0001,
-		//! Acknowledgements on start are expected.
-		ACK_START = 0b0010,
-		//! Acknowledgements on step are expected.
-		ACK_STEP = 0b0100,
-		//! Acknowledfgement on completion are expected.
-		ACK_COMPLETION = 0b1000
-	};
+    enum AckField {
+        //! No acknowledgements are expected.
+        ACK_NONE = 0b0000,
+        //! Acknowledgements on acceptance are expected.
+        ACK_ACCEPTANCE = 0b0001,
+        //! Acknowledgements on start are expected.
+        ACK_START = 0b0010,
+        //! Acknowledgements on step are expected.
+        ACK_STEP = 0b0100,
+        //! Acknowledfgement on completion are expected.
+        ACK_COMPLETION = 0b1000
+    };
 
-	static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP |
-			ACK_COMPLETION;
+    static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP |
+            ACK_COMPLETION;
 
-	/**
-	 * This is the default constructor.
-	 * It sets its internal data pointer to the address passed and also
-	 * forwards the data pointer to the parent SpacePacketBase class.
-	 * @param setData	The position where the packet data lies.
-	 */
-	TcPacketBase( const uint8_t* setData );
-	/**
-	 * This is the empty default destructor.
-	 */
-	virtual ~TcPacketBase();
+    /**
+     * This is the default constructor.
+     * It sets its internal data pointer to the address passed and also
+     * forwards the data pointer to the parent SpacePacketBase class.
+     * @param setData	The position where the packet data lies.
+     */
+    TcPacketBase( const uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TcPacketBase();
 
-	/**
-	 * This command returns the CCSDS Secondary Header Flag.
-	 * It shall always be zero for PUS Packets. This is the
-	 * highest bit of the first byte of the Data Field Header.
-	 * @return	the CCSDS Secondary Header Flag
-	 */
-	virtual uint8_t getSecondaryHeaderFlag() const = 0;
-	/**
-	 * This command returns the TC Packet PUS Version Number.
-	 * The version number of ECSS PUS 2003 is 1.
-	 * It consists of the second to fourth highest bits of the
-	 * first byte.
-	 * @return
-	 */
-	virtual uint8_t getPusVersionNumber() const = 0;
-	/**
-	 * This is a getter for the packet's Ack field, which are the lowest four
-	 * bits of the first byte of the Data Field Header.
-	 *
-	 * It is packed in a uint8_t variable.
-	 * @return	The packet's PUS Ack field.
-	 */
-	virtual uint8_t getAcknowledgeFlags() const = 0;
-	/**
-	 * This is a getter for the packet's PUS Service ID, which is the second
-	 * byte of the Data Field Header.
-	 * @return	The packet's PUS Service ID.
-	 */
-	virtual uint8_t getService() const = 0;
-	/**
-	 * This is a getter for the packet's PUS Service Subtype, which is the
-	 * third byte of the Data Field Header.
-	 * @return	The packet's PUS Service Subtype.
-	 */
-	virtual uint8_t getSubService() const = 0;
-	/**
-	 * The source ID can be used to have an additional identifier, e.g. for different ground
-	 * station.
-	 * @return
-	 */
-	virtual uint16_t getSourceId() const = 0;
+    /**
+     * This command returns the CCSDS Secondary Header Flag.
+     * It shall always be zero for PUS Packets. This is the
+     * highest bit of the first byte of the Data Field Header.
+     * @return	the CCSDS Secondary Header Flag
+     */
+    virtual uint8_t getSecondaryHeaderFlag() const = 0;
+    /**
+     * This command returns the TC Packet PUS Version Number.
+     * The version number of ECSS PUS 2003 is 1.
+     * It consists of the second to fourth highest bits of the
+     * first byte.
+     * @return
+     */
+    virtual uint8_t getPusVersionNumber() const = 0;
+    /**
+     * This is a getter for the packet's Ack field, which are the lowest four
+     * bits of the first byte of the Data Field Header.
+     *
+     * It is packed in a uint8_t variable.
+     * @return	The packet's PUS Ack field.
+     */
+    virtual uint8_t getAcknowledgeFlags() const = 0;
+    /**
+     * This is a getter for the packet's PUS Service ID, which is the second
+     * byte of the Data Field Header.
+     * @return	The packet's PUS Service ID.
+     */
+    virtual uint8_t getService() const = 0;
+    /**
+     * This is a getter for the packet's PUS Service Subtype, which is the
+     * third byte of the Data Field Header.
+     * @return	The packet's PUS Service Subtype.
+     */
+    virtual uint8_t getSubService() const = 0;
+    /**
+     * The source ID can be used to have an additional identifier, e.g. for different ground
+     * station.
+     * @return
+     */
+    virtual uint16_t getSourceId() const = 0;
 
-	/**
-	 * This is a getter for a pointer to the packet's Application data.
-	 *
-	 * These are the bytes that follow after the Data Field Header. They form
-	 * the packet's application data.
-	 * @return	A pointer to the PUS Application Data.
-	 */
-	virtual const uint8_t* getApplicationData() const = 0;
-	/**
-	 * This method calculates the size of the PUS Application data field.
-	 *
-	 * It takes the information stored in the CCSDS Packet Data Length field
-	 * and subtracts the Data Field Header size and the CRC size.
-	 * @return	The size of the PUS Application Data (without Error Control
-	 * 		field)
-	 */
-	virtual uint16_t getApplicationDataSize() const = 0;
-	/**
-	 * This getter returns the Error Control Field of the packet.
-	 *
-	 * The field is placed after any possible Application Data. If no
-	 * Application Data is present there's still an Error Control field. It is
-	 * supposed to be a 16bit-CRC.
-	 * @return	The PUS Error Control
-	 */
-	virtual uint16_t getErrorControl() const = 0;
-	/**
-	 * With this method, the Error Control Field is updated to match the
-	 * current content of the packet.
-	 */
-	virtual void setErrorControl() = 0;
+    /**
+     * This is a getter for a pointer to the packet's Application data.
+     *
+     * These are the bytes that follow after the Data Field Header. They form
+     * the packet's application data.
+     * @return	A pointer to the PUS Application Data.
+     */
+    virtual const uint8_t* getApplicationData() const = 0;
+    /**
+     * This method calculates the size of the PUS Application data field.
+     *
+     * It takes the information stored in the CCSDS Packet Data Length field
+     * and subtracts the Data Field Header size and the CRC size.
+     * @return	The size of the PUS Application Data (without Error Control
+     * 		field)
+     */
+    virtual uint16_t getApplicationDataSize() const = 0;
+    /**
+     * This getter returns the Error Control Field of the packet.
+     *
+     * The field is placed after any possible Application Data. If no
+     * Application Data is present there's still an Error Control field. It is
+     * supposed to be a 16bit-CRC.
+     * @return	The PUS Error Control
+     */
+    virtual uint16_t getErrorControl() const = 0;
+    /**
+     * With this method, the Error Control Field is updated to match the
+     * current content of the packet.
+     */
+    virtual void setErrorControl() = 0;
 
-	/**
-	 * Calculate full packet length from application data length.
-	 * @param appDataLen
-	 * @return
-	 */
-	virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
+    /**
+     * Calculate full packet length from application data length.
+     * @param appDataLen
+     * @return
+     */
+    virtual size_t calculateFullPacketLength(size_t appDataLen) const = 0;
 
     /**
      * This is a debugging helper method that prints the whole packet content
diff --git a/tmtcpacket/pus/tc/TcPacketStoredBase.cpp b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
index daae35f5a..cf980e682 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.cpp
@@ -18,32 +18,32 @@ TcPacketStoredBase::~TcPacketStoredBase() {
 }
 
 ReturnValue_t TcPacketStoredBase::getData(const uint8_t ** dataPtr,
-		size_t* dataSize) {
-	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
+        size_t* dataSize) {
+    auto result = this->store->getData(storeAddress, dataPtr, dataSize);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
+        sif::warning << "TcPacketStoredBase: Could not get data!" << std::endl;
 #else
-		sif::printWarning("TcPacketStoredBase: Could not get data!\n");
+        sif::printWarning("TcPacketStoredBase: Could not get data!\n");
 #endif
-	}
-	return result;
+    }
+    return result;
 }
 
 
 
 bool TcPacketStoredBase::checkAndSetStore() {
-	if (this->store == nullptr) {
-		this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
-		if (this->store == nullptr) {
+    if (this->store == nullptr) {
+        this->store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+        if (this->store == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
-					<< std::endl;
+            sif::error << "TcPacketStoredBase::TcPacketStoredBase: TC Store not found!"
+                    << std::endl;
 #endif
-			return false;
-		}
-	}
-	return true;
+            return false;
+        }
+    }
+    return true;
 }
 
 void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
@@ -68,5 +68,5 @@ void TcPacketStoredBase::setStoreAddress(store_address_t setAddress) {
 }
 
 store_address_t TcPacketStoredBase::getStoreAddress() {
-	return this->storeAddress;
+    return this->storeAddress;
 }
diff --git a/tmtcpacket/pus/tc/TcPacketStoredBase.h b/tmtcpacket/pus/tc/TcPacketStoredBase.h
index b7d735310..1e1681f65 100644
--- a/tmtcpacket/pus/tc/TcPacketStoredBase.h
+++ b/tmtcpacket/pus/tc/TcPacketStoredBase.h
@@ -34,35 +34,35 @@ public:
      */
     TcPacketStoredBase(const uint8_t* data, uint32_t size);
 
-	virtual~ TcPacketStoredBase();
+    virtual~ TcPacketStoredBase();
 
-	/**
-	 * Getter function for the raw data.
-	 * @param dataPtr [out] Pointer to the data pointer to set
-	 * @param dataSize [out] Address of size to set.
-	 * @return -@c RETURN_OK if data was retrieved successfully.
-	 */
-	ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
+    /**
+     * Getter function for the raw data.
+     * @param dataPtr [out] Pointer to the data pointer to set
+     * @param dataSize [out] Address of size to set.
+     * @return -@c RETURN_OK if data was retrieved successfully.
+     */
+    ReturnValue_t getData(const uint8_t ** dataPtr, size_t* dataSize) override;
 
     void setStoreAddress(store_address_t setAddress) override;
     store_address_t getStoreAddress() override;
 
-	/**
-	 * With this call, the packet is deleted.
-	 * It removes itself from the store and sets its data pointer to NULL.
-	 * @return	returncode from deleting the data.
-	 */
-	virtual ReturnValue_t deletePacket() = 0;
+    /**
+     * With this call, the packet is deleted.
+     * It removes itself from the store and sets its data pointer to NULL.
+     * @return	returncode from deleting the data.
+     */
+    virtual ReturnValue_t deletePacket() = 0;
 
-	/**
-	 * This method performs a size check.
-	 * It reads the stored size and compares it with the size entered in the
-	 * packet header. This class is the optimal place for such a check as it
-	 * has access to both the header data and the store.
-	 * @return	true if size is correct, false if packet is not registered in
-	 * 			store or size is incorrect.
-	 */
-	virtual bool isSizeCorrect() = 0;
+    /**
+     * This method performs a size check.
+     * It reads the stored size and compares it with the size entered in the
+     * packet header. This class is the optimal place for such a check as it
+     * has access to both the header data and the store.
+     * @return	true if size is correct, false if packet is not registered in
+     * 			store or size is incorrect.
+     */
+    virtual bool isSizeCorrect() = 0;
 
 protected:
     /**
diff --git a/tmtcpacket/pus/tm/TmPacketBase.cpp b/tmtcpacket/pus/tm/TmPacketBase.cpp
index 91ec7d58e..3dd1749fe 100644
--- a/tmtcpacket/pus/tm/TmPacketBase.cpp
+++ b/tmtcpacket/pus/tm/TmPacketBase.cpp
@@ -11,9 +11,7 @@
 TimeStamperIF* TmPacketBase::timeStamper = nullptr;
 object_id_t TmPacketBase::timeStamperId = objects::NO_OBJECT;
 
-TmPacketBase::TmPacketBase(uint8_t* setData):
-        SpacePacketBase(setData) {
-}
+TmPacketBase::TmPacketBase(uint8_t* setData): SpacePacketBase(setData) {}
 
 TmPacketBase::~TmPacketBase() {
     //Nothing to do.
diff --git a/tmtcpacket/pus/tm/TmPacketPusA.h b/tmtcpacket/pus/tm/TmPacketPusA.h
index 486b68f30..49f26b348 100644
--- a/tmtcpacket/pus/tm/TmPacketPusA.h
+++ b/tmtcpacket/pus/tm/TmPacketPusA.h
@@ -23,7 +23,7 @@ struct PUSTmDataFieldHeaderPusA {
     uint8_t service_type;
     uint8_t service_subtype;
     uint8_t subcounter;
-//  uint8_t destination;
+    // uint8_t destination;
     uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
 };
 
@@ -51,8 +51,7 @@ public:
     static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
             sizeof(PUSTmDataFieldHeaderPusA) + 2);
     //! Maximum size of a TM Packet in this mission.
-    //! TODO: Make this dependant on a config variable.
-    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
 
     /**
      * This is the default constructor.
diff --git a/tmtcpacket/pus/tm/TmPacketPusC.h b/tmtcpacket/pus/tm/TmPacketPusC.h
index 97e5a9a5a..2a6d3bf61 100644
--- a/tmtcpacket/pus/tm/TmPacketPusC.h
+++ b/tmtcpacket/pus/tm/TmPacketPusC.h
@@ -53,8 +53,7 @@ public:
     static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
             sizeof(PUSTmDataFieldHeaderPusC) + 2);
     //! Maximum size of a TM Packet in this mission.
-    //! TODO: Make this dependant on a config variable.
-    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
 
     /**
      * This is the default constructor.

From c371cf485130178fa9ebd8ba1d7f8cfd8debd1ea Mon Sep 17 00:00:00 2001
From: "Robin.Mueller" <robin.mueller.m@gmail.com>
Date: Tue, 15 Jun 2021 14:21:46 +0200
Subject: [PATCH 11/11] changelog update

---
 CHANGELOG | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/CHANGELOG b/CHANGELOG
index add8e1a5b..09b8db6a1 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,16 @@
+## Changes from ASTP 1.0.0 to 1.1.0
+
+### PUS
+
+- Added PUS C support
+
+### Configuration
+
+- Additional configuration option fsfwconfig::FSFW_MAX_TM_PACKET_SIZE which 
+  need to be specified in FSFWConfig.h
+
+
+
 ## Changes from ASTP 0.0.1 to 1.0.0
 
 ### Host OSAL