trying to fuse header / inc

src/fsfw/tmtcpacket/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    SpacePacket.cpp
+    SpacePacketBase.cpp
+)
+
+add_subdirectory(packetmatcher)
+add_subdirectory(pus)

src/fsfw/tmtcpacket/SpacePacket.cpp

@@ -0,0 +1,27 @@
+#include "fsfw/tmtcpacket/ccsds_header.h"
+#include "fsfw/tmtcpacket/SpacePacket.h"
+#include <cstring>
+
+SpacePacket::SpacePacket(uint16_t packetDataLength, bool isTelecommand, uint16_t apid,
+        uint16_t sequenceCount):
+        SpacePacketBase( (uint8_t*)&this->localData ) {
+	initSpacePacketHeader(isTelecommand, false, apid, sequenceCount);
+	this->setPacketSequenceCount(sequenceCount);
+	if ( packetDataLength <= sizeof(this->localData.fields.buffer) ) {
+		this->setPacketDataLength(packetDataLength);
+	} else {
+		this->setPacketDataLength( sizeof(this->localData.fields.buffer) );
+	}
+}
+
+SpacePacket::~SpacePacket( void ) {
+}
+
+bool SpacePacket::addWholeData( const uint8_t* p_Data, uint32_t packet_size ) {
+	if ( packet_size <= sizeof(this->data) ) {
+		memcpy( &this->localData.byteStream, p_Data, packet_size );
+		return true;
+	} else {
+		return false;
+	}
+}

src/fsfw/tmtcpacket/SpacePacket.h

@@ -0,0 +1,70 @@
+#ifndef SPACEPACKET_H_
+#define SPACEPACKET_H_
+
+#include "SpacePacketBase.h"
+
+/**
+ * The SpacePacket class is a representation of a simple CCSDS Space Packet
+ * without (control over) a secondary header.
+ * It can be instantiated with a size smaller than \c PACKET_MAX_SIZE. Its
+ * main use is to serve as an idle packet in case no other packets are sent.
+ * For the ECSS PUS part the TcPacket and TmPacket classes are used.
+ * A pointer to \c local_data is passed to the \c SpacePacketBase parent class,
+ * so the parent's methods are reachable.
+ * @ingroup tmtcpackets
+ */
+class SpacePacket: public SpacePacketBase {
+public:
+	static const uint16_t PACKET_MAX_SIZE = 1024;
+	/**
+	 * The constructor initializes the packet and sets all header information
+	 * according to the passed parameters.
+	 * @param packetDataLength Sets the packet data length field and therefore specifies the size of the packet.
+	 * @param isTelecommand Sets the packet type field to either TC (true) or TM (false).
+	 * @param apid Sets the packet's APID field. The default value describes an idle packet.
+	 * @param sequenceCount ets the packet's Source Sequence Count field.
+	 */
+	SpacePacket(uint16_t packetDataLength, bool isTelecommand = false,
+			uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0);
+	/**
+	 * The class's default destructor.
+	 */
+	virtual ~SpacePacket();
+	/**
+	 * With this call, the complete data content (including the CCSDS Primary
+	 * Header) is overwritten with the byte stream given.
+	 * @param p_data		Pointer to data to overwrite the content with
+	 * @param packet_size	Size of the data
+	 * @return	@li	\c true if packet_size is smaller than \c MAX_PACKET_SIZE.
+	 * 			@li \c false else.
+	 */
+	bool addWholeData(const uint8_t* p_data, uint32_t packet_size);
+protected:
+	/**
+	 * This structure defines the data structure of a Space Packet as local data.
+	 * There's a buffer which corresponds to the Space Packet Data Field with a
+	 * maximum size of \c PACKET_MAX_SIZE.
+	 */
+	struct PacketStructured {
+		CCSDSPrimaryHeader header;
+		uint8_t buffer[PACKET_MAX_SIZE];
+	};
+	/**
+	 * This union simplifies accessing the full data content of the Space Packet.
+	 * This is achieved by putting the \c PacketStructured struct in a union with
+	 * a plain buffer.
+	 */
+	union SpacePacketData {
+		PacketStructured fields;
+		uint8_t byteStream[PACKET_MAX_SIZE + sizeof(CCSDSPrimaryHeader)];
+	};
+	/**
+	 * This is the data representation of the class.
+	 * It is a struct of CCSDS Primary Header and a data field, which again is
+	 * packed in an union, so the data can be accessed as a byte stream without
+	 * a cast.
+	 */
+	SpacePacketData localData;
+};
+
+#endif /* SPACEPACKET_H_ */

src/fsfw/tmtcpacket/SpacePacketBase.cpp

@@ -0,0 +1,105 @@
+#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+#include <cstring>
+
+SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) {
+	this->data = (SpacePacketPointer*) set_address;
+}
+
+SpacePacketBase::~SpacePacketBase() {
+};
+
+//CCSDS Methods:
+uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
+	return (this->data->header.packet_id_h & 0b11100000) >> 5;
+}
+
+void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
+		bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
+	//reset header to zero:
+	memset(data,0, sizeof(this->data->header) );
+	//Set TC/TM bit.
+	data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
+	//Set secondaryHeader bit
+	data->header.packet_id_h |= ((hasSecondaryHeader? 1 : 0)) << 3;
+	this->setAPID( apid );
+	//Always initialize as standalone packets.
+	data->header.sequence_control_h = 0b11000000;
+	setPacketSequenceCount(sequenceCount);
+
+}
+
+bool SpacePacketBase::isTelecommand( void ) {
+	return (this->data->header.packet_id_h & 0b00010000) >> 4;
+}
+
+bool SpacePacketBase::hasSecondaryHeader( void ) {
+	return (this->data->header.packet_id_h & 0b00001000) >> 3;
+}
+
+uint16_t SpacePacketBase::getPacketId() {
+	return ( (this->data->header.packet_id_h) << 8 ) +
+			this->data->header.packet_id_l;
+}
+
+uint16_t SpacePacketBase::getAPID( void ) const {
+	return ( (this->data->header.packet_id_h & 0b00000111) << 8 ) +
+			this->data->header.packet_id_l;
+}
+
+void SpacePacketBase::setAPID( uint16_t new_apid ) {
+	//Use first three bits of new APID, but keep rest of packet id as it was (see specification).
+	this->data->header.packet_id_h = (this->data->header.packet_id_h & 0b11111000) | ( ( new_apid & 0x0700 ) >> 8 );
+	this->data->header.packet_id_l = ( new_apid & 0x00FF );
+}
+
+uint16_t SpacePacketBase::getPacketSequenceControl( void ) {
+	return ( (this->data->header.sequence_control_h) << 8 )
+		+ this->data->header.sequence_control_l;
+}
+
+uint8_t SpacePacketBase::getSequenceFlags( void ) {
+	return (this->data->header.sequence_control_h & 0b11000000) >> 6 ;
+}
+
+uint16_t SpacePacketBase::getPacketSequenceCount( void ) const {
+	return ( (this->data->header.sequence_control_h & 0b00111111) << 8 )
+		+ this->data->header.sequence_control_l;
+}
+
+void SpacePacketBase::setPacketSequenceCount( uint16_t new_count) {
+	this->data->header.sequence_control_h = ( this->data->header.sequence_control_h & 0b11000000 ) | ( ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x3F00 ) >> 8 );
+	this->data->header.sequence_control_l = ( (new_count%LIMIT_SEQUENCE_COUNT) & 0x00FF );
+}
+
+uint16_t SpacePacketBase::getPacketDataLength() const {
+	return ( (this->data->header.packet_length_h) << 8 )
+		+ this->data->header.packet_length_l;
+}
+
+void SpacePacketBase::setPacketDataLength( uint16_t new_length) {
+	this->data->header.packet_length_h = ( ( new_length & 0xFF00 ) >> 8 );
+	this->data->header.packet_length_l = ( new_length & 0x00FF );
+}
+
+size_t SpacePacketBase::getFullSize() {
+	//+1 is done because size in packet data length field is: size of data field -1
+	return this->getPacketDataLength() + sizeof(this->data->header) + 1;
+}
+
+uint8_t* SpacePacketBase::getWholeData() {
+	return (uint8_t*)this->data;
+}
+
+void SpacePacketBase::setData( const uint8_t* p_Data ) {
+	this->data = (SpacePacketPointer*)p_Data;
+}
+
+uint32_t SpacePacketBase::getApidAndSequenceCount() const {
+	return (getAPID() << 16) + getPacketSequenceCount();
+}
+
+uint8_t* SpacePacketBase::getPacketData() {
+	return &(data->packet_data);
+}

