Merge remote-tracking branch 'ksat/mueller/master' into mueller/pus-c-support

defaultcfg/fsfwconfig/FSFWConfig.h

@@ -17,6 +17,8 @@
 	#define FSFW_DISABLE_PRINTOUT 	0
 #endif
 
+#define FSFW_USE_PUS_C_TELEMETRY    1
+
 //! Can be used to disable the ANSI color sequences for C stdio.
 #define FSFW_COLORED_OUTPUT 		1
 
@@ -50,7 +52,7 @@
 namespace fsfwconfig {
 //! Default timestamp size. The default timestamp will be an eight byte CDC
 //! short timestamp.
-static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 8;
+static constexpr uint8_t FSFW_MISSION_TIMESTAMP_SIZE = 7;
 
 //! Configure the allocated pool sizes for the event manager.
 static constexpr size_t FSFW_EVENTMGMR_MATCHTREE_NODES = 240;

devicehandlers/HealthDevice.cpp

@@ -16,9 +16,9 @@ ReturnValue_t HealthDevice::performOperation(uint8_t opCode) {
 	CommandMessage command;
 	ReturnValue_t result = commandQueue->receiveMessage(&command);
 	if (result == HasReturnvaluesIF::RETURN_OK) {
-		healthHelper.handleHealthCommand(&command);
+		result = healthHelper.handleHealthCommand(&command);
 	}
-	return HasReturnvaluesIF::RETURN_OK;
+	return result;
 }
 
 ReturnValue_t HealthDevice::initialize() {

osal/linux/tcpipHelpers.cpp

@@ -99,8 +99,8 @@ void tcpip::handleError(Protocol protocol, ErrorSources errorSrc, dur_millis_t s
     sif::warning << "tcpip::handleError: " << protocolString << " | " << errorSrcString <<
             " | " << infoString << std::endl;
 #else
-    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString,
+    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString.c_str(),
-            errorSrcString, infoString);
+            errorSrcString.c_str(), infoString.c_str());
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
     if(sleepDuration > 0) {

pus/Service17Test.cpp

@@ -1,6 +1,7 @@
 #include "Service17Test.h"
+#include <FSFWConfig.h>
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/SystemObject.h"
 #include "../tmtcpacket/pus/TmPacketStored.h"
 
@@ -17,15 +18,25 @@ Service17Test::~Service17Test() {
 ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
 	switch(subservice) {
 	case Subservice::CONNECTION_TEST: {
-		TmPacketStored connectionPacket(apid, serviceId,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+		TmPacketStoredPusA connectionPacket(apid, serviceId,
 		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+		TmPacketStoredPusC connectionPacket(apid, serviceId,
+		        Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
 		connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
 				requestQueue->getId());
 		return HasReturnvaluesIF::RETURN_OK;
 	}
     case Subservice::EVENT_TRIGGER_TEST: {
-        TmPacketStored connectionPacket(apid, serviceId,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+        TmPacketStoredPusA connectionPacket(apid, serviceId,
                 Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#else
+        TmPacketStoredPusC connectionPacket(apid, serviceId,
+                Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
+#endif
         connectionPacket.sendPacket(requestQueue->getDefaultDestination(),
                 requestQueue->getId());
         triggerEvent(TEST, 1234, 5678);

pus/Service1TelecommandVerification.cpp

@@ -68,8 +68,13 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
 			message->getTcSequenceControl(), message->getStep(),
 			message->getErrorCode(), message->getParameter1(),
 			message->getParameter2());
-	TmPacketStored tmPacket(apid, serviceId, message->getReportId(),
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
 	return result;
@@ -79,8 +84,13 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
         PusVerificationMessage *message) {
 	SuccessReport report(message->getReportId(),message->getTcPacketId(),
 			message->getTcSequenceControl(),message->getStep());
-	TmPacketStored tmPacket(apid, serviceId, message->getReportId(),
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
 	        packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
+            packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
 			tmQueue->getId());
 	return result;

pus/Service5EventReporting.cpp

@@ -52,8 +52,13 @@ ReturnValue_t Service5EventReporting::generateEventReport(
 {
 	EventReport report(message.getEventId(),message.getReporter(),
 			message.getParameter1(),message.getParameter2());
-	TmPacketStored tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
 			message.getSeverity(), packetSubCounter++, &report);
+#else
+    TmPacketStoredPusC tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
+            message.getSeverity(), packetSubCounter++, &report);
+#endif
 	ReturnValue_t result = tmPacket.sendPacket(
 	        requestQueue->getDefaultDestination(),requestQueue->getId());
 	if(result != HasReturnvaluesIF::RETURN_OK) {

thermal/Heater.cpp

@@ -286,7 +286,10 @@ void Heater::handleQueue() {
 		if (result == HasReturnvaluesIF::RETURN_OK) {
 			return;
 		}
-		parameterHelper.handleParameterMessage(&command);
+		result = parameterHelper.handleParameterMessage(&command);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
 	}
 }
 

timemanager/TimeMessage.cpp

@@ -25,6 +25,6 @@ uint32_t TimeMessage::getCounterValue() {
 	return temp;
 }
 
-size_t TimeMessage::getMinimumMessageSize() {
+size_t TimeMessage::getMinimumMessageSize() const {
 	return this->MAX_SIZE;
 }

timemanager/TimeMessage.h

@@ -11,7 +11,7 @@ protected:
 	 * @brief	This call always returns the same fixed size of the message.
 	 * @return	Returns HEADER_SIZE + \c sizeof(timeval) + sizeof(uint32_t).
 	 */
-	size_t getMinimumMessageSize();
+	size_t getMinimumMessageSize() const override;
 public:
 
 	/**

timemanager/TimeStamperIF.h

@@ -1,6 +1,7 @@
 #ifndef FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 #define FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 
+#include <FSFWConfig.h>
 #include "../returnvalues/HasReturnvaluesIF.h"
 
 /**
@@ -16,8 +17,8 @@ public:
 
 	//! This is a mission-specific constant and determines the total
 	//! size reserved for timestamps.
-	//! TODO: Default define in FSFWConfig ?
+	static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;
-	static const uint8_t MISSION_TIMESTAMP_SIZE = 8;
+
 	virtual ReturnValue_t addTimeStamp(uint8_t* buffer,
 	        const uint8_t maxSize) = 0;
 	virtual ~TimeStamperIF() {}

tmtcpacket/pus/CMakeLists.txt

@@ -4,5 +4,9 @@ target_sources(${LIB_FSFW_NAME}
 		TcPacketStored.cpp
 		TmPacketBase.cpp
 		TmPacketMinimal.cpp
-		TmPacketStored.cpp
+		TmPacketStoredPusA.cpp
+		TmPacketStoredPusC.cpp
+		TmPacketPusA.cpp
+		TmPacketPusC.cpp
+		TmPacketStoredBase.cpp
 )

tmtcpacket/pus/TmPacketBase.cpp

@@ -3,43 +3,30 @@
 #include "../../globalfunctions/CRC.h"
 #include "../../globalfunctions/arrayprinter.h"
 #include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../timemanager/CCSDSTime.h"
 
 #include <cstring>
 
 TimeStamperIF* TmPacketBase::timeStamper = nullptr;
-object_id_t TmPacketBase::timeStamperId = 0;
+object_id_t TmPacketBase::timeStamperId = objects::NO_OBJECT;
 
-TmPacketBase::TmPacketBase(uint8_t* setData) :
+TmPacketBase::TmPacketBase(uint8_t* setData):
         SpacePacketBase(setData) {
-	tmData = reinterpret_cast<TmPacketPointer*>(setData);
 }
 
 TmPacketBase::~TmPacketBase() {
     //Nothing to do.
 }
 
-uint8_t TmPacketBase::getService() {
-	return tmData->data_field.service_type;
-}
-
-uint8_t TmPacketBase::getSubService() {
-	return tmData->data_field.service_subtype;
-}
-
-uint8_t* TmPacketBase::getSourceData() {
-	return &tmData->data;
-}
 
 uint16_t TmPacketBase::getSourceDataSize() {
-	return getPacketDataLength() - sizeof(tmData->data_field)
+    return getPacketDataLength() - getDataFieldSize() - CRC_SIZE + 1;
-			- CRC_SIZE + 1;
 }
 
 uint16_t TmPacketBase::getErrorControl() {
     uint32_t size = getSourceDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tmData->data;
+    uint8_t* p_to_buffer = getSourceData();
     return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
 }
 
@@ -51,9 +38,10 @@ void TmPacketBase::setErrorControl() {
     getSourceData()[size + 1] = (crc) & 0X00FF; 		// CRCL
 }
 
-void TmPacketBase::setData(const uint8_t* p_Data) {
+ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
-	SpacePacketBase::setData(p_Data);
+    size_t tempSize = 0;
-	tmData = (TmPacketPointer*) p_Data;
+    return CCSDSTime::convertFromCcsds(timestamp, getPacketTimeRaw(),
+            &tempSize, getTimestampSize());
 }
 
 void TmPacketBase::print() {
@@ -68,8 +56,9 @@ bool TmPacketBase::checkAndSetStamper() {
         timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
         if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
+            sif::warning << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl;
-					<< std::endl;
+#else
+            sif::printWarning("TmPacketBase::checkAndSetStamper: Stamper not found!\n");
 #endif
             return false;
         }
@@ -77,45 +66,3 @@ bool TmPacketBase::checkAndSetStamper() {
     return true;
 }
 
-ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
-	size_t tempSize = 0;
-	return CCSDSTime::convertFromCcsds(timestamp, tmData->data_field.time,
-			&tempSize, sizeof(tmData->data_field.time));
-}
-
-uint8_t* TmPacketBase::getPacketTimeRaw() const{
-	return tmData->data_field.time;
-
-}
-
-void TmPacketBase::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 (checkAndSetStamper()) {
-		timeStamper->addTimeStamp(tmData->data_field.time,
-		        sizeof(tmData->data_field.time));
-	}
-}
-
-void TmPacketBase::setSourceDataSize(uint16_t size) {
-	setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
-}
-
-size_t TmPacketBase::getTimestampSize() const {
-	return sizeof(tmData->data_field.time);
-}

tmtcpacket/pus/TmPacketBase.h

@@ -10,32 +10,6 @@ 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 PUSTmDataFieldHeader {
-	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 TmPacketPointer {
-	CCSDSPrimaryHeader primary;
-	PUSTmDataFieldHeader data_field;
-	uint8_t data;
-};
 
 /**
  * This class is the basic data handler for any ECSS PUS Telemetry packet.
@@ -51,17 +25,12 @@ struct TmPacketPointer {
 class TmPacketBase : public SpacePacketBase {
     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(PUSTmDataFieldHeader) + 2);
     //! Maximum size of a TM Packet in this mission.
     //! TODO: Make this dependant on a config variable.
     static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
-	//! First byte of secondary header for PUS-A packets.
+    //! First four bits of first byte of secondary header
-	//! TODO: Maybe also support PUS-C via config?
+    static const uint8_t VERSION_NUMBER_BYTE = 0b00010000;
-	static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000;
 
     /**
      * This is the default constructor.
@@ -80,13 +49,13 @@ public:
      * byte of the Data Field Header.
      * @return	The packet's PUS Service ID.
      */
-	uint8_t getService();
+    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.
      */
-	uint8_t getSubService();
+    virtual uint8_t getSubService() = 0;
     /**
      * This is a getter for a pointer to the packet's Source data.
      *
@@ -94,7 +63,7 @@ public:
      * the packet's source data.
      * @return	A pointer to the PUS Source Data.
      */
-	uint8_t* getSourceData();
+    virtual uint8_t* getSourceData() = 0;
     /**
      * This method calculates the size of the PUS Source data field.
      *
@@ -102,8 +71,35 @@ public:
      * and subtracts the Data Field Header size and the CRC size.
      * @return	The size of the PUS Source Data (without Error Control field)
      */
-	uint16_t getSourceDataSize();
+    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
@@ -111,7 +107,6 @@ public:
      * like the sequence count.
      */
     void setErrorControl();
-
     /**
      * This getter returns the Error Control Field of the packet.
      *
@@ -122,33 +117,7 @@ public:
      */
     uint16_t getErrorControl();
 
-	/**
-	 * This is a debugging helper method that prints the whole packet content
-	 * to the screen.
-	 */
-	void print();
-	/**
-	 * Interprets the "time"-field in the secondary header and returns it in
-	 * timeval format.
-	 * @return Converted timestamp of packet.
-	 */
-	ReturnValue_t getPacketTime(timeval* timestamp) const;
-	/**
-	 * Returns a raw pointer to the beginning of the time field.
-	 * @return Raw pointer to time field.
-	 */
-	uint8_t* getPacketTimeRaw() const;
-
-	size_t getTimestampSize() const;
-
 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.
-	 */
-	TmPacketPointer* tmData;
     /**
      * The timeStamper is responsible for adding a timestamp to the packet.
      * It is initialized lazy.
@@ -157,34 +126,6 @@ protected:
     //! The ID to use when looking for a time stamper.
     static object_id_t timeStamperId;
 
-    /**
-     * 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.

tmtcpacket/pus/TmPacketIF.h

@@ -0,0 +1,8 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETIF_H_
+
+
+
+
+
+#endif /* FSFW_TMTCPACKET_PUS_TMPACKETIF_H_ */

tmtcpacket/pus/TmPacketPusA.cpp

@@ -0,0 +1,87 @@
+#include "TmPacketPusA.h"
+#include "TmPacketBase.h"
+
+#include "../../globalfunctions/CRC.h"
+#include "../../globalfunctions/arrayprinter.h"
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../timemanager/CCSDSTime.h"
+
+#include <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);
+}

tmtcpacket/pus/TmPacketPusA.h

@@ -0,0 +1,129 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSA_H_
+
+#include "TmPacketBase.h"
+#include "../SpacePacketBase.h"
+#include "../../timemanager/TimeStamperIF.h"
+#include "../../timemanager/Clock.h"
+#include "../../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.
+    //! TODO: Make this dependant on a config variable.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+
+    /**
+     * 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_ */

tmtcpacket/pus/TmPacketPusC.cpp

@@ -0,0 +1,87 @@
+#include "TmPacketPusC.h"
+#include "TmPacketBase.h"
+
+#include "../../globalfunctions/CRC.h"
+#include "../../globalfunctions/arrayprinter.h"
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../timemanager/CCSDSTime.h"
+
+#include <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);
+}

tmtcpacket/pus/TmPacketPusC.h

@@ -0,0 +1,126 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETPUSC_H_
+
+#include "TmPacketBase.h"
+#include "../SpacePacketBase.h"
+#include "../../timemanager/TimeStamperIF.h"
+#include "../../timemanager/Clock.h"
+#include "../../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.
+    //! TODO: Make this dependant on a config variable.
+    static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048;
+
+    /**
+     * 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_ */

tmtcpacket/pus/TmPacketStored.cpp

@@ -1,147 +0,0 @@
-#include "TmPacketStored.h"
-
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-#include "../../tmtcservices/TmTcMessage.h"
-
-#include <cstring>
-
-StorageManagerIF *TmPacketStored::store = nullptr;
-InternalErrorReporterIF *TmPacketStored::internalErrorReporter = nullptr;
-
-TmPacketStored::TmPacketStored(store_address_t setAddress) :
-		TmPacketBase(nullptr), storeAddress(setAddress) {
-	setStoreAddress(storeAddress);
-}
-
-TmPacketStored::TmPacketStored(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) :
-		TmPacketBase(NULL) {
-	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not checkAndSetStore()) {
-		return;
-	}
-	uint8_t *pData = nullptr;
-	ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
-			(TmPacketBase::TM_PACKET_MIN_SIZE + size + headerSize), &pData);
-
-	if (returnValue != store->RETURN_OK) {
-		checkAndReportLostTm();
-		return;
-	}
-	setData(pData);
-	initializeTmPacket(apid, service, subservice, packetSubcounter);
-	memcpy(getSourceData(), headerData, headerSize);
-	memcpy(getSourceData() + headerSize, data, size);
-	setPacketDataLength(
-			size + headerSize + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1);
-}
-
-TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service,
-		uint8_t subservice, uint8_t packetSubcounter, SerializeIF *content,
-		SerializeIF *header) :
-		TmPacketBase(NULL) {
-	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	if (not 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,
-			(TmPacketBase::TM_PACKET_MIN_SIZE + sourceDataSize), &p_data);
-	if (returnValue != store->RETURN_OK) {
-		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(PUSTmDataFieldHeader) + CRC_SIZE - 1);
-}
-
-store_address_t TmPacketStored::getStoreAddress() {
-	return storeAddress;
-}
-
-void TmPacketStored::deletePacket() {
-	store->deleteData(storeAddress);
-	storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	setData(nullptr);
-}
-
-void TmPacketStored::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) {
-		setData(tempData);
-	} else {
-		setData(nullptr);
-		storeAddress.raw = StorageManagerIF::INVALID_ADDRESS;
-	}
-}
-
-bool TmPacketStored::checkAndSetStore() {
-	if (store == nullptr) {
-		store = objectManager->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 TmPacketStored::sendPacket(MessageQueueId_t destination,
-		MessageQueueId_t sentFrom, bool doErrorReporting) {
-	if (getWholeData() == 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 TmPacketStored::checkAndReportLostTm() {
-	if (internalErrorReporter == nullptr) {
-		internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
-				objects::INTERNAL_ERROR_REPORTER);
-	}
-	if (internalErrorReporter != nullptr) {
-		internalErrorReporter->lostTm();
-	}
-}

tmtcpacket/pus/TmPacketStored.h

@@ -1,108 +1,13 @@
 #ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
 #define FSFW_TMTCPACKET_PUS_TMPACKETSTORED_H_
 
-#include "TmPacketBase.h"
+#include <FSFWConfig.h>
 
-#include "../../serialize/SerializeIF.h"
+#if FSFW_USE_PUS_C_TELEMETRY == 1
-#include "../../storagemanager/StorageManagerIF.h"
+#include "TmPacketStoredPusC.h"
-#include "../../internalError/InternalErrorReporterIF.h"
+#else
-#include "../../ipc/MessageQueueSenderIF.h"
+#include "TmPacketStoredPusA.h"
-
+#endif
-/**
- *	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 TmPacketStored : public TmPacketBase {
-public:
-	/**
-	 * This is a default constructor which does not set the data pointer.
-	 * However, it does try to set the packet store.
-	 */
-	TmPacketStored( 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
-	 */
-	TmPacketStored( 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.
-	 */
-	TmPacketStored( uint16_t apid, uint8_t service,	uint8_t subservice,
-			uint8_t packet_counter, SerializeIF* content,
-			SerializeIF* header = nullptr);
-	/**
-	 * 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 );
-private:
-	/**
-	 * This is a pointer to the store all instances of the class use.
-	 * If the store is not yet set (i.e. @c store is NULL), every constructor
-	 * call tries to set it and throws an error message in case of failures.
-	 * The default store is objects::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_TMPACKETSTORED_H_ */

tmtcpacket/pus/TmPacketStoredBase.cpp

@@ -0,0 +1,94 @@
+#include "TmPacketStoredBase.h"
+
+#include "../../objectmanager/ObjectManagerIF.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../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->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->get<InternalErrorReporterIF>(
+                objects::INTERNAL_ERROR_REPORTER);
+    }
+    if (internalErrorReporter != nullptr) {
+        internalErrorReporter->lostTm();
+    }
+}
+
+

tmtcpacket/pus/TmPacketStoredBase.h

@@ -0,0 +1,89 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDBASE_H_
+
+#include "TmPacketBase.h"
+#include "TmPacketStoredBase.h"
+#include <FSFWConfig.h>
+
+#include "../../tmtcpacket/pus/TmPacketPusA.h"
+#include "../../serialize/SerializeIF.h"
+#include "../../storagemanager/StorageManagerIF.h"
+#include "../../internalError/InternalErrorReporterIF.h"
+#include "../../ipc/MessageQueueSenderIF.h"
+
+/**
+ *  This class generates a ECSS PUS Telemetry packet within a given
+ *  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_ */
+

tmtcpacket/pus/TmPacketStoredPusA.cpp

@@ -0,0 +1,79 @@
+#include "TmPacketStoredPusA.h"
+
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../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);
+}

tmtcpacket/pus/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_ */

tmtcpacket/pus/TmPacketStoredPusC.cpp

@@ -0,0 +1,80 @@
+#include "TmPacketStoredPusC.h"
+
+#include "../../serviceinterface/ServiceInterface.h"
+#include "../../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);
+}

tmtcpacket/pus/TmPacketStoredPusC.h

@@ -0,0 +1,65 @@
+#ifndef FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+#define FSFW_TMTCPACKET_PUS_TMPACKETSTOREDPUSC_H_
+
+#include <fsfw/tmtcpacket/pus/TmPacketPusC.h>
+#include <fsfw/tmtcpacket/pus/TmPacketStoredBase.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 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
+     */
+    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_ */

tmtcservices/CommandingServiceBase.cpp

@@ -1,6 +1,7 @@
 #include "AcceptsTelemetryIF.h"
 #include "CommandingServiceBase.h"
 #include "TmTcMessage.h"
+#include <FSFWConfig.h>
 
 #include "../tcdistribution/PUSDistributorIF.h"
 #include "../objectmanager/ObjectManagerIF.h"
@@ -293,8 +294,13 @@ void CommandingServiceBase::handleRequestQueue() {
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 		const uint8_t* data, size_t dataLen, const uint8_t* headerData,
 		size_t headerSize) {
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+	TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
 			this->tmPacketCounter, data, dataLen, headerData, headerSize);
+#else
+	TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, data, dataLen, headerData, headerSize);
+#endif
 	ReturnValue_t result = tmPacketStored.sendPacket(
 			requestQueue->getDefaultDestination(), requestQueue->getId());
 	if (result == HasReturnvaluesIF::RETURN_OK) {
@@ -311,8 +317,13 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
     size_t size = 0;
     SerializeAdapter::serialize(&objectId, &pBuffer, &size,
                 sizeof(object_id_t), SerializeIF::Endianness::BIG);
-    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+    TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, data, dataLen, buffer, size);
+#else
+    TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, data, dataLen, buffer, size);
+#endif
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
     if (result == HasReturnvaluesIF::RETURN_OK) {
@@ -324,8 +335,13 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
 
 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
         SerializeIF* content, SerializeIF* header) {
-    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+#if FSFW_USE_PUS_C_TELEMETRY == 0
+    TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice,
             this->tmPacketCounter, content, header);
+#else
+    TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, content, header);
+#endif
     ReturnValue_t result = tmPacketStored.sendPacket(
             requestQueue->getDefaultDestination(), requestQueue->getId());
     if (result == HasReturnvaluesIF::RETURN_OK) {

tmtcservices/TmTcMessage.cpp

@@ -21,7 +21,7 @@ TmTcMessage::TmTcMessage(store_address_t storeId) {
 	this->setStorageId(storeId);
 }
 
-size_t TmTcMessage::getMinimumMessageSize() {
+size_t TmTcMessage::getMinimumMessageSize() const {
 	return this->HEADER_SIZE + sizeof(store_address_t);
 }
 

tmtcservices/TmTcMessage.h

@@ -18,7 +18,7 @@ protected:
 	 * @brief	This call always returns the same fixed size of the message.
 	 * @return	Returns HEADER_SIZE + @c sizeof(store_address_t).
 	 */
-	size_t getMinimumMessageSize();
+	size_t getMinimumMessageSize() const override;
 public:
 	/**
 	 * @brief	In the default constructor, only the message_size is set.

unittest/tests/tmtcpacket/CMakeLists.txt

@@ -0,0 +1,3 @@
+target_sources(${TARGET_NAME} PRIVATE
+	PusTmTest.cpp
+)

unittest/tests/tmtcpacket/PusTmTest.cpp

@@ -0,0 +1,3 @@
+
+
+

unittest/user/unittest/core/CatchFactory.cpp

@@ -1,6 +1,6 @@
+#include "CatchFactory.h"
 #include <fsfw/datapoollocal/LocalDataPoolManager.h>
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
-#include "CatchFactory.h"
 
 #include <fsfw/events/EventManager.h>
 #include <fsfw/health/HealthTable.h>
@@ -74,7 +74,7 @@ void Factory::setStaticFrameworkObjectIds() {
 
 	DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT;
 
-	TmPacketStored::timeStamperId = objects::NO_OBJECT;
+	TmPacketBase::timeStamperId = objects::NO_OBJECT;
 }