src/fsfw/tmtcpacket/SpacePacketBase.h

@@ -0,0 +1,180 @@
+#ifndef FSFW_TMTCPACKET_SPACEPACKETBASE_H_
+#define FSFW_TMTCPACKET_SPACEPACKETBASE_H_
+
+#include "ccsds_header.h"
+#include <cstddef>
+
+/**
+ * @defgroup tmtcpackets Space Packets
+ * This is the group, where all classes associated with Telecommand and
+ * Telemetry packets belong to.
+ * The class hierarchy resembles the dependency between the different standards
+ * applied, namely the CCSDS Space Packet standard and the ECCSS Packet
+ * Utilization Standard. Most field and structure names are taken from these
+ * standards.
+ */
+
+/**
+ * This struct defines the data structure of a Space Packet when accessed
+ * via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct SpacePacketPointer {
+	CCSDSPrimaryHeader header;
+	uint8_t packet_data;
+};
+
+/**
+ * This class is the basic data handler for any CCSDS Space Packet
+ * compatible Telecommand and Telemetry packet.
+ * It does not contain the packet data itself but a pointer to the
+ * data must be set on instantiation. An invalid pointer may cause
+ * damage, as no getter method checks data validity. Anyway, a NULL
+ * check can be performed by making use of the getWholeData method.
+ * Remark: All bit numbers in this documentation are counted from
+ * the most significant bit (from left).
+ * @ingroup tmtcpackets
+ */
+class SpacePacketBase {
+protected:
+	/**
+	 * A pointer to a structure which defines the data structure of
+	 * the packet header.
+	 * To be hardware-safe, all elements are of byte size.
+	 */
+	SpacePacketPointer* data;
+public:
+	static const uint16_t LIMIT_APID = 2048; //2^1
+	static const uint16_t LIMIT_SEQUENCE_COUNT = 16384; // 2^14
+	static const uint16_t APID_IDLE_PACKET = 0x7FF;
+	static const uint8_t TELECOMMAND_PACKET = 1;
+	static const uint8_t TELEMETRY_PACKET = 0;
+	/**
+	 * This definition defines the CRC size in byte.
+	 */
+	static const uint8_t CRC_SIZE = 2;
+	/**
+	 * This is the minimum size of a SpacePacket.
+	 */
+	static const uint16_t MINIMUM_SIZE = sizeof(CCSDSPrimaryHeader) + CRC_SIZE;
+	/**
+	 * This is the default constructor.
+	 * It sets its internal data pointer to the address passed.
+	 * @param set_address	The position where the packet data lies.
+	 */
+	SpacePacketBase( const uint8_t* set_address );
+	/**
+	 * No data is allocated, so the destructor is empty.
+	 */
+	virtual ~SpacePacketBase();
+
+	//CCSDS Methods:
+	/**
+	 * Getter for the packet version number field.
+	 * @return Returns the highest three bit of the packet in one byte.
+	 */
+	uint8_t getPacketVersionNumber( void );
+	/**
+	 * This method checks the type field in the header.
+	 * This bit specifies, if the command is interpreted as Telecommand of
+	 * as Telemetry. For a Telecommand, the bit is set.
+	 * @return Returns true if the bit is set and false if not.
+	 */
+	bool isTelecommand( void );
+
+	void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
+	        uint16_t apid, uint16_t sequenceCount = 0);
+	/**
+	 * The CCSDS header provides a secondary header flag (the fifth-highest bit),
+	 *  which is checked with this method.
+	 * @return	Returns true if the bit is set and false if not.
+	 */
+	bool hasSecondaryHeader( void );
+	/**
+	 * Returns the complete first two bytes of the packet, which together form
+	 * the CCSDS packet id.
+	 * @return	The CCSDS packet id.
+	 */
+	uint16_t getPacketId( void );
+	/**
+	 * Returns the APID of a packet, which are the lowest 11 bit of the packet
+	 * id.
+	 * @return The CCSDS APID.
+	 */
+	uint16_t getAPID( void ) const;
+	/**
+	 * Sets the APID of a packet, which are the lowest 11 bit of the packet
+	 * id.
+	 * @param 	The APID to set. The highest five bits of the parameter are
+	 * 			ignored.
+	 */
+	void setAPID( uint16_t setAPID );
+	/**
+	 * Returns the CCSDS packet sequence control field, which are the third and
+	 * the fourth byte of the CCSDS primary header.
+	 * @return The CCSDS packet sequence control field.
+	 */
+	uint16_t getPacketSequenceControl( void );
+	/**
+	 * Returns the SequenceFlags, which are the highest two bit of the packet
+	 * sequence control field.
+	 * @return	The CCSDS sequence flags.
+	 */
+	uint8_t getSequenceFlags( void );
+	/**
+	 * Returns the packet sequence count, which are the lowest 14 bit of the
+	 * packet sequence control field.
+	 * @return The CCSDS sequence count.
+	 */
+	uint16_t getPacketSequenceCount( void ) const;
+	/**
+	 * Sets the packet sequence count, which are the lowest 14 bit of the
+	 * packet sequence control field.
+	 * setCount is modulo-divided by \c LIMIT_SEQUENCE_COUNT to avoid overflows.
+	 * @param setCount	The value to set the count to.
+	 */
+	void setPacketSequenceCount( uint16_t setCount );
+	/**
+	 * 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. 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.
+	 * @param setLength The value of the length to set. It must fit the true
+	 * 					CCSDS packet data length . The packet data length is
+	 * 					the size of every kind of data \b after the CCSDS
+	 * 					Primary Header \b -1.
+	 */
+	void setPacketDataLength( uint16_t setLength );
+
+	//Helper methods:
+	/**
+	 * This method returns a raw uint8_t pointer to the packet.
+	 * @return	A \c uint8_t pointer to the first byte of the CCSDS primary header.
+	 */
+	virtual uint8_t* getWholeData( void );
+
+	uint8_t* getPacketData();
+	/**
+	 * With this method, the packet data pointer can be redirected to another
+	 * location.
+	 * @param p_Data A pointer to another raw Space Packet.
+	 */
+	virtual void setData( const uint8_t* p_Data );
+	/**
+	 * This method returns the full raw packet size.
+	 * @return	The full size of the packet in bytes.
+	 */
+	size_t getFullSize();
+
+	uint32_t getApidAndSequenceCount() const;
+
+};
+
+#endif /* FSFW_TMTCPACKET_SPACEPACKETBASE_H_ */

src/fsfw/tmtcpacket/ccsds_header.h

@@ -0,0 +1,15 @@
+#ifndef CCSDS_HEADER_H_
+#define CCSDS_HEADER_H_
+
+#include <stdint.h>
+
+struct CCSDSPrimaryHeader {
+	uint8_t packet_id_h;
+	uint8_t packet_id_l;
+	uint8_t sequence_control_h;
+	uint8_t sequence_control_l;
+	uint8_t packet_length_h;
+	uint8_t packet_length_l;
+};
+
+#endif /* CCSDS_HEADER_H_ */

src/fsfw/tmtcpacket/packetmatcher/ApidMatcher.h

@@ -0,0 +1,40 @@
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_
+
+#include "../../globalfunctions/matching/SerializeableMatcherIF.h"
+#include "../../serialize/SerializeAdapter.h"
+#include "../../tmtcpacket/pus/tm/TmPacketMinimal.h"
+
+class ApidMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
+private:
+    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);
+    }
+};
+
+
+
+#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_APIDMATCHER_H_ */

src/fsfw/tmtcpacket/packetmatcher/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		PacketMatchTree.cpp
+)

src/fsfw/tmtcpacket/packetmatcher/PacketMatchTree.cpp

@@ -0,0 +1,198 @@
+#include "fsfw/tmtcpacket/packetmatcher/ApidMatcher.h"
+#include "fsfw/tmtcpacket/packetmatcher/PacketMatchTree.h"
+#include "fsfw/tmtcpacket/packetmatcher/ServiceMatcher.h"
+#include "fsfw/tmtcpacket/packetmatcher/SubserviceMatcher.h"
+
+// This should be configurable..
+const LocalPool::LocalPoolConfig PacketMatchTree::poolConfig = {
+        {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(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() {
+}
+
+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;
+}
+
+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;
+}
+
+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);
+}
+
+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;
+}
+
+ReturnValue_t PacketMatchTree::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);
+    }
+}
+
+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);
+}

src/fsfw/tmtcpacket/packetmatcher/PacketMatchTree.h

@@ -0,0 +1,37 @@
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_
+
+#include "fsfw/container/PlacementFactory.h"
+#include "fsfw/globalfunctions/matching/MatchTree.h"
+#include "fsfw/storagemanager/LocalPool.h"
+#include "fsfw/tmtcpacket/pus/tm/TmPacketMinimal.h"
+
+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();
+protected:
+    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);
+};
+
+#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_PACKETMATCHTREE_H_ */
+

src/fsfw/tmtcpacket/packetmatcher/ServiceMatcher.h

@@ -0,0 +1,39 @@
+#ifndef FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_
+#define FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_
+
+#include "../../globalfunctions/matching/SerializeableMatcherIF.h"
+#include "../../serialize/SerializeAdapter.h"
+#include "../pus/tm/TmPacketMinimal.h"
+
+class ServiceMatcher: public SerializeableMatcherIF<TmPacketMinimal*> {
+private:
+    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);
+    }
+};
+
+
+#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_SERVICEMATCHER_H_ */

src/fsfw/tmtcpacket/packetmatcher/SubserviceMatcher.h

@@ -0,0 +1,40 @@
+#ifndef FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+#define FSFW_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_
+
+#include "../../globalfunctions/matching/SerializeableMatcherIF.h"
+#include "../../serialize/SerializeAdapter.h"
+#include "../pus/tm/TmPacketMinimal.h"
+
+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);
+    }
+private:
+    uint8_t subService;
+};
+
+
+
+#endif /* FRAMEWORK_TMTCPACKET_PACKETMATCHER_SUBSERVICEMATCHER_H_ */

src/fsfw/tmtcpacket/pus/CMakeLists.txt

@@ -0,0 +1,2 @@
+add_subdirectory(tm)
+add_subdirectory(tc)

src/fsfw/tmtcpacket/pus/PacketTimestampInterpreterIF.h

@@ -0,0 +1,19 @@
+#ifndef FSFW_TMTCPACKET_PUS_PACKETTIMESTAMPINTERPRETERIF_H_
+#define FSFW_TMTCPACKET_PUS_PACKETTIMESTAMPINTERPRETERIF_H_
+
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+
+class TmPacketMinimal;
+
+class PacketTimestampInterpreterIF {
+public:
+	virtual ~PacketTimestampInterpreterIF() {}
+	virtual ReturnValue_t getPacketTime(TmPacketMinimal* packet,
+	        timeval* timestamp) const = 0;
+	virtual ReturnValue_t getPacketTimeRaw(TmPacketMinimal* packet, const uint8_t** timePtr,
+	        uint32_t* size) const = 0;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_PACKETTIMESTAMPINTERPRETERIF_H_ */

src/fsfw/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_ */

src/fsfw/tmtcpacket/pus/tc/CMakeLists.txt

@@ -0,0 +1,6 @@
+target_sources(${LIB_FSFW_NAME} PRIVATE    
+    TcPacketBase.cpp
+    TcPacketPus.cpp
+    TcPacketStoredBase.cpp
+    TcPacketStoredPus.cpp
+)

src/fsfw/tmtcpacket/pus/tc/TcPacketBase.cpp

@@ -0,0 +1,20 @@
+#include "fsfw/tmtcpacket/pus/tc/TcPacketBase.h"
+
+#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+#include <cstring>
+
+TcPacketBase::TcPacketBase(const uint8_t* setData): SpacePacketBase(setData) {}
+
+TcPacketBase::~TcPacketBase() {}
+
+void TcPacketBase::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::info << "TcPacketBase::print:" << std::endl;
+#else
+    sif::printInfo("TcPacketBase::print:\n");
+#endif
+    arrayprinter::print(getWholeData(), getFullSize());
+}

src/fsfw/tmtcpacket/pus/tc/TcPacketBase.h

@@ -0,0 +1,150 @@
+#ifndef TMTCPACKET_PUS_TCPACKETBASE_H_
+#define TMTCPACKET_PUS_TCPACKETBASE_H_
+
+#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include <cstddef>
+
+/**
+ * This class is the basic data handler for any ECSS PUS Telecommand packet.
+ *
+ * In addition to #SpacePacketBase, the class provides methods to handle
+ * the standardized entries of the PUS TC Packet Data Field Header.
+ * It does not contain the packet data itself but a pointer to the
+ * data must be set on instantiation. An invalid pointer may cause
+ * damage, as no getter method checks data validity. Anyway, a NULL
+ * check can be performed by making use of the getWholeData method.
+ * @ingroup tmtcpackets
+ */
+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
+    };
+
+    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 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;
+
+    /**
+     * 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
+     * to the screen.
+     */
+    void print();
+protected:
+
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     * This call overwrites the parent's setData method to set both its
+     * @c tc_data pointer and the parent's @c data pointer.
+     *
+     * @param p_data    A pointer to another PUS Telecommand Packet.
+     */
+    void setData( const uint8_t* pData ) = 0;
+};
+
+
+#endif /* TMTCPACKET_PUS_TCPACKETBASE_H_ */

src/fsfw/tmtcpacket/pus/tc/TcPacketPus.cpp

@@ -0,0 +1,98 @@
+#include "fsfw/tmtcpacket/pus/tc/TcPacketPus.h"
+#include "fsfw/globalfunctions/CRC.h"
+
+#include <cstring>
+
+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, uint16_t sourceId) {
+    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;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    tcData->dataField.sourceIdH = (sourceId >> 8) | 0xff;
+    tcData->dataField.sourceIdL = sourceId & 0xff;
+#else
+    tcData->dataField.sourceId = sourceId;
+#endif
+}
+
+uint8_t TcPacketPus::getService() const {
+    return tcData->dataField.serviceType;
+}
+
+uint8_t TcPacketPus::getSubService() const {
+    return tcData->dataField.serviceSubtype;
+}
+
+uint8_t TcPacketPus::getAcknowledgeFlags() const {
+    return tcData->dataField.versionTypeAck & 0b00001111;
+}
+
+const uint8_t* TcPacketPus::getApplicationData() const {
+    return &tcData->appData;
+}
+
+uint16_t TcPacketPus::getApplicationDataSize() const {
+    return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1;
+}
+
+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];
+}
+
+void TcPacketPus::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 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 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 {
+#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) const {
+    return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
+            appDataLen + TcPacketBase::CRC_SIZE;
+}

src/fsfw/tmtcpacket/pus/tc/TcPacketPus.h

@@ -0,0 +1,90 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_
+
+#include "../../../FSFW.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 versionTypeAck;
+    uint8_t serviceType;
+    uint8_t serviceSubtype;
+#if FSFW_USE_PUS_C_TELECOMMANDS == 1
+    uint8_t sourceIdH;
+    uint8_t sourceIdL;
+#else
+    uint8_t sourceId;
+#endif
+};
+
+/**
+ * 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 TcPacketPus: public TcPacketBase {
+public:
+    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
+     */
+    TcPacketPus(const uint8_t* setData);
+
+    // Base class overrides
+    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() const override;
+    uint16_t getErrorControl() const override;
+    void setErrorControl() override;
+    size_t calculateFullPacketLength(size_t appDataLen) const 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, uint16_t sourceId = 0);
+
+    /**
+     * 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;
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETPUSA_H_ */

src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.cpp

@@ -0,0 +1,72 @@
+#include "fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h"
+
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/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;
+}

src/fsfw/tmtcpacket/pus/tc/TcPacketStoredBase.h

@@ -0,0 +1,90 @@
+#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_
+#define TMTCPACKET_PUS_TCPACKETSTORED_H_
+
+#include "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_ */

src/fsfw/tmtcpacket/pus/tc/TcPacketStoredIF.h

@@ -0,0 +1,38 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDIF_H_
+
+#include "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_ */

src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.cpp

@@ -0,0 +1,79 @@
+#include "fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.h"
+
+#include "fsfw/serviceinterface/ServiceInterface.h"
+
+#include <cstring>
+
+TcPacketStoredPus::TcPacketStoredPus(uint16_t apid,  uint8_t service,
+        uint8_t subservice, uint8_t sequenceCount, const uint8_t* data,
+        size_t size, uint8_t ack) :
+        TcPacketPus(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();
+}
+
+TcPacketStoredPus::TcPacketStoredPus(): TcPacketStoredBase(), TcPacketPus(nullptr) {
+}
+
+TcPacketStoredPus::TcPacketStoredPus(store_address_t setAddress): TcPacketPus(nullptr) {
+    TcPacketStoredBase::setStoreAddress(setAddress);
+}
+
+TcPacketStoredPus::TcPacketStoredPus(const uint8_t* data, size_t size): TcPacketPus(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 TcPacketStoredPus::deletePacket() {
+    ReturnValue_t result = this->store->deleteData(this->storeAddress);
+    this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    this->setData(nullptr);
+    return result;
+}
+
+TcPacketBase* TcPacketStoredPus::getPacketBase() {
+    return this;
+}
+
+
+bool TcPacketStoredPus::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;
+}

src/fsfw/tmtcpacket/pus/tc/TcPacketStoredPus.h

@@ -0,0 +1,53 @@
+#ifndef FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_
+
+#include "TcPacketStoredBase.h"
+#include "TcPacketPus.h"
+
+class TcPacketStoredPus:
+        public TcPacketStoredBase,
+        public TcPacketPus {
+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.
+     */
+    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);
+    /**
+     * Create stored packet with existing data.
+     * @param data
+     * @param size
+     */
+    TcPacketStoredPus(const uint8_t* data, size_t size);
+    /**
+     * Create stored packet from existing packet in store
+     * @param setAddress
+     */
+    TcPacketStoredPus(store_address_t setAddress);
+    TcPacketStoredPus();
+
+    ReturnValue_t deletePacket() override;
+    TcPacketBase* getPacketBase() override;
+
+private:
+
+    bool isSizeCorrect() override;
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TCPACKETSTOREDPUSA_H_ */

src/fsfw/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_ */

src/fsfw/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
+)

src/fsfw/tmtcpacket/pus/tm/TmPacketBase.cpp

@@ -0,0 +1,67 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketBase.h"
+
+#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/timemanager/CCSDSTime.h"
+
+#include <cstring>
+
+TimeStamperIF* TmPacketBase::timeStamper = nullptr;
+object_id_t TmPacketBase::timeStamperId = objects::NO_OBJECT;
+
+TmPacketBase::TmPacketBase(uint8_t* setData): SpacePacketBase(setData) {}
+
+TmPacketBase::~TmPacketBase() {
+    //Nothing to do.
+}
+
+
+uint16_t TmPacketBase::getSourceDataSize() {
+    return getPacketDataLength() - getDataFieldSize() - CRC_SIZE + 1;
+}
+
+uint16_t TmPacketBase::getErrorControl() {
+    uint32_t size = getSourceDataSize() + CRC_SIZE;
+    uint8_t* p_to_buffer = getSourceData();
+    return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+}
+
+void TmPacketBase::setErrorControl() {
+    uint32_t full_size = getFullSize();
+    uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE);
+    uint32_t size = getSourceDataSize();
+    getSourceData()[size] = (crc & 0XFF00) >> 8;	// CRCH
+    getSourceData()[size + 1] = (crc) & 0X00FF; 		// CRCL
+}
+
+ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
+    size_t tempSize = 0;
+    return CCSDSTime::convertFromCcsds(timestamp, getPacketTimeRaw(),
+            &tempSize, getTimestampSize());
+}
+
+bool TmPacketBase::checkAndSetStamper() {
+    if (timeStamper == NULL) {
+        timeStamper = ObjectManager::instance()->get<TimeStamperIF>(timeStamperId);
+        if (timeStamper == NULL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl;
+#else
+            sif::printWarning("TmPacketBase::checkAndSetStamper: Stamper not found!\n");
+#endif
+            return false;
+        }
+    }
+    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());
+}

src/fsfw/tmtcpacket/pus/tm/TmPacketBase.h

@@ -0,0 +1,139 @@
+#ifndef TMTCPACKET_PUS_TMPACKETBASE_H_
+#define TMTCPACKET_PUS_TMPACKETBASE_H_
+
+#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/timemanager/TimeStamperIF.h"
+#include "fsfw/timemanager/Clock.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+
+namespace Factory {
+
+void setStaticFrameworkObjectIds();
+
+}
+
+
+/**
+ * This class is the basic data handler for any ECSS PUS Telemetry packet.
+ *
+ * In addition to #SpacePacketBase, the class provides methods to handle
+ * the standardized entries of the PUS TM Packet Data Field Header.
+ * It does not contain the packet data itself but a pointer to the
+ * data must be set on instantiation. An invalid pointer may cause
+ * damage, as no getter method checks data validity. Anyway, a NULL
+ * check can be performed by making use of the getWholeData method.
+ * @ingroup tmtcpackets
+ */
+class TmPacketBase : public SpacePacketBase {
+    friend void (Factory::setStaticFrameworkObjectIds)();
+public:
+
+    //! 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;
+    //! First four bits of first byte of secondary header
+    static const uint8_t VERSION_NUMBER_BYTE = 0b00010000;
+
+    /**
+     * 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 set_address	The position where the packet data lies.
+     */
+    TmPacketBase( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketBase();
+
+    /**
+     * 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() = 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() = 0;
+    /**
+     * This is a getter for a pointer to the packet's Source data.
+     *
+     * These are the bytes that follow after the Data Field Header. They form
+     * the packet's source data.
+     * @return	A pointer to the PUS Source Data.
+     */
+    virtual uint8_t* getSourceData() = 0;
+    /**
+     * This method calculates the size of the PUS Source 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 Source Data (without Error Control field)
+     */
+    virtual uint16_t getSourceDataSize() = 0;
+
+    /**
+     * Get size of data field which can differ based on implementation
+     * @return
+     */
+    virtual uint16_t getDataFieldSize() = 0;
+
+    virtual size_t getPacketMinimumSize() const = 0;
+
+    /**
+     * Interprets the "time"-field in the secondary header and returns it in
+     * timeval format.
+     * @return Converted timestamp of packet.
+     */
+    virtual ReturnValue_t getPacketTime(timeval* timestamp) const;
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    virtual uint8_t* getPacketTimeRaw() const = 0;
+
+    virtual size_t getTimestampSize() const = 0;
+
+    /**
+     * This is a debugging helper method that prints the whole packet content
+     * to the screen.
+     */
+    void print();
+    /**
+     * With this method, the Error Control Field is updated to match the
+     * current content of the packet. This method is not protected because
+     * a recalculation by the user might be necessary when manipulating fields
+     * like the sequence count.
+     */
+    void setErrorControl();
+    /**
+     * This getter returns the Error Control Field of the packet.
+     *
+     * The field is placed after any possible Source Data. If no
+     * Source Data is present there's still an Error Control field. It is
+     * supposed to be a 16bit-CRC.
+     * @return  The PUS Error Control
+     */
+    uint16_t getErrorControl();
+
+protected:
+    /**
+     * The timeStamper is responsible for adding a timestamp to the packet.
+     * It is initialized lazy.
+     */
+    static TimeStamperIF* timeStamper;
+    //! The ID to use when looking for a time stamper.
+    static object_id_t timeStamperId;
+
+    /**
+     * Checks if a time stamper is available and tries to set it if not.
+     * @return Returns false if setting failed.
+     */
+    bool checkAndSetStamper();
+};
+
+
+#endif /* TMTCPACKET_PUS_TMPACKETBASE_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketMinimal.cpp

@@ -0,0 +1,46 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketMinimal.h"
+#include "fsfw/tmtcpacket/pus/PacketTimestampInterpreterIF.h"
+
+#include <cstddef>
+#include <ctime>
+
+TmPacketMinimal::TmPacketMinimal(const uint8_t* set_data) : SpacePacketBase( set_data ) {
+	this->tm_data = (TmPacketMinimalPointer*)set_data;
+}
+
+TmPacketMinimal::~TmPacketMinimal() {
+}
+
+uint8_t TmPacketMinimal::getService() {
+	return tm_data->data_field.service_type;
+}
+
+uint8_t TmPacketMinimal::getSubService() {
+	return tm_data->data_field.service_subtype;
+}
+
+uint8_t TmPacketMinimal::getPacketSubcounter() {
+	return tm_data->data_field.subcounter;
+}
+
+ReturnValue_t TmPacketMinimal::getPacketTime(timeval* timestamp) {
+	if (timestampInterpreter == NULL) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return timestampInterpreter->getPacketTime(this, timestamp);
+}
+
+ReturnValue_t TmPacketMinimal::getPacketTimeRaw(const uint8_t** timePtr, uint32_t* size) {
+	if (timestampInterpreter == NULL) {
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	return timestampInterpreter->getPacketTimeRaw(this, timePtr, size);
+}
+
+void TmPacketMinimal::setInterpretTimestampObject(PacketTimestampInterpreterIF* interpreter) {
+	if (TmPacketMinimal::timestampInterpreter == NULL) {
+		TmPacketMinimal::timestampInterpreter = interpreter;
+	}
+}
+
+PacketTimestampInterpreterIF* TmPacketMinimal::timestampInterpreter = NULL;

src/fsfw/tmtcpacket/pus/tm/TmPacketMinimal.h

@@ -0,0 +1,84 @@
+#ifndef FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_
+#define FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_
+
+
+#include "../../SpacePacketBase.h"
+#include "../../../returnvalues/HasReturnvaluesIF.h"
+
+struct timeval;
+class PacketTimestampInterpreterIF;
+/**
+ * This is a minimal version of a PUS TmPacket without any variable field, or,
+ * in other words with Service Type, Subtype and subcounter only.
+ * This is required for handling TM packets with different APIDs with different
+ * secondary headers.
+ */
+class TmPacketMinimal : public SpacePacketBase {
+public:
+	/**
+	 * 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 set_address	The position where the packet data lies.
+	 */
+	TmPacketMinimal( const uint8_t* set_data );
+	/**
+	 * This is the empty default destructor.
+	 */
+	virtual ~TmPacketMinimal();
+	/**
+	 * 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();
+	/**
+	 * 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();
+	/**
+	 * Returns the subcounter.
+	 * @return the subcounter of the Data Field Header.
+	 */
+	uint8_t getPacketSubcounter();
+	struct PUSTmMinimalHeader {
+		uint8_t version_type_ack;
+		uint8_t service_type;
+		uint8_t service_subtype;
+		uint8_t subcounter;
+	};
+
+	ReturnValue_t getPacketTime(timeval* timestamp);
+
+	ReturnValue_t getPacketTimeRaw(const uint8_t** timePtr, uint32_t* size);
+
+	static void setInterpretTimestampObject(PacketTimestampInterpreterIF* interpreter);
+	/**
+	 * This struct defines the data structure of a PUS Telecommand Packet when
+	 * accessed via a pointer.
+	 * @ingroup tmtcpackets
+	 */
+	struct TmPacketMinimalPointer {
+		CCSDSPrimaryHeader primary;
+		PUSTmMinimalHeader data_field;
+		uint8_t rest;
+	};
+	//Must include a checksum and is therefore at least one larger than the above struct.
+	static const uint16_t MINIMUM_SIZE = sizeof(TmPacketMinimalPointer) +1;
+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.
+	 */
+	TmPacketMinimalPointer* tm_data;
+
+	static PacketTimestampInterpreterIF* timestampInterpreter;
+};
+
+
+
+#endif /* FRAMEWORK_TMTCPACKET_PUS_TMPACKETMINIMAL_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketPusA.cpp

@@ -0,0 +1,87 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketPusA.h"
+#include "fsfw/tmtcpacket/pus/tm/TmPacketBase.h"
+
+#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/timemanager/CCSDSTime.h"
+
+#include <cstring>
+
+
+TmPacketPusA::TmPacketPusA(uint8_t* setData) : TmPacketBase(setData) {
+    tmData = reinterpret_cast<TmPacketPointerPusA*>(setData);
+}
+
+TmPacketPusA::~TmPacketPusA() {
+    //Nothing to do.
+}
+
+uint8_t TmPacketPusA::getService() {
+    return tmData->data_field.service_type;
+}
+
+uint8_t TmPacketPusA::getSubService() {
+    return tmData->data_field.service_subtype;
+}
+
+uint8_t* TmPacketPusA::getSourceData() {
+    return &tmData->data;
+}
+
+uint16_t TmPacketPusA::getSourceDataSize() {
+    return getPacketDataLength() - sizeof(tmData->data_field)
+            - CRC_SIZE + 1;
+}
+
+void TmPacketPusA::setData(const uint8_t* p_Data) {
+    SpacePacketBase::setData(p_Data);
+    tmData = (TmPacketPointerPusA*) p_Data;
+}
+
+
+size_t TmPacketPusA::getPacketMinimumSize() const {
+    return TM_PACKET_MIN_SIZE;
+}
+
+uint16_t TmPacketPusA::getDataFieldSize() {
+    return sizeof(PUSTmDataFieldHeaderPusA);
+}
+
+uint8_t* TmPacketPusA::getPacketTimeRaw() const {
+    return tmData->data_field.time;
+
+}
+
+void TmPacketPusA::initializeTmPacket(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint8_t packetSubcounter) {
+    //Set primary header:
+    initSpacePacketHeader(false, true, apid);
+    //Set data Field Header:
+    //First, set to zero.
+    memset(&tmData->data_field, 0, sizeof(tmData->data_field));
+
+    // NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits.
+    // The other 4 bits of the first byte are the spacecraft time reference
+    // status. To change to PUS-C, set 0b00100000.
+    // Set CCSDS_secondary header flag to 0, version number to 001 and ack
+    // to 0000
+    tmData->data_field.version_type_ack = 0b00010000;
+    tmData->data_field.service_type = service;
+    tmData->data_field.service_subtype = subservice;
+    tmData->data_field.subcounter = packetSubcounter;
+    //Timestamp packet
+    if (TmPacketBase::checkAndSetStamper()) {
+        timeStamper->addTimeStamp(tmData->data_field.time,
+                sizeof(tmData->data_field.time));
+    }
+}
+
+void TmPacketPusA::setSourceDataSize(uint16_t size) {
+    setPacketDataLength(size + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
+}
+
+size_t TmPacketPusA::getTimestampSize() const {
+    return sizeof(tmData->data_field.time);
+}

src/fsfw/tmtcpacket/pus/tm/TmPacketPusA.h

@@ -0,0 +1,128 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+
+#include "TmPacketBase.h"
+#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/timemanager/TimeStamperIF.h"
+#include "fsfw/timemanager/Clock.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+
+namespace Factory{
+void setStaticFrameworkObjectIds();
+}
+
+/**
+ * This struct defines a byte-wise structured PUS TM Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, no Destination field is present, but an eigth-byte representation
+ * for a time tag.
+ * @ingroup tmtcpackets
+ */
+struct PUSTmDataFieldHeaderPusA {
+    uint8_t version_type_ack;
+    uint8_t service_type;
+    uint8_t service_subtype;
+    uint8_t subcounter;
+    // uint8_t destination;
+    uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+};
+
+/**
+ * This struct defines the data structure of a PUS Telecommand Packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TmPacketPointerPusA {
+    CCSDSPrimaryHeader primary;
+    PUSTmDataFieldHeaderPusA data_field;
+    uint8_t data;
+};
+
+/**
+ * PUS A packet implementation
+ * @ingroup tmtcpackets
+ */
+class TmPacketPusA: public TmPacketBase {
+    friend void (Factory::setStaticFrameworkObjectIds)();
+public:
+    /**
+     * This constant defines the minimum size of a valid PUS Telemetry Packet.
+     */
+    static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTmDataFieldHeaderPusA) + 2);
+    //! Maximum size of a TM Packet in this mission.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
+
+    /**
+     * 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 set_address   The position where the packet data lies.
+     */
+    TmPacketPusA( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketPusA();
+
+    /* TmPacketBase implementations */
+    uint8_t getService() override;
+    uint8_t getSubService() override;
+    uint8_t* getSourceData() override;
+    uint16_t getSourceDataSize() override;
+    uint16_t getDataFieldSize() override;
+
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    uint8_t* getPacketTimeRaw() const override;
+    size_t getTimestampSize() const override;
+
+    size_t getPacketMinimumSize() const override;
+
+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.
+     */
+    TmPacketPointerPusA* tmData;
+
+    /**
+     * Initializes the Tm Packet header.
+     * Does set the timestamp (to now), but not the error control field.
+     * @param apid APID used.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     * @param packetSubcounter Additional subcounter used.
+     */
+    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint8_t packetSubcounter);
+
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     *
+     * This call overwrites the parent's setData method to set both its
+     * @c tc_data pointer and the parent's @c data pointer.
+     *
+     * @param p_data    A pointer to another PUS Telemetry Packet.
+     */
+    void setData( const uint8_t* pData );
+
+    /**
+     * In case data was filled manually (almost never the case).
+     * @param size Size of source data (without CRC and data filed header!).
+     */
+    void setSourceDataSize(uint16_t size);
+
+    /**
+     * Checks if a time stamper is available and tries to set it if not.
+     * @return Returns false if setting failed.
+     */
+    bool checkAndSetStamper();
+};
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.cpp

@@ -0,0 +1,87 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketPusC.h"
+#include "fsfw/tmtcpacket/pus/tm/TmPacketBase.h"
+
+#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/globalfunctions/arrayprinter.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/timemanager/CCSDSTime.h"
+
+#include <cstring>
+
+
+TmPacketPusC::TmPacketPusC(uint8_t* setData) : TmPacketBase(setData) {
+    tmData = reinterpret_cast<TmPacketPointerPusC*>(setData);
+}
+
+TmPacketPusC::~TmPacketPusC() {
+    //Nothing to do.
+}
+
+uint8_t TmPacketPusC::getService() {
+    return tmData->dataField.serviceType;
+}
+
+uint8_t TmPacketPusC::getSubService() {
+    return tmData->dataField.serviceSubtype;
+}
+
+uint8_t* TmPacketPusC::getSourceData() {
+    return &tmData->data;
+}
+
+uint16_t TmPacketPusC::getSourceDataSize() {
+    return getPacketDataLength() - sizeof(tmData->dataField) - CRC_SIZE + 1;
+}
+
+void TmPacketPusC::setData(const uint8_t* p_Data) {
+    SpacePacketBase::setData(p_Data);
+    tmData = (TmPacketPointerPusC*) p_Data;
+}
+
+
+size_t TmPacketPusC::getPacketMinimumSize() const {
+    return TM_PACKET_MIN_SIZE;
+}
+
+uint16_t TmPacketPusC::getDataFieldSize() {
+    return sizeof(PUSTmDataFieldHeaderPusC);
+}
+
+uint8_t* TmPacketPusC::getPacketTimeRaw() const{
+    return tmData->dataField.time;
+
+}
+
+void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId,
+        uint8_t timeRefField) {
+    //Set primary header:
+    initSpacePacketHeader(false, true, apid);
+    //Set data Field Header:
+    //First, set to zero.
+    memset(&tmData->dataField, 0, sizeof(tmData->dataField));
+
+    /* Only account for last 4 bytes for time reference field */
+    timeRefField &= 0b1111;
+    tmData->dataField.versionTimeReferenceField = VERSION_NUMBER_BYTE | timeRefField;
+    tmData->dataField.serviceType = service;
+    tmData->dataField.serviceSubtype = subservice;
+    tmData->dataField.subcounterMsb = packetSubcounter << 8 & 0xff;
+    tmData->dataField.subcounterLsb = packetSubcounter & 0xff;
+    tmData->dataField.destinationIdMsb = destinationId << 8 & 0xff;
+    tmData->dataField.destinationIdLsb = destinationId & 0xff;
+    //Timestamp packet
+    if (TmPacketBase::checkAndSetStamper()) {
+        timeStamper->addTimeStamp(tmData->dataField.time,
+                sizeof(tmData->dataField.time));
+    }
+}
+
+void TmPacketPusC::setSourceDataSize(uint16_t size) {
+    setPacketDataLength(size + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+size_t TmPacketPusC::getTimestampSize() const {
+    return sizeof(tmData->dataField.time);
+}

src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h

@@ -0,0 +1,125 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+
+#include "TmPacketBase.h"
+#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/timemanager/TimeStamperIF.h"
+#include "fsfw/timemanager/Clock.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+
+namespace Factory{
+void setStaticFrameworkObjectIds();
+}
+
+/**
+ * This struct defines a byte-wise structured PUS TM Data Field Header.
+ * Any optional fields in the header must be added or removed here.
+ * Currently, no Destination field is present, but an eigth-byte representation
+ * for a time tag.
+ * @ingroup tmtcpackets
+ */
+struct PUSTmDataFieldHeaderPusC {
+    uint8_t versionTimeReferenceField;
+    uint8_t serviceType;
+    uint8_t serviceSubtype;
+    uint8_t subcounterMsb;
+    uint8_t subcounterLsb;
+    uint8_t destinationIdMsb;
+    uint8_t destinationIdLsb;
+    uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+};
+
+/**
+ * This struct defines the data structure of a PUS Telecommand Packet when
+ * accessed via a pointer.
+ * @ingroup tmtcpackets
+ */
+struct TmPacketPointerPusC {
+    CCSDSPrimaryHeader primary;
+    PUSTmDataFieldHeaderPusC dataField;
+    uint8_t data;
+};
+
+/**
+ * PUS A packet implementation
+ * @ingroup tmtcpackets
+ */
+class TmPacketPusC: public TmPacketBase {
+    friend void (Factory::setStaticFrameworkObjectIds)();
+public:
+    /**
+     * This constant defines the minimum size of a valid PUS Telemetry Packet.
+     */
+    static const uint32_t  TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) +
+            sizeof(PUSTmDataFieldHeaderPusC) + 2);
+    //! Maximum size of a TM Packet in this mission.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = fsfwconfig::FSFW_MAX_TM_PACKET_SIZE;
+
+    /**
+     * 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 set_address   The position where the packet data lies.
+     */
+    TmPacketPusC( uint8_t* setData );
+    /**
+     * This is the empty default destructor.
+     */
+    virtual ~TmPacketPusC();
+
+    /* TmPacketBase implementations */
+    uint8_t getService() override;
+    uint8_t getSubService() override;
+    uint8_t* getSourceData() override;
+    uint16_t getSourceDataSize() override;
+    uint16_t getDataFieldSize() override;
+
+    /**
+     * Returns a raw pointer to the beginning of the time field.
+     * @return Raw pointer to time field.
+     */
+    uint8_t* getPacketTimeRaw() const override;
+    size_t getTimestampSize() const override;
+
+    size_t getPacketMinimumSize() const override;
+
+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.
+     */
+    TmPacketPointerPusC* tmData;
+
+    /**
+     * Initializes the Tm Packet header.
+     * Does set the timestamp (to now), but not the error control field.
+     * @param apid APID used.
+     * @param service   PUS Service
+     * @param subservice PUS Subservice
+     * @param packetSubcounter Additional subcounter used.
+     */
+    void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0);
+
+    /**
+     * With this method, the packet data pointer can be redirected to another
+     * location.
+     *
+     * This call overwrites the parent's setData method to set both its
+     * @c tc_data pointer and the parent's @c data pointer.
+     *
+     * @param p_data    A pointer to another PUS Telemetry Packet.
+     */
+    void setData( const uint8_t* pData );
+
+    /**
+     * In case data was filled manually (almost never the case).
+     * @param size Size of source data (without CRC and data filed header!).
+     */
+    void setSourceDataSize(uint16_t size);
+
+};
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketStored.h

@@ -0,0 +1,13 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
+
+#include <FSFWConfig.h>
+
+#if FSFW_USE_PUS_C_TELEMETRY == 1
+#include "TmPacketStoredPusC.h"
+#else
+#include "TmPacketStoredPusA.h"
+#endif
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredBase.cpp

@@ -0,0 +1,94 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketStoredBase.h"
+
+#include "fsfw/objectmanager/ObjectManager.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
+
+#include <cstring>
+
+StorageManagerIF *TmPacketStoredBase::store = nullptr;
+InternalErrorReporterIF *TmPacketStoredBase::internalErrorReporter = nullptr;
+
+TmPacketStoredBase::TmPacketStoredBase(store_address_t setAddress): storeAddress(setAddress) {
+    setStoreAddress(storeAddress);
+}
+
+TmPacketStoredBase::TmPacketStoredBase() {
+}
+
+
+TmPacketStoredBase::~TmPacketStoredBase() {
+}
+
+store_address_t TmPacketStoredBase::getStoreAddress() {
+    return storeAddress;
+}
+
+void TmPacketStoredBase::deletePacket() {
+    store->deleteData(storeAddress);
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    setDataPointer(nullptr);
+}
+
+void TmPacketStoredBase::setStoreAddress(store_address_t setAddress) {
+    storeAddress = setAddress;
+    const uint8_t* tempData = nullptr;
+    size_t tempSize;
+    if (not checkAndSetStore()) {
+        return;
+    }
+    ReturnValue_t status = store->getData(storeAddress, &tempData, &tempSize);
+    if (status == StorageManagerIF::RETURN_OK) {
+        setDataPointer(tempData);
+    } else {
+        setDataPointer(nullptr);
+        storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    }
+}
+
+bool TmPacketStoredBase::checkAndSetStore() {
+    if (store == nullptr) {
+        store = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+        if (store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "TmPacketStored::TmPacketStored: TM Store not found!"
+                    << std::endl;
+#endif
+            return false;
+        }
+    }
+    return true;
+}
+
+ReturnValue_t TmPacketStoredBase::sendPacket(MessageQueueId_t destination,
+        MessageQueueId_t sentFrom, bool doErrorReporting) {
+    if (getAllTmData() == nullptr) {
+        //SHOULDDO: More decent code.
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    TmTcMessage tmMessage(getStoreAddress());
+    ReturnValue_t result = MessageQueueSenderIF::sendMessage(destination,
+            &tmMessage, sentFrom);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        deletePacket();
+        if (doErrorReporting) {
+            checkAndReportLostTm();
+        }
+        return result;
+    }
+    //SHOULDDO: In many cases, some counter is incremented for successfully sent packets. The check is often not done, but just incremented.
+    return HasReturnvaluesIF::RETURN_OK;
+
+}
+
+void TmPacketStoredBase::checkAndReportLostTm() {
+    if (internalErrorReporter == nullptr) {
+        internalErrorReporter = ObjectManager::instance()->get<InternalErrorReporterIF>(
+                objects::INTERNAL_ERROR_REPORTER);
+    }
+    if (internalErrorReporter != nullptr) {
+        internalErrorReporter->lostTm();
+    }
+}
+
+

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredBase.h

@@ -0,0 +1,89 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+
+#include "fsfw/FSFW.h"
+#include "TmPacketBase.h"
+#include "TmPacketStoredBase.h"
+#include "TmPacketPusA.h"
+
+#include "fsfw/serialize/SerializeIF.h"
+#include "fsfw/storagemanager/StorageManagerIF.h"
+#include "fsfw/internalerror/InternalErrorReporterIF.h"
+#include "fsfw/ipc/MessageQueueSenderIF.h"
+
+/**
+ *  This class generates a ECSS PUS Telemetry 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 TmPacketStoredBase {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer.
+     * However, it does try to set the packet store.
+     */
+    TmPacketStoredBase( store_address_t setAddress );
+    TmPacketStoredBase();
+
+    virtual ~TmPacketStoredBase();
+
+    virtual uint8_t* getAllTmData() = 0;
+    virtual void setDataPointer(const uint8_t* newPointer) = 0;
+
+    /**
+     * 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.
+     */
+    void 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);
+
+    ReturnValue_t sendPacket(MessageQueueId_t destination, MessageQueueId_t sentFrom,
+            bool doErrorReporting = true);
+
+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::TM_STORE.
+     */
+    static StorageManagerIF* store;
+
+    static InternalErrorReporterIF *internalErrorReporter;
+
+    /**
+     * 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();
+
+    void checkAndReportLostTm();
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_ */
+

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.cpp

@@ -0,0 +1,79 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.h"
+
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
+
+#include <cstring>
+
+TmPacketStoredPusA::TmPacketStoredPusA(store_address_t setAddress) :
+		TmPacketStoredBase(setAddress), TmPacketPusA(nullptr){
+}
+
+TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service,
+		uint8_t subservice, uint8_t packetSubcounter, const uint8_t *data,
+		uint32_t size, const uint8_t *headerData, uint32_t headerSize) :
+		TmPacketPusA(nullptr) {
+	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+	if (not TmPacketStoredBase::checkAndSetStore()) {
+		return;
+	}
+	uint8_t *pData = nullptr;
+	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+			(getPacketMinimumSize() + size + headerSize), &pData);
+
+	if (returnValue != store->RETURN_OK) {
+	    TmPacketStoredBase::checkAndReportLostTm();
+		return;
+	}
+	setData(pData);
+	initializeTmPacket(apid, service, subservice, packetSubcounter);
+	memcpy(getSourceData(), headerData, headerSize);
+	memcpy(getSourceData() + headerSize, data, size);
+	setPacketDataLength(
+			size + headerSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
+}
+
+TmPacketStoredPusA::TmPacketStoredPusA(uint16_t apid, uint8_t service,
+		uint8_t subservice, uint8_t packetSubcounter, SerializeIF *content,
+		SerializeIF *header) :
+		TmPacketPusA(nullptr) {
+	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+	if (not TmPacketStoredBase::checkAndSetStore()) {
+		return;
+	}
+	size_t sourceDataSize = 0;
+	if (content != NULL) {
+		sourceDataSize += content->getSerializedSize();
+	}
+	if (header != NULL) {
+		sourceDataSize += header->getSerializedSize();
+	}
+	uint8_t *p_data = NULL;
+	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+			(getPacketMinimumSize() + sourceDataSize), &p_data);
+	if (returnValue != store->RETURN_OK) {
+	    TmPacketStoredBase::checkAndReportLostTm();
+	}
+	setData(p_data);
+	initializeTmPacket(apid, service, subservice, packetSubcounter);
+	uint8_t *putDataHere = getSourceData();
+	size_t size = 0;
+	if (header != NULL) {
+		header->serialize(&putDataHere, &size, sourceDataSize,
+				SerializeIF::Endianness::BIG);
+	}
+	if (content != NULL) {
+		content->serialize(&putDataHere, &size, sourceDataSize,
+				SerializeIF::Endianness::BIG);
+	}
+	setPacketDataLength(
+			sourceDataSize + sizeof(PUSTmDataFieldHeaderPusA) + CRC_SIZE - 1);
+}
+
+uint8_t* TmPacketStoredPusA::getAllTmData() {
+    return getWholeData();
+}
+
+void TmPacketStoredPusA::setDataPointer(const uint8_t *newPointer) {
+    setData(newPointer);
+}

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusA.h

@@ -0,0 +1,65 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_
+
+#include "TmPacketStoredBase.h"
+#include "TmPacketPusA.h"
+#include <FSFWConfig.h>
+
+/**
+ *	This class generates a ECSS PUS A Telemetry 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 TmPacketStoredPusA :
+        public TmPacketStoredBase,
+        public TmPacketPusA {
+public:
+	/**
+	 * This is a default constructor which does not set the data pointer.
+	 * However, it does try to set the packet store.
+	 */
+	TmPacketStoredPusA( store_address_t setAddress );
+	/**
+	 * With this constructor, new space is allocated in the packet store and
+	 * a new PUS Telemetry Packet is created there.
+	 * Packet Application Data passed in data is copied into the packet.
+	 * The Application data is passed in two parts, first a header, then a
+	 * data field. This allows building a Telemetry Packet from two separate
+	 * data sources.
+	 * @param apid			Sets the packet's APID field.
+	 * @param service		Sets the packet's Service ID field.
+	 * 		This specifies the source service.
+	 * @param subservice	Sets the packet's Service Subtype field.
+	 * 		This specifies the source sub-service.
+	 * @param packet_counter	Sets the Packet counter field of this packet
+	 * @param data			The payload data to be copied to the
+	 * 						Application Data Field
+	 * @param size			The amount of data to be copied.
+	 * @param headerData 	The header Data of the Application field,
+	 * 						 will be copied in front of data
+	 * @param headerSize	The size of the headerDataF
+	 */
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
+			uint8_t packet_counter = 0, const uint8_t* data = nullptr,
+			uint32_t size = 0, const uint8_t* headerData = nullptr,
+			uint32_t headerSize = 0);
+	/**
+	 * Another ctor to directly pass structured content and header data to the
+	 * packet to avoid additional buffers.
+	 */
+	TmPacketStoredPusA( uint16_t apid, uint8_t service,	uint8_t subservice,
+			uint8_t packet_counter, SerializeIF* content,
+			SerializeIF* header = nullptr);
+
+    uint8_t* getAllTmData() override;
+    void setDataPointer(const uint8_t* newPointer) override;
+
+};
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTORED_PUSA_H_ */

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.cpp

@@ -0,0 +1,80 @@
+#include "fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.h"
+
+#include "fsfw/serviceinterface/ServiceInterface.h"
+#include "fsfw/tmtcservices/TmTcMessage.h"
+
+#include <cstring>
+
+TmPacketStoredPusC::TmPacketStoredPusC(store_address_t setAddress) :
+        TmPacketStoredBase(setAddress), TmPacketPusC(nullptr){
+}
+
+TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, const uint8_t *data,
+        uint32_t size, const uint8_t *headerData, uint32_t headerSize, uint16_t destinationId,
+        uint8_t timeRefField) :
+        TmPacketPusC(nullptr) {
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not TmPacketStoredBase::checkAndSetStore()) {
+        return;
+    }
+    uint8_t *pData = nullptr;
+    ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+            (getPacketMinimumSize() + size + headerSize), &pData);
+
+    if (returnValue != store->RETURN_OK) {
+        TmPacketStoredBase::checkAndReportLostTm();
+        return;
+    }
+    setData(pData);
+    initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
+    memcpy(getSourceData(), headerData, headerSize);
+    memcpy(getSourceData() + headerSize, data, size);
+    setPacketDataLength(
+            size + headerSize + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
+        uint8_t subservice, uint16_t packetSubcounter, SerializeIF *content,
+        SerializeIF *header, uint16_t destinationId, uint8_t timeRefField) :
+        TmPacketPusC(nullptr) {
+    storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
+    if (not TmPacketStoredBase::checkAndSetStore()) {
+        return;
+    }
+    size_t sourceDataSize = 0;
+    if (content != NULL) {
+        sourceDataSize += content->getSerializedSize();
+    }
+    if (header != NULL) {
+        sourceDataSize += header->getSerializedSize();
+    }
+    uint8_t *p_data = NULL;
+    ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
+            (getPacketMinimumSize() + sourceDataSize), &p_data);
+    if (returnValue != store->RETURN_OK) {
+        TmPacketStoredBase::checkAndReportLostTm();
+    }
+    setData(p_data);
+    initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
+    uint8_t *putDataHere = getSourceData();
+    size_t size = 0;
+    if (header != NULL) {
+        header->serialize(&putDataHere, &size, sourceDataSize,
+                SerializeIF::Endianness::BIG);
+    }
+    if (content != NULL) {
+        content->serialize(&putDataHere, &size, sourceDataSize,
+                SerializeIF::Endianness::BIG);
+    }
+    setPacketDataLength(
+            sourceDataSize + sizeof(PUSTmDataFieldHeaderPusC) + CRC_SIZE - 1);
+}
+
+uint8_t* TmPacketStoredPusC::getAllTmData() {
+    return getWholeData();
+}
+
+void TmPacketStoredPusC::setDataPointer(const uint8_t *newPointer) {
+    setData(newPointer);
+}

src/fsfw/tmtcpacket/pus/tm/TmPacketStoredPusC.h

@@ -0,0 +1,68 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+
+#include "TmPacketPusC.h"
+#include "TmPacketStoredBase.h"
+
+/**
+ *  This class generates a ECSS PUS C Telemetry 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 TmPacketStoredPusC:
+        public TmPacketStoredBase,
+        public TmPacketPusC {
+public:
+    /**
+     * This is a default constructor which does not set the data pointer.
+     * However, it does try to set the packet store.
+     */
+    TmPacketStoredPusC( store_address_t setAddress );
+    /**
+     * With this constructor, new space is allocated in the packet store and
+     * a new PUS Telemetry Packet is created there.
+     * Packet Application Data passed in data is copied into the packet.
+     * The Application data is passed in two parts, first a header, then a
+     * data field. This allows building a Telemetry Packet from two separate
+     * data sources.
+     * @param apid          Sets the packet's APID field.
+     * @param service       Sets the packet's Service ID field.
+     *      This specifies the source service.
+     * @param subservice    Sets the packet's Service Subtype field.
+     *      This specifies the source sub-service.
+     * @param packet_counter    Sets the Packet counter field of this packet
+     * @param data          The payload data to be copied to the
+     *                      Application Data Field
+     * @param size          The amount of data to be copied.
+     * @param headerData    The header Data of the Application field,
+     *                       will be copied in front of data
+     * @param headerSize    The size of the headerDataF
+     * @param destinationId Destination ID containing the application process ID as specified
+     *                      by PUS C
+     * @param timeRefField  4 bit time reference field as specified by PUS C
+     */
+    TmPacketStoredPusC( uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetCounter = 0, const uint8_t* data = nullptr,
+            uint32_t size = 0, const uint8_t* headerData = nullptr,
+            uint32_t headerSize = 0, uint16_t destinationId = 0, uint8_t timeRefField = 0);
+    /**
+     * Another ctor to directly pass structured content and header data to the
+     * packet to avoid additional buffers.
+     */
+    TmPacketStoredPusC( uint16_t apid, uint8_t service, uint8_t subservice,
+            uint16_t packetCounter, SerializeIF* content,
+            SerializeIF* header = nullptr, uint16_t destinationId = 0, uint8_t timeRefField = 0);
+
+    uint8_t* getAllTmData() override;
+    void setDataPointer(const uint8_t* newPointer) override;
+
+};
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_ */