Merge remote-tracking branch 'upstream/mueller/master' into mueller/master

Reviewed-on: fsfw/fsfw#415

CMakeLists.txt

@@ -1,5 +1,13 @@
 cmake_minimum_required(VERSION 3.13)
 
+option(FSFW_GENERATE_SECTIONS
+    "Generate function and data sections. Required to remove unused code" ON
+)
+
+if(FSFW_GENERATE_SECTIONS)
+    option(FSFW_REMOVE_UNUSED_CODE "Remove unused code" ON)
+endif()
+
 option(FSFW_WARNING_SHADOW_LOCAL_GCC "Enable -Wshadow=local warning in GCC" ON)
 # Options to exclude parts of the FSFW from compilation.
 option(FSFW_USE_RMAP "Compile with RMAP" ON)
@@ -26,15 +34,22 @@ if(NOT OS_FSFW)
 
 endif()
 
+set(FSFW_OSAL_DEFINITION FSFW_HOST)
+
 if(${OS_FSFW} STREQUAL host)
 	set(OS_FSFW_NAME "Host")
 elseif(${OS_FSFW} STREQUAL linux)
 	set(OS_FSFW_NAME "Linux")
+	set(FSFW_OSAL_DEFINITION FSFW_LINUX)
 elseif(${OS_FSFW} STREQUAL  freertos)
 	set(OS_FSFW_NAME "FreeRTOS")
-	target_link_libraries(${LIB_FSFW_NAME} ${LIB_OS_NAME})
+	set(FSFW_OSAL_DEFINITION FSFW_FREERTOS)
+	target_link_libraries(${LIB_FSFW_NAME} PRIVATE
+        ${LIB_OS_NAME}
+	)
 elseif(${OS_FSFW} STREQUAL rtems)
 	set(OS_FSFW_NAME "RTEMS")
+    set(FSFW_OSAL_DEFINITION FSFW_RTEMS)
 else()
 	message(WARNING 
 		"Invalid operating system for FSFW specified! Setting to host.."
@@ -43,6 +58,14 @@ else()
 	set(OS_FSFW "host")
 endif()
 
+target_compile_definitions(${LIB_FSFW_NAME} PRIVATE
+    ${FSFW_OSAL_DEFINITION}
+)
+
+target_compile_definitions(${LIB_FSFW_NAME} INTERFACE
+    ${FSFW_OSAL_DEFINITION}
+)
+
 message(STATUS "Compiling FSFW for the ${OS_FSFW_NAME} operating system.")
 
 add_subdirectory(action)
@@ -88,6 +111,7 @@ add_subdirectory(timemanager)
 add_subdirectory(tmstorage)
 add_subdirectory(tmtcpacket)
 add_subdirectory(tmtcservices)
+add_subdirectory(unittest)
 
 # The project CMakeLists file has to set the FSFW_CONFIG_PATH and add it.
 # If this is not given, we include the default configuration and emit a warning.
@@ -107,6 +131,21 @@ else()
 	)
 endif()
 
+foreach(INCLUDE_PATH ${FSFW_ADDITIONAL_INC_PATH})
+    if(IS_ABSOLUTE ${INCLUDE_PATH})
+        set(CURR_ABS_INC_PATH "${FREERTOS_PATH}")
+    else()
+        get_filename_component(CURR_ABS_INC_PATH
+            ${INCLUDE_PATH} REALPATH BASE_DIR ${CMAKE_SOURCE_DIR})
+    endif()
+
+    if(CMAKE_VERBOSE)
+        message(STATUS "FSFW include path: ${CURR_ABS_INC_PATH}")
+    endif()
+
+    list(APPEND FSFW_ADD_INC_PATHS_ABS ${CURR_ABS_INC_PATH})
+endforeach()
+
 if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
     if(NOT DEFINED FSFW_WARNING_FLAGS)
 	   set(FSFW_WARNING_FLAGS
@@ -117,7 +156,20 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
 		  -Wno-psabi
         )
     endif()
-        
+
+    if(FSFW_GENERATE_SECTIONS)
+        target_compile_options(${LIB_FSFW_NAME} PRIVATE
+            "-ffunction-sections"
+            "-fdata-sections"
+        )
+    endif()
+
+    if(FSFW_REMOVE_UNUSED_CODE)
+        target_link_options(${LIB_FSFW_NAME} PRIVATE
+            "Wl,--gc-sections"
+        )
+    endif()
+  
     if(FSFW_WARNING_SHADOW_LOCAL_GCC)
         list(APPEND WARNING_FLAGS "-Wshadow=local")
     endif()
@@ -132,6 +184,7 @@ endif()
 target_include_directories(${LIB_FSFW_NAME} INTERFACE 
 	${CMAKE_SOURCE_DIR}
 	${FSFW_CONFIG_PATH_ABSOLUTE}
+	${FSFW_ADD_INC_PATHS_ABS}
 )
 
 # Includes path required to compile FSFW itself as well
@@ -140,9 +193,14 @@ target_include_directories(${LIB_FSFW_NAME} INTERFACE
 target_include_directories(${LIB_FSFW_NAME} PRIVATE 
 	${CMAKE_SOURCE_DIR}
 	${FSFW_CONFIG_PATH_ABSOLUTE}
+	${FSFW_ADD_INC_PATHS_ABS}
 )
 
 target_compile_options(${LIB_FSFW_NAME} PRIVATE 
 	${FSFW_WARNING_FLAGS}
 	${COMPILER_FLAGS}
 )
+
+target_link_libraries(${LIB_FSFW_NAME} PRIVATE
+    ${FSFW_ADDITIONAL_LINK_LIBS}
+)

FSFWVersion.h

@@ -3,9 +3,9 @@
 
 const char* const FSFW_VERSION_NAME = "ASTP";
 
-#define FSFW_VERSION 		0
-#define	FSFW_SUBVERSION 	0
-#define	FSFW_REVISION 	1
+#define FSFW_VERSION        1
+#define	FSFW_SUBVERSION     0
+#define	FSFW_REVISION       0
 
 
 

action/ActionHelper.cpp

@@ -147,11 +147,6 @@ ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo,
         return result;
     }
 
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        ipcStore->deleteData(storeAddress);
-        return result;
-    }
-
     /* We don't need to report the objectId, as we receive REQUESTED data before the completion
     success message. True aperiodic replies need to be reported with another dedicated message. */
     ActionMessage::setDataReply(&reply, replyId, storeAddress);

container/SharedRingBuffer.cpp

@@ -17,6 +17,9 @@ SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer,
     mutex = MutexFactory::instance()->createMutex();
 }
 
+SharedRingBuffer::~SharedRingBuffer() {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
 
 void SharedRingBuffer::setToUseReceiveSizeFIFO(size_t fifoDepth) {
     this->fifoDepth = fifoDepth;

container/SharedRingBuffer.h

@@ -26,6 +26,18 @@ public:
      */
     SharedRingBuffer(object_id_t objectId, const size_t size,
             bool overwriteOld, size_t maxExcessBytes);
+    /**
+     * This constructor takes an external buffer with the specified size.
+     * @param buffer
+     * @param size
+     * @param overwriteOld
+     * If the ring buffer is overflowing at a write operartion, the oldest data
+     * will be overwritten.
+     */
+    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
+            bool overwriteOld, size_t maxExcessBytes);
+
+    virtual~ SharedRingBuffer();
 
     /**
      * @brief    This function can be used to add an optional FIFO to the class
@@ -37,16 +49,7 @@ public:
      */
     void setToUseReceiveSizeFIFO(size_t fifoDepth);
 
-    /**
-     * This constructor takes an external buffer with the specified size.
-     * @param buffer
-     * @param size
-     * @param overwriteOld
-     * If the ring buffer is overflowing at a write operartion, the oldest data
-     * will be overwritten.
-     */
-    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
-            bool overwriteOld, size_t maxExcessBytes);
+
 
     /**
      * Unless a read-only constant value is read, all operations on the

datapool/HkSwitchHelper.cpp

@@ -7,7 +7,7 @@ HkSwitchHelper::HkSwitchHelper(EventReportingProxyIF* eventProxy) :
 }
 
 HkSwitchHelper::~HkSwitchHelper() {
-	// TODO Auto-generated destructor stub
+    QueueFactory::instance()->deleteMessageQueue(actionQueue);
 }
 
 ReturnValue_t HkSwitchHelper::initialize() {

datapool/PoolDataSetBase.cpp

@@ -64,11 +64,11 @@ ReturnValue_t PoolDataSetBase::read(MutexIF::TimeoutType timeoutType,
     }
     else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "DataSet::read(): Call made in wrong position. Don't forget to commit"
-                " member datasets!" << std::endl;
+        sif::warning << "PoolDataSetBase::read: Call made in wrong position. Don't forget to "
+                "commit member datasets!" << std::endl;
 #else
-        sif::printError("DataSet::read(): Call made in wrong position. Don't forget to commit"
-                " member datasets!\n");
+        sif::printWarning("PoolDataSetBase::read: Call made in wrong position. Don't forget to "
+                "commit member datasets!\n");
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
         result = SET_WAS_ALREADY_READ;
     }

datapoollocal/LocalDataPoolManager.cpp

@@ -909,27 +909,29 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
 			errorPrint = "Unknown error";
 		}
 	}
+	object_id_t objectId = 0xffffffff;
+	if(owner != nullptr) {
+	    objectId = owner->getObjectId();
+	}
 
 	if(outputType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 	else if(outputType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 #endif /* #if FSFW_VERBOSE_LEVEL >= 1 */

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
 
@@ -40,10 +42,17 @@
 //! Specify whether a special mode store is used for Subsystem components.
 #define FSFW_USE_MODESTORE          0
 
+//! Defines if the real time scheduler for linux should be used.
+//! If set to 0, this will also disable priority settings for linux
+//! as most systems will not allow to set nice values without privileges
+//! For embedded linux system set this to 1.
+//! If set to 1 the binary needs "cap_sys_nice=eip" privileges to run
+#define FSFW_USE_REALTIME_FOR_LINUX 1
+
 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;
+
+//! Default timestamp size. The default timestamp will be an seven byte CDC short timestamp.
+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;
@@ -52,11 +61,12 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 
 //! Defines the FIFO depth of each commanding service base which
 //! also determines how many commands a CSB service can handle in one cycle
-//! simulataneously. This will increase the required RAM for
+//! simultaneously. This will increase the required RAM for
 //! each CSB service !
 static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
 
 static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
+
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */

devicehandlers/AssemblyBase.h

@@ -24,6 +24,13 @@
  * 1. check logic when active-> checkChildrenStateOn
  * 2. transition logic to change the mode -> commandChildren
  *
+ * Important:
+ *
+ * The implementation must call registerChild(object_id_t child)
+ * for all commanded children during initialization.
+ * The implementation must call the initialization function of the base class.
+ * (This will call the function in SubsystemBase)
+ *
  */
 class AssemblyBase: public SubsystemBase {
 public:
@@ -41,9 +48,6 @@ public:
 	virtual ~AssemblyBase();
 
 protected:
-
-    // SHOULDDO: Change that OVERWRITE_HEALTH may be returned
-    // (or return internalState directly?)
     /**
      * Command children to reach [mode,submode] combination
      * Can be done by setting #commandsOutstanding correctly,
@@ -68,6 +72,18 @@ protected:
     virtual ReturnValue_t checkChildrenStateOn(Mode_t wantedMode,
             Submode_t wantedSubmode) = 0;
 
+    /**
+     * Check whether a combination of mode and submode is valid.
+     *
+     * Ground Controller like precise return values from HasModesIF.
+     * So, please return any of them.
+     *
+     * @param mode The targeted mode
+     * @param submode The targeted submmode
+     * @return Any information why this combination is invalid from HasModesIF
+     * 			like HasModesIF::INVALID_SUBMODE.
+     * 			On success return HasReturnvaluesIF::RETURN_OK
+     */
     virtual ReturnValue_t isModeCombinationValid(Mode_t mode,
             Submode_t submode) = 0;
 

devicehandlers/DeviceHandlerBase.cpp

@@ -1491,7 +1491,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
         if(errorCode == ObjectManagerIF::CHILD_INIT_FAILED) {
             errorPrint = "Initialization error";
         }
-        if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
+        else if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
             if(errorType == sif::OutputTypes::OUT_WARNING) {
                 errorPrint = "Generic Warning";
             }
@@ -1503,6 +1503,9 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
             errorPrint = "Unknown error";
         }
     }
+    if(functionName == nullptr) {
+        functionName = "unknown function";
+    }
 
     if(errorType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -1511,18 +1514,18 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
                 std::dec << std::setfill(' ') << std::endl;
 #else
         sif::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
     else if(errorType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "DeviceHandlerBase::" << functionName << ": Object ID "
+        sif::error << "DeviceHandlerBase::" << functionName << ": Object ID 0x"
                 << std::hex << std::setw(8) << std::setfill('0')
                 << this->getObjectId() << " | " << errorPrint << std::dec
                 << std::setfill(' ') << std::endl;
 #else
         sif::printError("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
 

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() {

events/EventMessage.cpp

@@ -109,6 +109,6 @@ bool EventMessage::isClearedEventMessage() {
 	return getEvent() == INVALID_EVENT;
 }
 
-size_t EventMessage::getMinimumMessageSize() {
+size_t EventMessage::getMinimumMessageSize() const {
 	return EVENT_MESSAGE_SIZE;
 }

events/EventMessage.h

@@ -45,7 +45,7 @@ public:
 
 protected:
 	static const Event INVALID_EVENT = 0;
-	virtual size_t getMinimumMessageSize();
+	virtual size_t getMinimumMessageSize() const override;
 
 };
 

globalfunctions/arrayprinter.cpp

@@ -60,7 +60,7 @@ void arrayprinter::printHex(const uint8_t *data, size_t size,
 #else
     // General format: 0x01, 0x02, 0x03 so it is number of chars times 6
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 7 + 1];
+    char printBuffer[(size + 1) * 7 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.
@@ -103,7 +103,7 @@ void arrayprinter::printDec(const uint8_t *data, size_t size,
 #else
     // General format: 32, 243, -12 so it is number of chars times 5
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 5 + 1];
+    char printBuffer[(size + 1) * 5 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.

health/HealthTable.cpp

@@ -68,14 +68,30 @@ void HealthTable::printAll(uint8_t* pointer, size_t maxSize) {
     MutexGuard(mutex, timeoutType, mutexTimeoutMs);
 	size_t size = 0;
 	uint16_t count = healthMap.size();
-	SerializeAdapter::serialize(&count,
+	ReturnValue_t result = SerializeAdapter::serialize(&count,
 			&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::warning << "HealthTable::printAll: Serialization of health table failed" << std::endl;
+#else
+	    sif::printWarning("HealthTable::printAll: Serialization of health table failed\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	    return;
+	}
 	for (const auto& health: healthMap) {
-		SerializeAdapter::serialize(&health.first,
+		result = SerializeAdapter::serialize(&health.first,
 				&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+		    return;
+		}
 		uint8_t healthValue = health.second;
-		SerializeAdapter::serialize(&healthValue, &pointer, &size,
+		result = SerializeAdapter::serialize(&healthValue, &pointer, &size,
 				maxSize, SerializeIF::Endianness::BIG);
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
 	}
 }
 
@@ -86,7 +102,7 @@ ReturnValue_t HealthTable::iterate(HealthEntry *value, bool reset) {
 		mapIterator = healthMap.begin();
 	}
 	if (mapIterator == healthMap.end()) {
-		result = HasReturnvaluesIF::RETURN_FAILED;
+		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	*value = *mapIterator;
 	mapIterator++;

internalError/InternalErrorReporter.cpp

@@ -3,19 +3,20 @@
 #include "../ipc/QueueFactory.h"
 #include "../ipc/MutexFactory.h"
 #include "../serviceinterface/ServiceInterface.h"
+#include "../datapool/PoolReadGuard.h"
 
 InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId,
         uint32_t messageQueueDepth): SystemObject(setObjectId),
-		commandQueue(QueueFactory::instance()->
-		        createMessageQueue(messageQueueDepth)),
-		poolManager(this, commandQueue),
-		internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID),
-		internalErrorDataset(this) {
-	mutex = MutexFactory::instance()->createMutex();
+        commandQueue(QueueFactory::instance()->
+                createMessageQueue(messageQueueDepth)),
+                poolManager(this, commandQueue),
+                internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID),
+                internalErrorDataset(this) {
+    mutex = MutexFactory::instance()->createMutex();
 }
 
 InternalErrorReporter::~InternalErrorReporter() {
-	MutexFactory::instance()->deleteMutex(mutex);
+    MutexFactory::instance()->deleteMutex(mutex);
 }
 
 void InternalErrorReporter::setDiagnosticPrintout(bool enable) {
@@ -23,126 +24,128 @@ void InternalErrorReporter::setDiagnosticPrintout(bool enable) {
 }
 
 ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
-    internalErrorDataset.read(timeoutType, timeoutMs);
+    CommandMessage message;
+    ReturnValue_t result = commandQueue->receiveMessage(&message);
+    if(result != MessageQueueIF::EMPTY) {
+        poolManager.handleHousekeepingMessage(&message);
+    }
 
-	uint32_t newQueueHits = getAndResetQueueHits();
-	uint32_t newTmHits = getAndResetTmHits();
-	uint32_t newStoreHits = getAndResetStoreHits();
+    uint32_t newQueueHits = getAndResetQueueHits();
+    uint32_t newTmHits = getAndResetTmHits();
+    uint32_t newStoreHits = getAndResetStoreHits();
 
-#if FSFW_VERBOSE_LEVEL == 1
-	if(diagnosticPrintout) {
-	    if((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) {
+#if FSFW_VERBOSE_LEVEL >= 1
+    if(diagnosticPrintout) {
+        if((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	        sif::debug << "InternalErrorReporter::performOperation: Errors "
-	                << "occured!" << std::endl;
-	        sif::debug << "Queue errors: " << newQueueHits << std::endl;
-	        sif::debug << "TM errors: " << newTmHits << std::endl;
-	        sif::debug << "Store errors: " << newStoreHits << std::endl;
+            sif::debug << "InternalErrorReporter::performOperation: Errors "
+                    << "occured!" << std::endl;
+            sif::debug << "Queue errors: " << newQueueHits << std::endl;
+            sif::debug << "TM errors: " << newTmHits << std::endl;
+            sif::debug << "Store errors: " << newStoreHits << std::endl;
 #else
-	        sif::printDebug("InternalErrorReporter::performOperation: Errors occured!\n");
-	        sif::printDebug("Queue errors: %lu\n", static_cast<unsigned int>(newQueueHits));
-	        sif::printDebug("TM errors: %lu\n", static_cast<unsigned int>(newTmHits));
-	        sif::printDebug("Store errors: %lu\n", static_cast<unsigned int>(newStoreHits));
+            sif::printDebug("InternalErrorReporter::performOperation: Errors occured!\n");
+            sif::printDebug("Queue errors: %lu\n", static_cast<unsigned int>(newQueueHits));
+            sif::printDebug("TM errors: %lu\n", static_cast<unsigned int>(newTmHits));
+            sif::printDebug("Store errors: %lu\n", static_cast<unsigned int>(newStoreHits));
 #endif
-	    }
-	}
+        }
+    }
 #endif
 
-	internalErrorDataset.queueHits.value += newQueueHits;
-	internalErrorDataset.storeHits.value += newStoreHits;
-	internalErrorDataset.tmHits.value += newTmHits;
-	internalErrorDataset.setValidity(true, true);
-	internalErrorDataset.commit(timeoutType, timeoutMs);
+    {
+        PoolReadGuard readGuard(&internalErrorDataset);
+        if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+            internalErrorDataset.queueHits.value += newQueueHits;
+            internalErrorDataset.storeHits.value += newStoreHits;
+            internalErrorDataset.tmHits.value += newTmHits;
+            internalErrorDataset.setValidity(true, true);
+        }
+    }
 
-	poolManager.performHkOperation();
-
-	CommandMessage message;
-	ReturnValue_t result = commandQueue->receiveMessage(&message);
-	if(result != MessageQueueIF::EMPTY) {
-	    poolManager.handleHousekeepingMessage(&message);
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+    poolManager.performHkOperation();
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void InternalErrorReporter::queueMessageNotSent() {
-	incrementQueueHits();
+    incrementQueueHits();
 }
 
 void InternalErrorReporter::lostTm() {
-	incrementTmHits();
+    incrementTmHits();
 }
 
 uint32_t InternalErrorReporter::getAndResetQueueHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = queueHits;
-	queueHits = 0;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = queueHits;
+    queueHits = 0;
+    mutex->unlockMutex();
+    return value;
 }
 
 uint32_t InternalErrorReporter::getQueueHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = queueHits;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = queueHits;
+    mutex->unlockMutex();
+    return value;
 }
 
 void InternalErrorReporter::incrementQueueHits() {
-	mutex->lockMutex(timeoutType, timeoutMs);
-	queueHits++;
-	mutex->unlockMutex();
+    mutex->lockMutex(timeoutType, timeoutMs);
+    queueHits++;
+    mutex->unlockMutex();
 }
 
 uint32_t InternalErrorReporter::getAndResetTmHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = tmHits;
-	tmHits = 0;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = tmHits;
+    tmHits = 0;
+    mutex->unlockMutex();
+    return value;
 }
 
 uint32_t InternalErrorReporter::getTmHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = tmHits;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = tmHits;
+    mutex->unlockMutex();
+    return value;
 }
 
 void InternalErrorReporter::incrementTmHits() {
-	mutex->lockMutex(timeoutType, timeoutMs);
-	tmHits++;
-	mutex->unlockMutex();
+    mutex->lockMutex(timeoutType, timeoutMs);
+    tmHits++;
+    mutex->unlockMutex();
 }
 
 void InternalErrorReporter::storeFull() {
-	incrementStoreHits();
+    incrementStoreHits();
 }
 
 uint32_t InternalErrorReporter::getAndResetStoreHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = storeHits;
-	storeHits = 0;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = storeHits;
+    storeHits = 0;
+    mutex->unlockMutex();
+    return value;
 }
 
 uint32_t InternalErrorReporter::getStoreHits() {
-	uint32_t value;
-	mutex->lockMutex(timeoutType, timeoutMs);
-	value = storeHits;
-	mutex->unlockMutex();
-	return value;
+    uint32_t value;
+    mutex->lockMutex(timeoutType, timeoutMs);
+    value = storeHits;
+    mutex->unlockMutex();
+    return value;
 }
 
 void InternalErrorReporter::incrementStoreHits() {
-	mutex->lockMutex(timeoutType, timeoutMs);
-	storeHits++;
-	mutex->unlockMutex();
+    mutex->lockMutex(timeoutType, timeoutMs);
+    storeHits++;
+    mutex->unlockMutex();
 }
 
 object_id_t InternalErrorReporter::getObjectId() const {
@@ -155,14 +158,11 @@ MessageQueueId_t InternalErrorReporter::getCommandQueue() const {
 
 ReturnValue_t InternalErrorReporter::initializeLocalDataPool(
         localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
-    localDataPoolMap.emplace(errorPoolIds::TM_HITS,
-            new PoolEntry<uint32_t>());
-    localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS,
-            new PoolEntry<uint32_t>());
-    localDataPoolMap.emplace(errorPoolIds::STORE_HITS,
-            new PoolEntry<uint32_t>());
-    poolManager.subscribeForPeriodicPacket(internalErrorSid, false,
-            getPeriodicOperationFrequency(), true);
+    localDataPoolMap.emplace(errorPoolIds::TM_HITS, new PoolEntry<uint32_t>());
+    localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS, new PoolEntry<uint32_t>());
+    localDataPoolMap.emplace(errorPoolIds::STORE_HITS, new PoolEntry<uint32_t>());
+    poolManager.subscribeForPeriodicPacket(internalErrorSid, false, getPeriodicOperationFrequency(),
+            true);
     internalErrorDataset.setValidity(true, true);
     return HasReturnvaluesIF::RETURN_OK;
 }
@@ -192,9 +192,9 @@ ReturnValue_t InternalErrorReporter::initializeAfterTaskCreation() {
 }
 
 void InternalErrorReporter::setMutexTimeout(MutexIF::TimeoutType timeoutType,
-		uint32_t timeoutMs) {
-	this->timeoutType = timeoutType;
-	this->timeoutMs = timeoutMs;
+        uint32_t timeoutMs) {
+    this->timeoutType = timeoutType;
+    this->timeoutMs = timeoutMs;
 }
 
 LocalDataPoolManager* InternalErrorReporter::getHkManagerHandle() {

internalError/InternalErrorReporter.h

@@ -17,77 +17,78 @@
  * All functions were kept virtual so this class can be extended easily
  * to store custom internal errors (e.g. communication interface errors).
  */
-class InternalErrorReporter: public SystemObject,
-		public ExecutableObjectIF,
-		public InternalErrorReporterIF,
-		public HasLocalDataPoolIF {
+class InternalErrorReporter:
+        public SystemObject,
+        public ExecutableObjectIF,
+        public InternalErrorReporterIF,
+        public HasLocalDataPoolIF {
 public:
 
-	InternalErrorReporter(object_id_t setObjectId,
-	        uint32_t messageQueueDepth = 5);
+    InternalErrorReporter(object_id_t setObjectId,
+            uint32_t messageQueueDepth = 5);
 
-	/**
-	 * Enable diagnostic printout. Please note that this feature will
-	 * only work if DEBUG has been supplied to the build defines.
-	 * @param enable
-	 */
-	void setDiagnosticPrintout(bool enable);
+    /**
+     * Enable diagnostic printout. Please note that this feature will
+     * only work if DEBUG has been supplied to the build defines.
+     * @param enable
+     */
+    void setDiagnosticPrintout(bool enable);
 
-	void setMutexTimeout(MutexIF::TimeoutType timeoutType,
-			uint32_t timeoutMs);
+    void setMutexTimeout(MutexIF::TimeoutType timeoutType,
+            uint32_t timeoutMs);
 
-	virtual ~InternalErrorReporter();
+    virtual ~InternalErrorReporter();
 
     virtual object_id_t getObjectId() const override;
     virtual MessageQueueId_t getCommandQueue() const override;
     virtual ReturnValue_t initializeLocalDataPool(
-    		localpool::DataPool& localDataPoolMap,
+            localpool::DataPool& localDataPoolMap,
             LocalDataPoolManager& poolManager) override;
     virtual dur_millis_t getPeriodicOperationFrequency() const override;
     virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override;
-	LocalDataPoolManager* getHkManagerHandle() override;
+    LocalDataPoolManager* getHkManagerHandle() override;
 
     virtual ReturnValue_t initialize() override;
     virtual ReturnValue_t initializeAfterTaskCreation() override;
-	virtual ReturnValue_t performOperation(uint8_t opCode) override;
+    virtual ReturnValue_t performOperation(uint8_t opCode) override;
 
-	virtual void queueMessageNotSent();
+    virtual void queueMessageNotSent();
 
-	virtual void lostTm();
+    virtual void lostTm();
 
-	virtual void storeFull();
+    virtual void storeFull();
 
-	virtual void setTaskIF(PeriodicTaskIF* task) override;
+    virtual void setTaskIF(PeriodicTaskIF* task) override;
 protected:
-	MessageQueueIF* commandQueue;
-	LocalDataPoolManager poolManager;
+    MessageQueueIF* commandQueue;
+    LocalDataPoolManager poolManager;
 
-	PeriodicTaskIF* executingTask = nullptr;
+    PeriodicTaskIF* executingTask = nullptr;
 
-	MutexIF* mutex = nullptr;
-	MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
-	uint32_t timeoutMs = 20;
+    MutexIF* mutex = nullptr;
+    MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+    uint32_t timeoutMs = 20;
 
-	sid_t internalErrorSid;
-	InternalErrorDataset internalErrorDataset;
+    sid_t internalErrorSid;
+    InternalErrorDataset internalErrorDataset;
 
-	bool diagnosticPrintout = true;
+    bool diagnosticPrintout = true;
 
-	uint32_t queueHits = 0;
-	uint32_t tmHits = 0;
-	uint32_t storeHits = 0;
+    uint32_t queueHits = 0;
+    uint32_t tmHits = 0;
+    uint32_t storeHits = 0;
 
-	uint32_t getAndResetQueueHits();
-	uint32_t getQueueHits();
-	void incrementQueueHits();
+    uint32_t getAndResetQueueHits();
+    uint32_t getQueueHits();
+    void incrementQueueHits();
 
-	uint32_t getAndResetTmHits();
-	uint32_t getTmHits();
-	void incrementTmHits();
+    uint32_t getAndResetTmHits();
+    uint32_t getTmHits();
+    void incrementTmHits();
 
-	uint32_t getAndResetStoreHits();
-	uint32_t getStoreHits();
-	void incrementStoreHits();
+    uint32_t getAndResetStoreHits();
+    uint32_t getStoreHits();
+    void incrementStoreHits();
 
 };
 

ipc/MessageQueueIF.h

@@ -27,7 +27,7 @@ public:
 	//! Returned if a reply method was called without partner
 	static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3);
 	//! Returned if the target destination is invalid.
-	static constexpr ReturnValue_t DESTINVATION_INVALID = MAKE_RETURN_CODE(4);
+	static constexpr ReturnValue_t DESTINATION_INVALID = MAKE_RETURN_CODE(4);
 
 	virtual ~MessageQueueIF() {}
 	/**

ipc/MessageQueueMessage.cpp

@@ -86,3 +86,7 @@ size_t MessageQueueMessage::getMaximumMessageSize() const {
     return this->MAX_MESSAGE_SIZE;
 }
 
+size_t MessageQueueMessage::getMaximumDataSize() const {
+    return this->MAX_DATA_SIZE;
+}
+

ipc/MessageQueueMessage.h

@@ -139,6 +139,7 @@ public:
 	virtual void setMessageSize(size_t messageSize) override;
 	virtual size_t getMinimumMessageSize() const override;
 	virtual size_t getMaximumMessageSize() const override;
+	virtual size_t getMaximumDataSize() const override;
 
 	/**
 	 * @brief	This is a debug method that prints the content.

ipc/MessageQueueMessageIF.h

@@ -72,6 +72,7 @@ public:
     virtual void setMessageSize(size_t messageSize) = 0;
     virtual size_t getMinimumMessageSize() const = 0;
     virtual size_t getMaximumMessageSize() const = 0;
+    virtual size_t getMaximumDataSize() const = 0;
 
 };
 

objectmanager/ObjectManager.cpp

@@ -108,9 +108,9 @@ void ObjectManager::initialize() {
 		result = it.second->checkObjectConnections();
 		if ( result != RETURN_OK ) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "ObjectManager::ObjectManager: Object " << std::hex <<
-					(int) it.first  << " connection check failed with code 0x"
-					<< result << std::dec << std::endl;
+			sif::error << "ObjectManager::ObjectManager: Object 0x" << std::hex <<
+					(int) it.first  << " connection check failed with code 0x" << result <<
+					std::dec << std::endl;
 #endif
 			errorCount++;
 		}

osal/CMakeLists.txt

@@ -1,35 +1,34 @@
 # Check the OS_FSFW variable
 if(${OS_FSFW} STREQUAL "freertos")
-	add_subdirectory(FreeRTOS)
+    add_subdirectory(FreeRTOS)
 elseif(${OS_FSFW} STREQUAL "rtems")
-	add_subdirectory(rtems)
+    add_subdirectory(rtems)
 elseif(${OS_FSFW} STREQUAL "linux")
-	add_subdirectory(linux)
+    add_subdirectory(linux)
 elseif(${OS_FSFW} STREQUAL "host")
-	add_subdirectory(host)
-	if (WIN32)
-		add_subdirectory(windows)
-	elseif(UNIX)
-		target_sources(${LIB_FSFW_NAME}
-			PUBLIC
-				linux/TcUnixUdpPollingTask.cpp
-				linux/TmTcUnixUdpBridge.cpp
-		)
-	endif ()
+    add_subdirectory(host)
+    if (WIN32)
+        add_subdirectory(windows)
+    elseif(UNIX)
+        # We still need to pull in some Linux specific sources
+        target_sources(${LIB_FSFW_NAME} PUBLIC
+            linux/tcpipHelpers.cpp
+        )
+    endif ()
 
 else()
 
-	message(WARNING "The OS_FSFW variable was not set. Assuming host OS..")
-	# Not set. Assumuing this is a host build, try to determine host OS
-	if (WIN32)
-		add_subdirectory(host)
-		add_subdirectory(windows)
-	elseif (UNIX)
-		add_subdirectory(linux)
-	else ()
-		# MacOS or other OSes have not been tested yet / are not supported.
-		message(FATAL_ERROR "The host OS could not be determined! Aborting.")
-	endif()
+    message(WARNING "The OS_FSFW variable was not set. Assuming host OS..")
+    # Not set. Assumuing this is a host build, try to determine host OS
+    if (WIN32)
+        add_subdirectory(host)
+        add_subdirectory(windows)
+    elseif (UNIX)
+        add_subdirectory(linux)
+    else ()
+        # MacOS or other OSes have not been tested yet / are not supported.
+        message(FATAL_ERROR "The host OS could not be determined! Aborting.")
+    endif()
 
 endif()
 

osal/FreeRTOS/Clock.cpp

@@ -111,7 +111,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from,
         timeval* to) {
-    struct tm time_tm;
+    struct tm time_tm = {};
 
     time_tm.tm_year = from->year - 1900;
     time_tm.tm_mon = from->month - 1;

osal/FreeRTOS/MessageQueue.cpp

@@ -135,7 +135,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	QueueHandle_t destination = nullptr;
 
 	if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) {
-		return MessageQueueIF::DESTINVATION_INVALID;
+		return MessageQueueIF::DESTINATION_INVALID;
 	}
 	else  {
 		destination = reinterpret_cast<QueueHandle_t>(sendTo);

osal/common/CMakeLists.txt

@@ -1,3 +1,16 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE 
-    tcpipCommon.cpp
-)
+if(DEFINED WIN32 OR DEFINED UNIX)
+    target_sources(${LIB_FSFW_NAME} PRIVATE 
+        tcpipCommon.cpp
+        TcpIpBase.cpp
+        UdpTcPollingTask.cpp
+        UdpTmTcBridge.cpp
+        TcpTmTcServer.cpp
+    )
+endif()
+
+if(WIN32)
+    target_link_libraries(${LIB_FSFW_NAME} PRIVATE
+        wsock32 
+        ws2_32
+    )
+endif()

osal/common/TcpIpBase.cpp

@@ -0,0 +1,54 @@
+#include "TcpIpBase.h"
+
+#ifdef __unix__
+
+#include <errno.h>
+#include <unistd.h>
+
+#endif
+
+TcpIpBase::TcpIpBase() {
+
+}
+
+ReturnValue_t TcpIpBase::initialize() {
+#ifdef _WIN32
+    /* Initiates Winsock DLL. */
+    WSAData wsaData;
+    WORD wVersionRequested = MAKEWORD(2, 2);
+    int err = WSAStartup(wVersionRequested, &wsaData);
+    if (err != 0) {
+        /* Tell the user that we could not find a usable Winsock DLL. */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: WSAStartup failed with error: " <<
+                err << std::endl;
+#endif
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+#endif
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+TcpIpBase::~TcpIpBase() {
+    closeSocket(serverSocket);
+#ifdef _WIN32
+    WSACleanup();
+#endif
+}
+
+int TcpIpBase::closeSocket(socket_t socket) {
+#ifdef _WIN32
+    return closesocket(socket);
+#elif defined(__unix__)
+    return close(socket);
+#endif
+}
+
+int TcpIpBase::getLastSocketError() {
+#ifdef _WIN32
+    return WSAGetLastError();
+#elif defined(__unix__)
+    return errno;
+#endif
+}
+

osal/common/TcpIpBase.h

@@ -0,0 +1,50 @@
+#ifndef FSFW_OSAL_COMMON_TCPIPIF_H_
+#define FSFW_OSAL_COMMON_TCPIPIF_H_
+
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+
+#ifdef _WIN32
+
+#include <winsock2.h>
+
+#elif defined(__unix__)
+
+#include <sys/socket.h>
+
+#endif
+
+class TcpIpBase {
+protected:
+
+#ifdef _WIN32
+    static constexpr int SHUT_RECV = SD_RECEIVE;
+    static constexpr int SHUT_SEND = SD_SEND;
+    static constexpr int SHUT_BOTH = SD_BOTH;
+
+    using socket_t = SOCKET;
+#elif defined(__unix__)
+    using socket_t = int;
+
+    static constexpr int INVALID_SOCKET = -1;
+    static constexpr int SOCKET_ERROR = -1;
+
+    static constexpr int SHUT_RECV = SHUT_RD;
+    static constexpr int SHUT_SEND = SHUT_WR;
+    static constexpr int SHUT_BOTH = SHUT_RDWR;
+#endif
+
+    TcpIpBase();
+    virtual ~TcpIpBase();
+
+    ReturnValue_t initialize();
+
+    int closeSocket(socket_t socket);
+
+    int getLastSocketError();
+
+    socket_t serverSocket = 0;
+
+};
+
+
+#endif /* FSFW_OSAL_COMMON_TCPIPIF_H_ */

osal/common/TcpTmTcServer.cpp

@@ -1,14 +1,21 @@
-#include "TcWinTcpServer.h"
+#include "TcpTmTcServer.h"
 #include "tcpipHelpers.h"
 #include "../../serviceinterface/ServiceInterface.h"
 
+#ifdef _WIN32
 #include <winsock2.h>
 #include <ws2tcpip.h>
 
-const std::string TcWinTcpServer::DEFAULT_TCP_SERVER_PORT =  "7301";
-const std::string TcWinTcpServer::DEFAULT_TCP_CLIENT_PORT =  "7302";
+#elif defined(__unix__)
 
-TcWinTcpServer::TcWinTcpServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+#include <netdb.h>
+
+#endif
+
+const std::string TcpTmTcServer::DEFAULT_TCP_SERVER_PORT =  "7301";
+const std::string TcpTmTcServer::DEFAULT_TCP_CLIENT_PORT =  "7302";
+
+TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
         std::string customTcpServerPort):
         SystemObject(objectId), tcpPort(customTcpServerPort) {
     if(tcpPort == "") {
@@ -16,25 +23,18 @@ TcWinTcpServer::TcWinTcpServer(object_id_t objectId, object_id_t tmtcUnixUdpBrid
     }
 }
 
-ReturnValue_t TcWinTcpServer::initialize() {
+ReturnValue_t TcpTmTcServer::initialize() {
     using namespace tcpip;
-    int retval = 0;
-    struct addrinfo *addrResult = nullptr;
-    struct addrinfo hints;
-    /* Initiates Winsock DLL. */
-    WSAData wsaData;
-    WORD wVersionRequested = MAKEWORD(2, 2);
-    int err = WSAStartup(wVersionRequested, &wsaData);
-    if (err != 0) {
-        /* Tell the user that we could not find a usable Winsock DLL. */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: WSAStartup failed with error: " <<
-                err << std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_FAILED;
+
+    ReturnValue_t result = TcpIpBase::initialize();
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
     }
 
-    ZeroMemory(&hints, sizeof (hints));
+    int retval = 0;
+    struct addrinfo *addrResult = nullptr;
+    struct addrinfo hints = {};
+
     hints.ai_family = AF_INET;
     hints.ai_socktype = SOCK_STREAM;
     hints.ai_protocol = IPPROTO_TCP;
@@ -43,7 +43,7 @@ ReturnValue_t TcWinTcpServer::initialize() {
     retval = getaddrinfo(nullptr, tcpPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TcWinTcpServer::TcWinTcpServer: Retrieving address info failed!" <<
+        sif::warning << "TcWinTcpServer::TcpTmTcServer: Retrieving address info failed!" <<
                 std::endl;
 #endif
         handleError(Protocol::TCP, ErrorSources::GETADDRINFO_CALL);
@@ -65,11 +65,12 @@ ReturnValue_t TcWinTcpServer::initialize() {
     retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval == SOCKET_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TcWinTcpServer::TcWinTcpServer: Binding socket failed!" <<
+        sif::warning << "TcWinTcpServer::TcpTmTcServer: Binding socket failed!" <<
                 std::endl;
 #endif
         freeaddrinfo(addrResult);
         handleError(Protocol::TCP, ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
 
     freeaddrinfo(addrResult);
@@ -77,17 +78,16 @@ ReturnValue_t TcWinTcpServer::initialize() {
 }
 
 
-TcWinTcpServer::~TcWinTcpServer() {
-    closesocket(listenerTcpSocket);
-    WSACleanup();
+TcpTmTcServer::~TcpTmTcServer() {
+    closeSocket(listenerTcpSocket);
 }
 
-ReturnValue_t TcWinTcpServer::performOperation(uint8_t opCode) {
+ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
     /* If a connection is accepted, the corresponding socket will be assigned to the new socket */
-    SOCKET clientSocket;
-    sockaddr_in clientSockAddr;
-    int connectorSockAddrLen = 0;
+    socket_t clientSocket = 0;
+    sockaddr clientSockAddr = {};
+    socklen_t connectorSockAddrLen = 0;
     int retval = 0;
 
     /* Listen for connection requests permanently for lifetime of program */
@@ -98,11 +98,11 @@ ReturnValue_t TcWinTcpServer::performOperation(uint8_t opCode) {
             continue;
         }
 
-        clientSocket = accept(listenerTcpSocket, reinterpret_cast<sockaddr*>(&clientSockAddr),
-                &connectorSockAddrLen);
+        clientSocket = accept(listenerTcpSocket, &clientSockAddr, &connectorSockAddrLen);
 
         if(clientSocket == INVALID_SOCKET) {
             handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500);
+            closeSocket(clientSocket);
             continue;
         };
 
@@ -110,7 +110,7 @@ ReturnValue_t TcWinTcpServer::performOperation(uint8_t opCode) {
                 receptionBuffer.size(), 0);
         if(retval > 0) {
 #if FSFW_TCP_RCV_WIRETAPPING_ENABLED == 1
-            sif::info << "TcWinTcpServer::performOperation: Received " << retval << " bytes."
+            sif::info << "TcpTmTcServer::performOperation: Received " << retval << " bytes."
                     std::endl;
 #endif
             handleError(Protocol::TCP, ErrorSources::RECV_CALL, 500);
@@ -123,7 +123,8 @@ ReturnValue_t TcWinTcpServer::performOperation(uint8_t opCode) {
         }
 
         /* Done, shut down connection */
-        retval = shutdown(clientSocket, SD_SEND);
+        retval = shutdown(clientSocket, SHUT_SEND);
+        closeSocket(clientSocket);
     }
     return HasReturnvaluesIF::RETURN_OK;
 }

osal/common/TcpTmTcServer.h

@@ -1,9 +1,14 @@
 #ifndef FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
 #define FSFW_OSAL_WINDOWS_TCWINTCPSERVER_H_
 
+#include "TcpIpBase.h"
 #include "../../objectmanager/SystemObject.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
+#ifdef __unix__
+#include <sys/socket.h>
+#endif
+
 #include <string>
 #include <vector>
 
@@ -15,17 +20,18 @@
  * @details
  * Based on: https://docs.microsoft.com/en-us/windows/win32/winsock/complete-server-code
  */
-class TcWinTcpServer:
+class TcpTmTcServer:
         public SystemObject,
+        public TcpIpBase,
         public ExecutableObjectIF {
 public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_TCP_SERVER_PORT;
     static const std::string DEFAULT_TCP_CLIENT_PORT;
 
-    TcWinTcpServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+    TcpTmTcServer(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
             std::string customTcpServerPort = "");
-    virtual~ TcWinTcpServer();
+    virtual~ TcpTmTcServer();
 
     ReturnValue_t initialize() override;
     ReturnValue_t performOperation(uint8_t opCode) override;
@@ -33,8 +39,8 @@ public:
 private:
 
     std::string tcpPort;
-    SOCKET listenerTcpSocket = 0;
-    struct sockaddr_in tcpAddress;
+    socket_t listenerTcpSocket = 0;
+    struct sockaddr tcpAddress;
     int tcpAddrLen = sizeof(tcpAddress);
     int currentBacklog = 3;
 

osal/common/UdpTcPollingTask.cpp

@@ -0,0 +1,180 @@
+#include "UdpTcPollingTask.h"
+#include "tcpipHelpers.h"
+#include "../../globalfunctions/arrayprinter.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+
+#ifdef _WIN32
+
+#include <winsock2.h>
+
+#else
+
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#endif
+
+//! Debugging preprocessor define.
+#define FSFW_UDP_RECV_WIRETAPPING_ENABLED    0
+
+UdpTcPollingTask::UdpTcPollingTask(object_id_t objectId,
+        object_id_t tmtcUnixUdpBridge, size_t maxRecvSize,
+        double timeoutSeconds): SystemObject(objectId),
+        tmtcBridgeId(tmtcUnixUdpBridge) {
+    if(frameSize > 0) {
+        this->frameSize = frameSize;
+    }
+    else {
+        this->frameSize = DEFAULT_MAX_RECV_SIZE;
+    }
+
+    /* Set up reception buffer with specified frame size.
+	For now, it is assumed that only one frame is held in the buffer! */
+    receptionBuffer.reserve(this->frameSize);
+    receptionBuffer.resize(this->frameSize);
+
+    if(timeoutSeconds == -1) {
+        receptionTimeout = DEFAULT_TIMEOUT;
+    }
+    else {
+        receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
+    }
+}
+
+UdpTcPollingTask::~UdpTcPollingTask() {}
+
+ReturnValue_t UdpTcPollingTask::performOperation(uint8_t opCode) {
+    /* Sender Address is cached here. */
+    struct sockaddr senderAddress;
+    socklen_t senderAddressSize = sizeof(senderAddress);
+
+    /* Poll for new UDP datagrams in permanent loop. */
+    while(true) {
+        int bytesReceived = recvfrom(
+                this->serverSocket,
+                reinterpret_cast<char*>(receptionBuffer.data()),
+                frameSize,
+                receptionFlags,
+                &senderAddress,
+                &senderAddressSize
+        );
+        if(bytesReceived == SOCKET_ERROR) {
+            /* Handle error */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "UdpTcPollingTask::performOperation: Reception error." << std::endl;
+#endif
+            tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 1000);
+            continue;
+        }
+#if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::debug << "UdpTcPollingTask::performOperation: " << bytesReceived <<
+                " bytes received" << std::endl;
+#else
+#endif
+#endif /* FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1 */
+
+        ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
+        if(result != HasReturnvaluesIF::RETURN_FAILED) {
+
+        }
+        tmtcBridge->checkAndSetClientAddress(senderAddress);
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+
+ReturnValue_t UdpTcPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
+    store_address_t storeId;
+
+#if FSFW_UDP_RECV_WIRETAPPING_ENABLED == 1
+    arrayprinter::print(receptionBuffer.data(), bytesRead);
+#endif
+
+    ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRead);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning<< "UdpTcPollingTask::transferPusToSoftwareBus: Data storage failed." <<
+                std::endl;
+        sif::warning << "Packet size: " << bytesRead << std::endl;
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    TmTcMessage message(storeId);
+
+    result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "UdpTcPollingTask::handleSuccessfullTcRead: "
+                " Sending message to queue failed" << std::endl;
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        tcStore->deleteData(storeId);
+    }
+    return result;
+}
+
+ReturnValue_t UdpTcPollingTask::initialize() {
+    tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
+    if (tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "UdpTcPollingTask::initialize: TC store uninitialized!" << std::endl;
+#endif
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    tmtcBridge = objectManager->get<UdpTmTcBridge>(tmtcBridgeId);
+    if(tmtcBridge == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "UdpTcPollingTask::initialize: Invalid TMTC bridge object!" <<
+                std::endl;
+#endif
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    ReturnValue_t result = TcpIpBase::initialize();
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t UdpTcPollingTask::initializeAfterTaskCreation() {
+    /* Initialize the destination after task creation. This ensures
+	that the destination has already been set in the TMTC bridge. */
+    targetTcDestination = tmtcBridge->getRequestQueue();
+    /* The server socket is set up in the bridge intialization. Calling this function here
+	ensures that it is set up regardless of which class was initialized first */
+    this->serverSocket = tmtcBridge->serverSocket;
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void UdpTcPollingTask::setTimeout(double timeoutSeconds) {
+#ifdef _WIN32
+    DWORD timeoutMs = timeoutSeconds * 1000.0;
+    int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO,
+            reinterpret_cast<const char*>(&timeoutMs), sizeof(DWORD));
+    if(result == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TcWinUdpPollingTask::TcSocketPollingTask: Setting "
+                "receive timeout failed with " << strerror(errno) << std::endl;
+#endif
+    }
+#elif defined(__unix__)
+    timeval tval;
+    tval = timevalOperations::toTimeval(timeoutSeconds);
+    int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO,
+            &tval, sizeof(receptionTimeout));
+    if(result == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
+                "receive timeout failed with " << strerror(errno) << std::endl;
+#endif
+    }
+#endif
+}

osal/common/UdpTcPollingTask.h

@@ -1,7 +1,7 @@
 #ifndef FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_
 #define FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_
 
-#include "TmTcWinUdpBridge.h"
+#include "UdpTmTcBridge.h"
 #include "../../objectmanager/SystemObject.h"
 #include "../../tasks/ExecutableObjectIF.h"
 #include "../../storagemanager/StorageManagerIF.h"
@@ -9,25 +9,22 @@
 #include <vector>
 
 /**
- * @brief 	This class can be used to implement the polling of a Unix socket,
- * 			using UDP for now.
- * @details
- * The task will be blocked while the specified number of bytes has not been
- * received, so TC reception is handled inside a separate task.
- * This class caches the IP address of the sender. It is assumed there
- * is only one sender for now.
+ * @brief   This class should be used with the UdpTmTcBridge to implement a UDP server
+ *          for receiving and sending PUS TMTC.
  */
-class TcWinUdpPollingTask: public SystemObject,
+class UdpTcPollingTask:
+        public TcpIpBase,
+        public SystemObject,
 		public ExecutableObjectIF {
 	friend class TmTcWinUdpBridge;
 public:
-	static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048;
+	static constexpr size_t DEFAULT_MAX_RECV_SIZE = 1500;
 	//! 0.5  default milliseconds timeout for now.
 	static constexpr timeval DEFAULT_TIMEOUT = {0, 500};
 
-	TcWinUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
-			size_t frameSize = 0, double timeoutSeconds = -1);
-	virtual~ TcWinUdpPollingTask();
+	UdpTcPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
+			size_t maxRecvSize = 0, double timeoutSeconds = -1);
+	virtual~ UdpTcPollingTask();
 
 	/**
 	 * Turn on optional timeout for UDP polling. In the default mode,
@@ -46,16 +43,12 @@ protected:
 private:
 	//! TMTC bridge is cached.
 	object_id_t tmtcBridgeId = objects::NO_OBJECT;
-	TmTcWinUdpBridge* tmtcBridge = nullptr;
+	UdpTmTcBridge* tmtcBridge = nullptr;
 	MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
 
 	//! See: https://docs.microsoft.com/en-us/windows/win32/api/winsock/nf-winsock-recvfrom
 	int receptionFlags = 0;
 
-	//! Server socket, which is member of TMTC bridge.
-	//! Will be cached shortly after SW intialization.
-	SOCKET serverUdpSocket = 0;
-
 	std::vector<uint8_t> receptionBuffer;
 
 	size_t frameSize = 0;

osal/common/UdpTmTcBridge.cpp

@@ -1,16 +1,26 @@
-#include "TmTcWinUdpBridge.h"
 #include "tcpipHelpers.h"
 
 #include <fsfw/serviceinterface/ServiceInterface.h>
 #include <fsfw/ipc/MutexGuard.h>
+#include <fsfw/osal/common/UdpTmTcBridge.h>
+
+#ifdef _WIN32
+
 #include <ws2tcpip.h>
 
+#elif defined(__unix__)
+
+#include <netdb.h>
+#include <arpa/inet.h>
+
+#endif
+
 //! Debugging preprocessor define.
 #define FSFW_UDP_SEND_WIRETAPPING_ENABLED    0
 
-const std::string TmTcWinUdpBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_UDP_SERVER_PORT;
+const std::string UdpTmTcBridge::DEFAULT_UDP_SERVER_PORT =  tcpip::DEFAULT_SERVER_PORT;
 
-TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId, object_id_t tcDestination,
+UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
         object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort):
         TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
     if(udpServerPort == "") {
@@ -24,16 +34,18 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId, object_id_t tcDestinati
     communicationLinkUp = false;
 }
 
-ReturnValue_t TmTcWinUdpBridge::initialize() {
+ReturnValue_t UdpTmTcBridge::initialize() {
     ReturnValue_t result = TmTcBridge::initialize();
     if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcWinUdpBridge::initialize: TmTcBridge initialization failed!"
+        sif::error << "TmTcUdpBridge::initialize: TmTcBridge initialization failed!"
                 << std::endl;
 #endif
         return result;
     }
 
+
+#ifdef _WIN32
     /* Initiates Winsock DLL. */
     WSAData wsaData;
     WORD wVersionRequested = MAKEWORD(2, 2);
@@ -42,25 +54,22 @@ ReturnValue_t TmTcWinUdpBridge::initialize() {
         /* Tell the user that we could not find a usable */
         /* Winsock DLL.                                  */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: WSAStartup failed with error: " <<
+        sif::error << "TmTcUdpBridge::TmTcUdpBridge: WSAStartup failed with error: " <<
                 err << std::endl;
 #else
-        sif::printError("TmTcWinUdpBridge::TmTcWinUdpBridge: WSAStartup failed with error: %d\n",
+        sif::printError("TmTcUdpBridge::TmTcUdpBridge: WSAStartup failed with error: %d\n",
                 err);
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
     }
+#endif
 
     struct addrinfo *addrResult = nullptr;
-    struct addrinfo hints;
+    struct addrinfo hints = {};
 
-    ZeroMemory(&hints, sizeof (hints));
     hints.ai_family = AF_INET;
     hints.ai_socktype = SOCK_DGRAM;
     hints.ai_protocol = IPPROTO_UDP;
-    /* See:
-    https://docs.microsoft.com/en-us/windows/win32/api/ws2tcpip/nf-ws2tcpip-getaddrinfo
-    for information about AI_PASSIVE. */
     hints.ai_flags = AI_PASSIVE;
 
     /* Set up UDP socket:
@@ -70,7 +79,7 @@ ReturnValue_t TmTcWinUdpBridge::initialize() {
     int retval = getaddrinfo(nullptr, udpServerPort.c_str(), &hints, &addrResult);
     if (retval != 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcWinUdpBridge::TmTcWinUdpBridge: Retrieving address info failed!" <<
+        sif::warning << "TmTcUdpBridge::TmTcUdpBridge: Retrieving address info failed!" <<
                 std::endl;
 #endif
         return HasReturnvaluesIF::RETURN_FAILED;
@@ -79,7 +88,7 @@ ReturnValue_t TmTcWinUdpBridge::initialize() {
     serverSocket = socket(addrResult->ai_family, addrResult->ai_socktype, addrResult->ai_protocol);
     if(serverSocket == INVALID_SOCKET) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not open UDP socket!" <<
+        sif::warning << "TmTcUdpBridge::TmTcUdpBridge: Could not open UDP socket!" <<
                 std::endl;
 #endif
         freeaddrinfo(addrResult);
@@ -87,37 +96,38 @@ ReturnValue_t TmTcWinUdpBridge::initialize() {
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(addrResult->ai_addr);
+#endif
+
     retval = bind(serverSocket, addrResult->ai_addr, static_cast<int>(addrResult->ai_addrlen));
     if(retval != 0) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not bind "
+        sif::error << "TmTcUdpBridge::TmTcUdpBridge: Could not bind "
                 "local port (" << udpServerPort << ") to server socket!" << std::endl;
 #endif
         freeaddrinfo(addrResult);
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     freeaddrinfo(addrResult);
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-TmTcWinUdpBridge::~TmTcWinUdpBridge() {
+UdpTmTcBridge::~UdpTmTcBridge() {
     if(mutex != nullptr) {
         MutexFactory::instance()->deleteMutex(mutex);
     }
-    closesocket(serverSocket);
-    WSACleanup();
 }
 
-ReturnValue_t TmTcWinUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
+ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) {
     int flags = 0;
 
     /* The target address can be set by different threads so this lock ensures thread-safety */
     MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
-            &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(&clientAddress);
 #endif
 
     int bytesSent = sendto(
@@ -125,33 +135,31 @@ ReturnValue_t TmTcWinUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
             reinterpret_cast<const char*>(data),
             dataLen,
             flags,
-            reinterpret_cast<sockaddr*>(&clientAddress),
+            &clientAddress,
             clientAddressLen
     );
     if(bytesSent == SOCKET_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "TmTcWinUdpBridge::sendTm: Send operation failed." << std::endl;
+        sif::warning << "TmTcUdpBridge::sendTm: Send operation failed." << std::endl;
 #endif
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::SENDTO_CALL);
     }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
+    sif::debug << "TmTcUdpBridge::sendTm: " << bytesSent << " bytes were"
             " sent." << std::endl;
 #endif
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-void TmTcWinUdpBridge::checkAndSetClientAddress(sockaddr_in& newAddress) {
+void UdpTmTcBridge::checkAndSetClientAddress(sockaddr& newAddress) {
     /* The target address can be set by different threads so this lock ensures thread-safety */
     MutexGuard lock(mutex, timeoutType, mutexTimeoutMs);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
-    char ipAddress [15];
-    sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
-            &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
-    sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
-            &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#if FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1
+    tcpip::printAddress(&newAddress);
+    tcpip::printAddress(&clientAddress);
 #endif
+
     registerCommConnect();
 
     /* Set new IP address to reply to */
@@ -159,7 +167,7 @@ void TmTcWinUdpBridge::checkAndSetClientAddress(sockaddr_in& newAddress) {
     clientAddressLen = sizeof(clientAddress);
 }
 
-void TmTcWinUdpBridge::setMutexProperties(MutexIF::TimeoutType timeoutType,
+void UdpTmTcBridge::setMutexProperties(MutexIF::TimeoutType timeoutType,
         dur_millis_t timeoutMs) {
     this->timeoutType = timeoutType;
     this->mutexTimeoutMs = timeoutMs;

osal/common/UdpTmTcBridge.h

@@ -1,20 +1,36 @@
 #ifndef FSFW_OSAL_WINDOWS_TMTCWINUDPBRIDGE_H_
 #define FSFW_OSAL_WINDOWS_TMTCWINUDPBRIDGE_H_
 
+#include "TcpIpBase.h"
 #include "../../tmtcservices/TmTcBridge.h"
 
-#include <string>
-#include <winsock2.h>
+#ifdef _WIN32
 
-class TmTcWinUdpBridge: public TmTcBridge {
-    friend class TcWinUdpPollingTask;
+#include <ws2tcpip.h>
+
+#elif defined(__unix__)
+
+#include <sys/socket.h>
+
+#endif
+
+#include <string>
+
+/**
+ * @brief   This class should be used with the UdpTcPollingTask to implement a UDP server
+ *          for receiving and sending PUS TMTC.
+ */
+class UdpTmTcBridge:
+        public TmTcBridge,
+        public TcpIpBase {
+    friend class UdpTcPollingTask;
 public:
     /* The ports chosen here should not be used by any other process. */
     static const std::string DEFAULT_UDP_SERVER_PORT;
 
-    TmTcWinUdpBridge(object_id_t objectId, object_id_t tcDestination,
+    UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
             object_id_t tmStoreId, object_id_t tcStoreId, std::string udpServerPort = "");
-    virtual~ TmTcWinUdpBridge();
+    virtual~ UdpTmTcBridge();
 
     /**
      * Set properties of internal mutex.
@@ -23,17 +39,16 @@ public:
 
     ReturnValue_t initialize() override;
 
-    void checkAndSetClientAddress(sockaddr_in& clientAddress);
+    void checkAndSetClientAddress(sockaddr& clientAddress);
 
 protected:
     virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
 
 private:
-    SOCKET serverSocket = 0;
     std::string udpServerPort;
 
-    struct sockaddr_in clientAddress;
-    int clientAddressLen = 0;
+    struct sockaddr clientAddress;
+    socklen_t clientAddressLen = 0;
 
     //! Access to the client address is mutex protected as it is set by another task.
     MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;

osal/common/tcpipCommon.cpp

@@ -1,4 +1,9 @@
 #include "tcpipCommon.h"
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#ifdef _WIN32
+#include <ws2tcpip.h>
+#endif
 
 void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string &protStr,
         std::string &srcString) {
@@ -34,3 +39,37 @@ void tcpip::determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std:
         srcString = "unknown call";
     }
 }
+
+void tcpip::printAddress(struct sockaddr* addr) {
+    char ipAddress[INET6_ADDRSTRLEN] = {};
+    const char* stringPtr = NULL;
+    switch(addr->sa_family) {
+    case AF_INET: {
+        struct sockaddr_in *addrIn = reinterpret_cast<struct sockaddr_in*>(addr);
+        stringPtr = inet_ntop(AF_INET, &(addrIn->sin_addr), ipAddress, INET_ADDRSTRLEN);
+        break;
+    }
+    case AF_INET6: {
+        struct sockaddr_in6 *addrIn = reinterpret_cast<struct sockaddr_in6*>(addr);
+        stringPtr = inet_ntop(AF_INET6, &(addrIn->sin6_addr), ipAddress, INET6_ADDRSTRLEN);
+        break;
+    }
+    }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(stringPtr == NULL) {
+        sif::debug << "Could not convert IP address to text representation, error code "
+                << errno << std::endl;
+    }
+    else {
+        sif::debug << "IP Address Sender: " <<  ipAddress << std::endl;
+    }
+#else
+    if(stringPtr == NULL) {
+        sif::printDebug("Could not convert IP address to text representation, error code %d\n",
+                errno);
+    }
+    else {
+        sif::printDebug("IP Address Sender: %s\n", ipAddress);
+    }
+#endif
+}

osal/common/tcpipCommon.h

@@ -4,10 +4,16 @@
 #include "../../timemanager/clockDefinitions.h"
 #include <string>
 
+#ifdef _WIN32
+#include <winsock2.h>
+#else
+#include <netdb.h>
+#include <arpa/inet.h>
+#endif
+
 namespace tcpip {
 
-const char* const DEFAULT_UDP_SERVER_PORT = "7301";
-const char* const DEFAULT_TCP_SERVER_PORT = "7303";
+const char* const DEFAULT_SERVER_PORT = "7301";
 
 enum class Protocol {
     UDP,
@@ -29,8 +35,8 @@ enum class ErrorSources {
 void determineErrorStrings(Protocol protocol, ErrorSources errorSrc, std::string& protStr,
         std::string& srcString);
 
+void printAddress(struct sockaddr* addr);
+
 }
 
-
-
 #endif /* FSFW_OSAL_COMMON_TCPIPCOMMON_H_ */

osal/common/tcpipHelpers.h

@@ -2,7 +2,7 @@
 #define FSFW_OSAL_WINDOWS_TCPIPHELPERS_H_
 
 #include "../../timemanager/clockDefinitions.h"
-#include "../common/tcpipCommon.h"
+#include "tcpipCommon.h"
 
 namespace tcpip {
 

osal/host/CMakeLists.txt

@@ -10,6 +10,7 @@ target_sources(${LIB_FSFW_NAME}
 		QueueMapManager.cpp
 		SemaphoreFactory.cpp
 		TaskFactory.cpp
+		taskHelpers.cpp
 )
 
 if(UNIX)

osal/host/FixedTimeslotTask.cpp

@@ -1,10 +1,9 @@
+#include "taskHelpers.h"
 #include "../../osal/host/FixedTimeslotTask.h"
-
 #include "../../ipc/MutexFactory.h"
 #include "../../osal/host/Mutex.h"
 #include "../../osal/host/FixedTimeslotTask.h"
-
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/ExecutableObjectIF.h"
 
 #include <thread>
@@ -12,6 +11,7 @@
 
 #if defined(WIN32)
 #include <windows.h>
+#include "../windows/winTaskHelpers.h"
 #elif defined(LINUX)
 #include <pthread.h>
 #endif
@@ -24,34 +24,12 @@ FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority,
     // It is propably possible to set task priorities by using the native
     // task handles for Windows / Linux
     mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this);
-#if defined(WIN32)
-    /* List of possible priority classes:
-     * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/
-     * nf-processthreadsapi-setpriorityclass
-     * And respective thread priority numbers:
-     * https://docs.microsoft.com/en-us/windows/
-     * win32/procthread/scheduling-priorities */
-    int result = SetPriorityClass(
-            reinterpret_cast<HANDLE>(mainThread.native_handle()),
-            ABOVE_NORMAL_PRIORITY_CLASS);
-    if(result != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
-                << GetLastError() << std::endl;
-#endif
-    }
-    result = SetThreadPriority(
-            reinterpret_cast<HANDLE>(mainThread.native_handle()),
-            THREAD_PRIORITY_NORMAL);
-    if(result != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
-                << GetLastError() << std::endl;
-#endif
-    }
-#elif defined(LINUX)
-    // TODO: we can just copy and paste the code from the linux OSAL here.
+#if defined(_WIN32)
+    tasks::setTaskPriority(reinterpret_cast<HANDLE>(mainThread.native_handle()), setPriority);
+#elif defined(__unix__)
+    // TODO: We could reuse existing code here.
 #endif
+    tasks::insertTaskName(mainThread.get_id(), taskName);
 }
 
 FixedTimeslotTask::~FixedTimeslotTask(void) {
@@ -60,7 +38,6 @@ FixedTimeslotTask::~FixedTimeslotTask(void) {
     if(mainThread.joinable()) {
         mainThread.join();
     }
-    delete this;
 }
 
 void FixedTimeslotTask::taskEntryPoint(void* argument) {
@@ -141,8 +118,11 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
     }
 
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "Component " << std::hex << componentId <<
-            " not found, not adding it to pst" << std::endl;
+    sif::error << "Component " << std::hex << "0x" << componentId << "not found, "
+            "not adding it to PST.." << std::dec << std::endl;
+#else
+    sif::printError("Component 0x%08x not found, not adding it to PST..\n",
+            static_cast<unsigned int>(componentId));
 #endif
     return HasReturnvaluesIF::RETURN_FAILED;
 }

osal/host/MessageQueue.cpp

@@ -63,12 +63,9 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 	if(messageQueue.empty()) {
 		return MessageQueueIF::EMPTY;
 	}
-	// not sure this will work..
-	//*message = std::move(messageQueue.front());
 	MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
-	MessageQueueMessage* currentMessage = &messageQueue.front();
-	std::copy(currentMessage->getBuffer(),
-			currentMessage->getBuffer() + messageSize, message->getBuffer());
+	std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
+	        message->getBuffer());
 	messageQueue.pop();
 	// The last partner is the first uint32_t field in the message
 	this->lastPartner = message->getSender();
@@ -82,7 +79,7 @@ MessageQueueId_t MessageQueue::getLastPartner() const {
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
 	*count = messageQueue.size();
 	// Clears the queue.
-	messageQueue = std::queue<MessageQueueMessage>();
+	messageQueue = std::queue<std::vector<uint8_t>>();
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -108,6 +105,9 @@ bool MessageQueue::isDefaultDestinationSet() const {
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 	message->setSender(sentFrom);
 	if(message->getMessageSize() > message->getMaximumMessageSize()) {
 		// Actually, this should never happen or an error will be emitted
@@ -130,21 +130,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
-		MutexGuard mutexLock(targetQueue->queueLock,
-		        MutexIF::TimeoutType::WAITING, 20);
-		// not ideal, works for now though.
-		MessageQueueMessage* mqmMessage =
-				dynamic_cast<MessageQueueMessage*>(message);
-		if(message != nullptr) {
-			targetQueue->messageQueue.push(*mqmMessage);
-		}
-		else {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
-					"is not MessageQueueMessage!" << std::endl;
-#endif
-		}
-
+		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
+		targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
+		memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
+		        message->getMaximumMessageSize());
 	}
 	else {
 		return MessageQueueIF::FULL;

osal/host/MessageQueue.h

@@ -212,7 +212,7 @@ protected:
 	//static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
 
 private:
-	std::queue<MessageQueueMessage> messageQueue;
+	std::queue<std::vector<uint8_t>> messageQueue;
 	/**
 	 * @brief	The class stores the queue id it got assigned.
 	 * 			If initialization fails, the queue id is set to zero.

osal/host/PeriodicTask.cpp

@@ -1,5 +1,6 @@
 #include "Mutex.h"
 #include "PeriodicTask.h"
+#include "taskHelpers.h"
 
 #include "../../ipc/MutexFactory.h"
 #include "../../serviceinterface/ServiceInterfaceStream.h"
@@ -10,7 +11,8 @@
 
 #if defined(WIN32)
 #include <processthreadsapi.h>
-#elif defined(LINUX)
+#include <fsfw/osal/windows/winTaskHelpers.h>
+#elif defined(__unix__)
 #include <pthread.h>
 #endif
 
@@ -19,37 +21,15 @@ PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
 		void (*setDeadlineMissedFunc)()) :
 		started(false), taskName(name), period(setPeriod),
 		deadlineMissedFunc(setDeadlineMissedFunc) {
-    // It is propably possible to set task priorities by using the native
+    // It is probably possible to set task priorities by using the native
     // task handles for Windows / Linux
 	mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this);
-#if defined(WIN32)
-    /* List of possible priority classes:
-     * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/
-     * nf-processthreadsapi-setpriorityclass
-     * And respective thread priority numbers:
-     * https://docs.microsoft.com/en-us/windows/
-     * win32/procthread/scheduling-priorities */
-    int result = SetPriorityClass(
-            reinterpret_cast<HANDLE>(mainThread.native_handle()),
-            ABOVE_NORMAL_PRIORITY_CLASS);
-    if(result != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
-                << GetLastError() << std::endl;
-#endif
-    }
-    result = SetThreadPriority(
-            reinterpret_cast<HANDLE>(mainThread.native_handle()),
-            THREAD_PRIORITY_NORMAL);
-    if(result != 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
-                << GetLastError() << std::endl;
-#endif
-    }
-#elif defined(LINUX)
-    // we can just copy and paste the code from linux here.
+#if defined(_WIN32)
+	tasks::setTaskPriority(reinterpret_cast<HANDLE>(mainThread.native_handle()), setPriority);
+#elif defined(__unix__)
+    // TODO: We could reuse existing code here.
 #endif
+    tasks::insertTaskName(mainThread.get_id(), taskName);
 }
 
 PeriodicTask::~PeriodicTask(void) {
@@ -58,7 +38,6 @@ PeriodicTask::~PeriodicTask(void) {
 	if(mainThread.joinable()) {
 		mainThread.join();
 	}
-	delete this;
 }
 
 void PeriodicTask::taskEntryPoint(void* argument) {

osal/host/QueueMapManager.cpp

@@ -10,6 +10,10 @@ QueueMapManager::QueueMapManager() {
     mapLock = MutexFactory::instance()->createMutex();
 }
 
+QueueMapManager::~QueueMapManager() {
+    MutexFactory::instance()->deleteMutex(mapLock);
+}
+
 QueueMapManager* QueueMapManager::instance() {
 	if (mqManagerInstance == nullptr){
 		mqManagerInstance = new QueueMapManager();

osal/host/QueueMapManager.h

@@ -36,6 +36,8 @@ public:
 private:
 	//! External instantiation is forbidden.
 	QueueMapManager();
+	~QueueMapManager();
+
 	uint32_t queueCounter = 0;
 	MutexIF* mapLock;
 	QueueMap queueMap;

osal/host/TaskFactory.cpp

@@ -1,14 +1,16 @@
+#include "taskHelpers.h"
+#include "../../tasks/TaskFactory.h"
 #include "../../osal/host/FixedTimeslotTask.h"
 #include "../../osal/host/PeriodicTask.h"
-#include "../../tasks/TaskFactory.h"
 #include "../../returnvalues/HasReturnvaluesIF.h"
 #include "../../tasks/PeriodicTaskIF.h"
+#include "../../serviceinterface/ServiceInterface.h"
 
 #include <chrono>
 
 TaskFactory* TaskFactory::factoryInstance = new TaskFactory();
 
-// Will propably not be used for hosted implementation
+// Not used for the host implementation for now because C++ thread abstraction is used
 const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0;
 
 TaskFactory::TaskFactory() {
@@ -49,8 +51,12 @@ ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){
 }
 
 void TaskFactory::printMissedDeadline() {
-    /* TODO: Implement */
-    return;
+    std::string name = tasks::getTaskName(std::this_thread::get_id());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::warning << "TaskFactory::printMissedDeadline: " << name << std::endl;
+#else
+    sif::printWarning("TaskFactory::printMissedDeadline: %s\n", name);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 }
 
 

osal/host/taskHelpers.cpp

@@ -0,0 +1,27 @@
+#include "taskHelpers.h"
+#include <map>
+#include <mutex>
+
+std::mutex nameMapLock;
+std::map<std::thread::id, std::string> taskNameMap;
+
+ReturnValue_t tasks::insertTaskName(std::thread::id threadId, std::string taskName) {
+    std::lock_guard<std::mutex> lg(nameMapLock);
+    auto returnPair = taskNameMap.emplace(threadId, taskName);
+    if(not returnPair.second) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+std::string tasks::getTaskName(std::thread::id threadId) {
+    std::lock_guard<std::mutex> lg(nameMapLock);
+    auto resultIter = taskNameMap.find(threadId);
+    if(resultIter != taskNameMap.end()) {
+        return resultIter->second;
+    }
+    else {
+        return "Unknown task";
+    }
+}
+

osal/host/taskHelpers.h

@@ -0,0 +1,16 @@
+#ifndef FSFW_OSAL_HOST_TASKHELPERS_H_
+#define FSFW_OSAL_HOST_TASKHELPERS_H_
+
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <thread>
+
+namespace tasks {
+
+ReturnValue_t insertTaskName(std::thread::id threadId, std::string taskName);
+std::string getTaskName(std::thread::id threadId);
+
+}
+
+
+
+#endif /* FSFW_OSAL_HOST_TASKHELPERS_H_ */

osal/linux/CMakeLists.txt

@@ -13,8 +13,6 @@ target_sources(${LIB_FSFW_NAME}
 		QueueFactory.cpp
 		SemaphoreFactory.cpp
 		TaskFactory.cpp
-		TcUnixUdpPollingTask.cpp
-		TmTcUnixUdpBridge.cpp
 		Timer.cpp
 		tcpipHelpers.cpp
 )

osal/linux/MessageQueue.cpp

@@ -190,13 +190,15 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 		return HasReturnvaluesIF::RETURN_OK;
-	}else if(status==0){
+	}
+	else if (status==0) {
 		//Success but no message received
 		return MessageQueueIF::EMPTY;
-	} else {
+	}
+	else {
 		//No message was received. Keep lastPartner anyway, I might send
 		//something later. But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 		switch(errno){
 		case EAGAIN:
 			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
@@ -371,7 +373,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 					<<"mq_send to: " << sendTo << " sent from "
 					<< sentFrom << std::endl;
 #endif
-			return DESTINVATION_INVALID;
+			return DESTINATION_INVALID;
 		}
 		case EINTR:
 			//The call was interrupted by a signal.

osal/linux/PosixThread.cpp

@@ -6,7 +6,7 @@
 #include <errno.h>
 
 PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_):
-        thread(0),priority(priority_),stackSize(stackSize_) {
+		thread(0), priority(priority_), stackSize(stackSize_) {
     name[0] = '\0';
     std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1);
 }
@@ -63,33 +63,33 @@ void PosixThread::resume(){
 }
 
 bool PosixThread::delayUntil(uint64_t* const prevoiusWakeTime_ms,
-        const uint64_t delayTime_ms) {
-    uint64_t nextTimeToWake_ms;
-    bool shouldDelay = false;
-    /* Get current Time */
-    const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
-    /* Generate the tick time at which the task wants to wake. */
-    nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
+		const uint64_t delayTime_ms) {
+	uint64_t nextTimeToWake_ms;
+	bool shouldDelay = false;
+	//Get current Time
+	const uint64_t currentTime_ms = getCurrentMonotonicTimeMs();
+	/* Generate the tick time at which the task wants to wake. */
+	nextTimeToWake_ms = (*prevoiusWakeTime_ms) + delayTime_ms;
 
-    if (currentTime_ms < *prevoiusWakeTime_ms) {
-        /* The tick count has overflowed since this function was
-		 lasted called. In this case the only time we should ever
-		 actually delay is if the wake time has also overflowed,
-		 and the wake time is greater than the tick time. When this
-		 is the case it is as if neither time had overflowed. */
-        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-                && (nextTimeToWake_ms > currentTime_ms)) {
-            shouldDelay = true;
-        }
-    } else {
-        /* The tick time has not overflowed.  In this case we will
-		 delay if either the wake time has overflowed, and/or the
-		 tick time is less than the wake time. */
-        if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
-                || (nextTimeToWake_ms > currentTime_ms)) {
-            shouldDelay = true;
-        }
-    }
+	if (currentTime_ms < *prevoiusWakeTime_ms) {
+		/* The tick count has overflowed since this function was
+		lasted called.  In this case the only time we should ever
+		actually delay is if the wake time has also overflowed,
+		and the wake time is greater than the tick time. When this
+		is the case it is as if neither time had overflowed. */
+		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+				&& (nextTimeToWake_ms > currentTime_ms)) {
+			shouldDelay = true;
+		}
+	} else {
+		/* The tick time has not overflowed. In this case we will
+		delay if either the wake time has overflowed, and/or the
+		tick time is less than the wake time. */
+		if ((nextTimeToWake_ms < *prevoiusWakeTime_ms)
+				|| (nextTimeToWake_ms > currentTime_ms)) {
+			shouldDelay = true;
+		}
+	}
 
     /* Update the wake time ready for the next call. */
 
@@ -181,11 +181,14 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
         sif::error << "Posix Thread attribute setinheritsched failed with: " <<
                 strerror(status) << std::endl;
 #endif
-    }
-
-    // TODO FIFO -> This needs root privileges for the process
-    status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
-    if(status != 0){
+	}
+#ifndef FSFW_USE_REALTIME_FOR_LINUX
+#error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1"
+#endif
+#if FSFW_USE_REALTIME_FOR_LINUX == 1
+	// FIFO -> This needs root privileges for the process
+	status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
+	if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "Posix Thread attribute schedule policy failed with: " <<
                 strerror(status) << std::endl;
@@ -200,14 +203,14 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
         sif::error << "Posix Thread attribute schedule params failed with: " <<
                 strerror(status) << std::endl;
 #endif
-    }
-
-    //Set Signal Mask for suspend until startTask is called
-    sigset_t waitSignal;
-    sigemptyset(&waitSignal);
-    sigaddset(&waitSignal, SIGUSR1);
-    status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
-    if(status != 0){
+	}
+#endif
+	//Set Signal Mask for suspend until startTask is called
+	sigset_t waitSignal;
+	sigemptyset(&waitSignal);
+	sigaddset(&waitSignal, SIGUSR1);
+	status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
+	if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "Posix Thread sigmask failed failed with: " <<
                 strerror(status) << " errno: " << strerror(errno) << std::endl;
@@ -218,8 +221,16 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
     status = pthread_create(&thread,&attributes,fnc_,arg_);
     if(status != 0){
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "Posix Thread create failed with: " <<
-                strerror(status) << std::endl;
+		sif::error << "PosixThread::createTask: Failed with: " <<
+				strerror(status) << std::endl;
+		sif::error << "For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call " <<
+		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+		        "/etc/security/limit.conf" << std::endl;
+#else
+		sif::printError("PosixThread::createTask: Create failed with: %s\n", strerror(status));
+		sif::printError("For FSFW_USE_REALTIME_FOR_LINUX == 1 make sure to call "
+		        "\"all sudo setcap 'cap_sys_nice=eip'\" on the application or set "
+                "/etc/security/limit.conf\n");
 #endif
     }
 

osal/linux/TcUnixUdpPollingTask.cpp

@@ -1,152 +0,0 @@
-#include "TcUnixUdpPollingTask.h"
-#include "tcpipHelpers.h"
-
-#include "../../globalfunctions/arrayprinter.h"
-
-#define FSFW_UDP_RCV_WIRETAPPING_ENABLED    0
-
-TcUnixUdpPollingTask::TcUnixUdpPollingTask(object_id_t objectId,
-		object_id_t tmtcUnixUdpBridge, size_t frameSize,
-		double timeoutSeconds): SystemObject(objectId),
-		tmtcBridgeId(tmtcUnixUdpBridge) {
-
-	if(frameSize > 0) {
-		this->frameSize = frameSize;
-	}
-	else {
-		this->frameSize = DEFAULT_MAX_FRAME_SIZE;
-	}
-
-	/* Set up reception buffer with specified frame size.
-	For now, it is assumed that only one frame is held in the buffer! */
-	receptionBuffer.reserve(this->frameSize);
-	receptionBuffer.resize(this->frameSize);
-
-	if(timeoutSeconds == -1) {
-		receptionTimeout = DEFAULT_TIMEOUT;
-	}
-	else {
-		receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
-	}
-}
-
-TcUnixUdpPollingTask::~TcUnixUdpPollingTask() {}
-
-ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
-    /* Sender Address is cached here. */
-    struct sockaddr_in senderAddress;
-    socklen_t senderAddressSize = sizeof(senderAddress);
-
-	/* Poll for new UDP datagrams in permanent loop. */
-	while(true) {
-		ssize_t bytesReceived = recvfrom(
-		        serverUdpSocket,
-				receptionBuffer.data(),
-				frameSize,
-				receptionFlags,
-				reinterpret_cast<sockaddr*>(&senderAddress),
-				&senderAddressSize
-		);
-		if(bytesReceived < 0) {
-			/* Handle error */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TcSocketPollingTask::performOperation: Reception error." << std::endl;
-#endif
-			tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 500);
-			continue;
-		}
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
-		sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived
-		        << " bytes received" << std::endl;
-#endif
-
-		ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
-		if(result != HasReturnvaluesIF::RETURN_FAILED) {
-
-		}
-		tmtcBridge->checkAndSetClientAddress(senderAddress);
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-
-ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
-	store_address_t storeId;
-
-#if FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
-    arrayprinter::print(receptionBuffer.data(), bytesRead);
-#endif
-
-    ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRead);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcUnixUdpPollingTask::handleSuccessfullTcRead: Data "
-				"storage failed" << std::endl;
-		sif::error << "Packet size: " << bytesRead << std::endl;
-#else
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	TmTcMessage message(storeId);
-
-	result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcUnixUdpPollingTask::handleSuccessfullTcRead: Sending message to queue "
-		        "failed" << std::endl;
-#else
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
-		tcStore->deleteData(storeId);
-	}
-	return result;
-}
-
-ReturnValue_t TcUnixUdpPollingTask::initialize() {
-	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
-	if (tcStore == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
-				<< std::endl;
-#endif
-		return ObjectManagerIF::CHILD_INIT_FAILED;
-	}
-
-	tmtcBridge = objectManager->get<TmTcUnixUdpBridge>(tmtcBridgeId);
-	if(tmtcBridge == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
-				" TMTC bridge object!" << std::endl;
-#endif
-		return ObjectManagerIF::CHILD_INIT_FAILED;
-	}
-
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t TcUnixUdpPollingTask::initializeAfterTaskCreation() {
-    /* Initialize the destination after task creation. This ensures
-    that the destination has already been set in the TMTC bridge. */
-	targetTcDestination = tmtcBridge->getRequestQueue();
-    /* The server socket is set up in the bridge intialization. Calling this function here
-    ensures that it is set up properly in any case*/
-    serverUdpSocket = tmtcBridge->serverSocket;
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-void TcUnixUdpPollingTask::setTimeout(double timeoutSeconds) {
-	timeval tval;
-	tval = timevalOperations::toTimeval(timeoutSeconds);
-	int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
-			&tval, sizeof(receptionTimeout));
-	if(result == -1) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
-				"receive timeout failed with " << strerror(errno) << std::endl;
-#endif
-	}
-}

osal/linux/TcUnixUdpPollingTask.h

@@ -1,67 +0,0 @@
-#ifndef FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
-#define FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
-
-#include "../../objectmanager/SystemObject.h"
-#include "../../osal/linux/TmTcUnixUdpBridge.h"
-#include "../../tasks/ExecutableObjectIF.h"
-
-#include <sys/socket.h>
-#include <vector>
-
-/**
- * @brief 	This class can be used to implement the polling of a Unix socket,
- * 			using UDP for now.
- * @details
- * The task will be blocked while the specified number of bytes has not been
- * received, so TC reception is handled inside a separate task.
- * This class caches the IP address of the sender. It is assumed there
- * is only one sender for now.
- */
-class TcUnixUdpPollingTask: public SystemObject,
-		public ExecutableObjectIF {
-	friend class TmTcUnixUdpBridge;
-public:
-	static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048;
-	//! 0.5  default milliseconds timeout for now.
-	static constexpr timeval DEFAULT_TIMEOUT = {.tv_sec = 0, .tv_usec = 500};
-
-	TcUnixUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
-			size_t frameSize = 0, double timeoutSeconds = -1);
-	virtual~ TcUnixUdpPollingTask();
-
-	/**
-	 * Turn on optional timeout for UDP polling. In the default mode,
-	 * the receive function will block until a packet is received.
-	 * @param timeoutSeconds
-	 */
-	void setTimeout(double timeoutSeconds);
-
-	virtual ReturnValue_t performOperation(uint8_t opCode) override;
-	virtual ReturnValue_t initialize() override;
-	virtual ReturnValue_t initializeAfterTaskCreation() override;
-
-protected:
-	StorageManagerIF* tcStore = nullptr;
-
-private:
-	//! TMTC bridge is cached.
-	object_id_t tmtcBridgeId = objects::NO_OBJECT;
-	TmTcUnixUdpBridge* tmtcBridge = nullptr;
-	MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
-
-	//! Reception flags: https://linux.die.net/man/2/recvfrom.
-	int receptionFlags = 0;
-
-	//! Server socket, which is member of TMTC bridge and is assigned in
-	//! constructor
-	int serverUdpSocket = 0;
-
-	std::vector<uint8_t> receptionBuffer;
-
-	size_t frameSize = 0;
-	timeval receptionTimeout;
-
-	ReturnValue_t handleSuccessfullTcRead(size_t bytesRead);
-};
-
-#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */

osal/linux/tcpipHelpers.cpp

@@ -1,5 +1,6 @@
-#include "tcpipHelpers.h"
+#include "../common/tcpipHelpers.h"
 
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/TaskFactory.h"
 
 #include <errno.h>
@@ -98,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,
-            errorSrcString, infoString);
+    sif::printWarning("tcpip::handleError: %s | %s | %s\n", protocolString.c_str(),
+            errorSrcString.c_str(), infoString.c_str());
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
     if(sleepDuration > 0) {

osal/linux/tcpipHelpers.h

@@ -1,14 +0,0 @@
-#ifndef FSFW_OSAL_LINUX_TCPIPHELPERS_H_
-#define FSFW_OSAL_LINUX_TCPIPHELPERS_H_
-
-#include "../../timemanager/clockDefinitions.h"
-#include "../common/tcpipCommon.h"
-
-namespace tcpip {
-
-
-void handleError(Protocol protocol, ErrorSources errorSrc, dur_millis_t sleepDuration = 0);
-
-}
-
-#endif /* FSFW_OSAL_LINUX_TCPIPHELPERS_H_ */

osal/rtems/MessageQueue.cpp

@@ -61,7 +61,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 	} else {
 		//No message was received. Keep lastPartner anyway, I might send something later.
 		//But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 	}
 	return convertReturnCode(status);
 }

osal/windows/CMakeLists.txt

@@ -1,11 +1,4 @@
 target_sources(${LIB_FSFW_NAME} PRIVATE 
-    TcWinUdpPollingTask.cpp
-    TmTcWinUdpBridge.cpp
-    TcWinTcpServer.cpp
     tcpipHelpers.cpp
+    winTaskHelpers.cpp
 )
-
-target_link_libraries(${LIB_FSFW_NAME} PRIVATE
-    wsock32 
-    ws2_32
-)

osal/windows/TcWinUdpPollingTask.cpp

@@ -1,149 +0,0 @@
-#include "TcWinUdpPollingTask.h"
-#include "tcpipHelpers.h"
-#include "../../globalfunctions/arrayprinter.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
-
-#include <winsock2.h>
-
-//! Debugging preprocessor define.
-#define FSFW_UDP_RCV_WIRETAPPING_ENABLED    0
-
-TcWinUdpPollingTask::TcWinUdpPollingTask(object_id_t objectId,
-		object_id_t tmtcUnixUdpBridge, size_t frameSize,
-		double timeoutSeconds): SystemObject(objectId),
-		tmtcBridgeId(tmtcUnixUdpBridge) {
-	if(frameSize > 0) {
-		this->frameSize = frameSize;
-	}
-	else {
-		this->frameSize = DEFAULT_MAX_FRAME_SIZE;
-	}
-
-	/* Set up reception buffer with specified frame size.
-	For now, it is assumed that only one frame is held in the buffer! */
-	receptionBuffer.reserve(this->frameSize);
-	receptionBuffer.resize(this->frameSize);
-
-	if(timeoutSeconds == -1) {
-		receptionTimeout = DEFAULT_TIMEOUT;
-	}
-	else {
-		receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
-	}
-}
-
-TcWinUdpPollingTask::~TcWinUdpPollingTask() {}
-
-ReturnValue_t TcWinUdpPollingTask::performOperation(uint8_t opCode) {
-    /* Sender Address is cached here. */
-    struct sockaddr_in senderAddress;
-    int senderAddressSize = sizeof(senderAddress);
-
-	/* Poll for new UDP datagrams in permanent loop. */
-	while(true) {
-		int bytesReceived = recvfrom(
-		        serverUdpSocket,
-		        reinterpret_cast<char*>(receptionBuffer.data()),
-		        frameSize,
-				receptionFlags,
-				reinterpret_cast<sockaddr*>(&senderAddress),
-				&senderAddressSize
-		);
-		if(bytesReceived == SOCKET_ERROR) {
-			/* Handle error */
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TcWinUdpPollingTask::performOperation: Reception error." << std::endl;
-#endif
-			tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::RECVFROM_CALL, 1000);
-			continue;
-		}
-#if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
-		sif::debug << "TcWinUdpPollingTask::performOperation: " << bytesReceived <<
-		        " bytes received" << std::endl;
-#endif
-
-		ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
-		if(result != HasReturnvaluesIF::RETURN_FAILED) {
-
-		}
-		tmtcBridge->checkAndSetClientAddress(senderAddress);
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-
-ReturnValue_t TcWinUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
-	store_address_t storeId;
-
-#if FSFW_UDP_RCV_WIRETAPPING_ENABLED == 1
-    arrayprinter::print(receptionBuffer.data(), bytesRead);
-#endif
-
-	ReturnValue_t result = tcStore->addData(&storeId, receptionBuffer.data(), bytesRead);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning<< "TcWinUdpPollingTask::transferPusToSoftwareBus: Data storage failed." <<
-		        std::endl;
-		sif::warning << "Packet size: " << bytesRead << std::endl;
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-
-	TmTcMessage message(storeId);
-
-	result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "TcWinUdpPollingTask::handleSuccessfullTcRead: "
-		        " Sending message to queue failed" << std::endl;
-#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
-#endif /* FSFW_VERBOSE_LEVEL >= 1 */
-		tcStore->deleteData(storeId);
-	}
-	return result;
-}
-
-ReturnValue_t TcWinUdpPollingTask::initialize() {
-	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
-	if (tcStore == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcWinUdpPollingTask::initialize: TC store uninitialized!" << std::endl;
-#endif
-		return ObjectManagerIF::CHILD_INIT_FAILED;
-	}
-
-	tmtcBridge = objectManager->get<TmTcWinUdpBridge>(tmtcBridgeId);
-	if(tmtcBridge == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcWinUdpPollingTask::initialize: Invalid TMTC bridge object!" <<
-		        std::endl;
-#endif
-		return ObjectManagerIF::CHILD_INIT_FAILED;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-ReturnValue_t TcWinUdpPollingTask::initializeAfterTaskCreation() {
-	/* Initialize the destination after task creation. This ensures
-	that the destination has already been set in the TMTC bridge. */
-	targetTcDestination = tmtcBridge->getRequestQueue();
-	/* The server socket is set up in the bridge intialization. Calling this function here
-	ensures that it is set up properly in any case*/
-    serverUdpSocket = tmtcBridge->serverSocket;
-	return HasReturnvaluesIF::RETURN_OK;
-}
-
-void TcWinUdpPollingTask::setTimeout(double timeoutSeconds) {
-	DWORD timeoutMs = timeoutSeconds * 1000.0;
-	int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
-			reinterpret_cast<const char*>(&timeoutMs), sizeof(DWORD));
-	if(result == -1) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "TcWinUdpPollingTask::TcSocketPollingTask: Setting "
-				"receive timeout failed with " << strerror(errno) << std::endl;
-#endif
-	}
-}

osal/windows/tcpipHelpers.cpp

@@ -1,4 +1,4 @@
-#include "tcpipHelpers.h"
+#include "../common/tcpipHelpers.h"
 #include <FSFWConfig.h>
 
 #include "../../tasks/TaskFactory.h"

osal/windows/winTaskHelpers.cpp

@@ -0,0 +1,107 @@
+#include <fsfw/osal/windows/winTaskHelpers.h>
+#include <mutex>
+
+TaskPriority tasks::makeWinPriority(PriorityClass prioClass, PriorityNumber prioNumber) {
+    return (static_cast<uint16_t>(prioClass) << 16) | static_cast<uint16_t> (prioNumber);
+}
+
+void tasks::getWinPriorityParameters(TaskPriority priority,
+        DWORD& priorityClass, int& priorityNumber) {
+    PriorityClass classInternal = static_cast<PriorityClass>(priority >> 16 & 0xff);
+    PriorityNumber numberInternal = static_cast<PriorityNumber>(priority & 0xff);
+    switch(classInternal) {
+    case(CLASS_IDLE): {
+        priorityClass = IDLE_PRIORITY_CLASS;
+        break;
+    }
+    case(CLASS_BELOW_NORMAL): {
+        priorityClass = BELOW_NORMAL_PRIORITY_CLASS;
+        break;
+    }
+    case(CLASS_NORMAL): {
+        priorityClass = NORMAL_PRIORITY_CLASS;
+        break;
+    }
+    case(CLASS_ABOVE_NORMAL): {
+        priorityClass = ABOVE_NORMAL_PRIORITY_CLASS;
+        break;
+    }
+    case(CLASS_HIGH): {
+        priorityClass = HIGH_PRIORITY_CLASS;
+        break;
+    }
+    case(CLASS_REALTIME): {
+        priorityClass = REALTIME_PRIORITY_CLASS;
+        break;
+    }
+    default: {
+        priorityClass = NORMAL_PRIORITY_CLASS;
+    }
+    }
+
+    switch(numberInternal) {
+    case(IDLE): {
+        priorityNumber = THREAD_PRIORITY_IDLE;
+        break;
+    }
+    case(LOWEST): {
+        priorityNumber = THREAD_PRIORITY_LOWEST;
+        break;
+    }
+    case(BELOW_NORMAL): {
+        priorityNumber = THREAD_PRIORITY_BELOW_NORMAL;
+        break;
+    }
+    case(NORMAL): {
+        priorityNumber = THREAD_PRIORITY_NORMAL;
+        break;
+    }
+    case(ABOVE_NORMAL): {
+        priorityNumber = THREAD_PRIORITY_ABOVE_NORMAL;
+        break;
+    }
+    case(HIGHEST): {
+        priorityNumber = THREAD_PRIORITY_HIGHEST;
+        break;
+    }
+    case(CRITICAL): {
+        priorityNumber = THREAD_PRIORITY_TIME_CRITICAL;
+        break;
+    }
+    default: {
+        priorityNumber = THREAD_PRIORITY_NORMAL;
+    }
+    }
+}
+
+ReturnValue_t tasks::setTaskPriority(HANDLE nativeHandle, TaskPriority priority) {
+    /* List of possible priority classes:
+    https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/nf-processthreadsapi-setpriorityclass
+    And respective thread priority numbers:
+    https://docs.microsoft.com/en-us/windows/win32/procthread/scheduling-priorities
+    */
+    DWORD dwPriorityClass = 0;
+    int nPriorityNumber = 0;
+    tasks::getWinPriorityParameters(priority, dwPriorityClass, nPriorityNumber);
+    int result = SetPriorityClass(
+            reinterpret_cast<HANDLE>(nativeHandle),
+            dwPriorityClass);
+    if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
+                << GetLastError() << std::endl;
+        return HasReturnvaluesIF::RETURN_FAILED;
+#endif
+    }
+    result = SetThreadPriority(
+            reinterpret_cast<HANDLE>(nativeHandle),
+            nPriorityNumber);
+    if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
+                << GetLastError() << std::endl;
+        return HasReturnvaluesIF::RETURN_FAILED;
+#endif
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}

osal/windows/winTaskHelpers.h

@@ -0,0 +1,37 @@
+#include "../../tasks/TaskFactory.h"
+
+#include <thread>
+#include <map>
+
+#ifdef _WIN32
+
+namespace tasks {
+
+enum PriorityClass: uint16_t {
+    CLASS_IDLE,
+    CLASS_BELOW_NORMAL,
+    CLASS_NORMAL,
+    CLASS_ABOVE_NORMAL,
+    CLASS_HIGH,
+    CLASS_REALTIME
+};
+enum PriorityNumber: uint16_t {
+    IDLE,
+    LOWEST,
+    BELOW_NORMAL,
+    NORMAL,
+    ABOVE_NORMAL,
+    HIGHEST,
+    CRITICAL
+};
+TaskPriority makeWinPriority(PriorityClass prioClass = PriorityClass::CLASS_NORMAL,
+        PriorityNumber prioNumber = PriorityNumber::NORMAL);
+void getWinPriorityParameters(TaskPriority priority, DWORD& priorityClass,
+        int& priorityNumber);
+
+ReturnValue_t setTaskPriority(HANDLE nativeHandle, TaskPriority priority);
+
+}
+
+#endif
+

pus/CService200ModeCommanding.cpp

@@ -61,8 +61,7 @@ ReturnValue_t CService200ModeCommanding::prepareCommand(
 		return result;
 	}
 
-	ModeMessage::setModeMessage(dynamic_cast<CommandMessage*>(message),
-	        ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+	ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
 	        modeCommandPacket.getSubmode());
 	return result;
 }

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

@@ -15,7 +15,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(
 	tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service1TelecommandVerification::~Service1TelecommandVerification() {}
+Service1TelecommandVerification::~Service1TelecommandVerification() {
+    QueueFactory::instance()->deleteMessageQueue(tmQueue);
+}
 
 MessageQueueId_t Service1TelecommandVerification::getVerificationQueue(){
 	return tmQueue->getId();
@@ -66,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;
@@ -77,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/Service20ParameterManagement.cpp

@@ -75,9 +75,8 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
 #else
         sif::printError("Service20ParameterManagement::checkInterfaceAndAcquire"
                 "MessageQueue: Can't access object\n");
-        sif::printError("Object ID: 0x%08x\n", objectId);
-        sif::printError("Make sure it implements "
-                "ReceivesParameterMessagesIF!\n");
+        sif::printError("Object ID: 0x%08x\n", *objectId);
+        sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
 #endif
 
         return CommandingServiceBase::INVALID_OBJECT;

pus/Service5EventReporting.cpp

@@ -15,7 +15,9 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId,
 	eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service5EventReporting::~Service5EventReporting(){}
+Service5EventReporting::~Service5EventReporting() {
+    QueueFactory::instance()->deleteMessageQueue(eventQueue);
+}
 
 ReturnValue_t Service5EventReporting::performService() {
 	EventMessage message;
@@ -50,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) {

pus/Service8FunctionManagement.cpp

@@ -53,12 +53,14 @@ ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t Service8FunctionManagement::prepareCommand(
 		CommandMessage* message, uint8_t subservice, const uint8_t* tcData,
 		size_t tcDataLen, uint32_t* state, object_id_t objectId) {
-	return prepareDirectCommand(dynamic_cast<CommandMessage*>(message),
-				tcData, tcDataLen);
+	return prepareDirectCommand(message, tcData, tcDataLen);
 }
 
 ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
 		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
     if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "Service8FunctionManagement::prepareDirectCommand:"

pus/servicepackets/Service8Packets.h

@@ -43,8 +43,8 @@ public:
 
 private:
 	DirectCommand(const DirectCommand &command);
-	object_id_t objectId;
-	ActionId_t actionId;
+	object_id_t objectId = 0;
+	ActionId_t actionId = 0;
 	uint32_t parametersSize; //!< [EXPORT] : [IGNORE]
 	const uint8_t * parameterBuffer; //!< [EXPORT] : [MAXSIZE] 65535 Bytes
 

serialize/SerializeElement.h

@@ -25,7 +25,7 @@ public:
 
 	}
 	SerializeElement() :
-			LinkedElement<SerializeIF>(this) {
+			LinkedElement<SerializeIF>(this), entry() {
 	}
 
 	ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,

subsystem/SubsystemBase.cpp

@@ -5,8 +5,7 @@
 
 SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent,
 		Mode_t initialMode, uint16_t commandQueueDepth) :
-		SystemObject(setObjectId), mode(initialMode), submode(SUBMODE_NONE),
-		childrenChangedMode(false),
+		SystemObject(setObjectId), mode(initialMode),
 		commandQueue(QueueFactory::instance()->createMessageQueue(
 		        commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE)),
 		healthHelper(this, setObjectId), modeHelper(this), parentId(parent) {
@@ -167,16 +166,16 @@ MessageQueueId_t SubsystemBase::getCommandQueue() const {
 }
 
 ReturnValue_t SubsystemBase::initialize() {
-	MessageQueueId_t parentQueue = 0;
+	MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE;
 	ReturnValue_t result = SystemObject::initialize();
 
 	if (result != RETURN_OK) {
 		return result;
 	}
 
-	if (parentId != 0) {
+	if (parentId != objects::NO_OBJECT) {
 		SubsystemBase *parent = objectManager->get<SubsystemBase>(parentId);
-		if (parent == NULL) {
+		if (parent == nullptr) {
 			return RETURN_FAILED;
 		}
 		parentQueue = parent->getCommandQueue();

subsystem/SubsystemBase.h

@@ -37,6 +37,17 @@ public:
 
 	virtual MessageQueueId_t getCommandQueue() const override;
 
+	/**
+	 * Function to register the child objects.
+	 * Performs a checks if the child does implement HasHealthIF and/or HasModesIF
+	 *
+	 * 	Also adds them to the internal childrenMap.
+	 *
+	 * @param objectId
+	 * @return RETURN_OK if successful
+	 * 		   CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF
+	 * 		   COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap
+	 */
 	ReturnValue_t registerChild(object_id_t objectId);
 
 	virtual ReturnValue_t initialize() override;
@@ -56,9 +67,9 @@ protected:
 
 	Mode_t mode;
 
-	Submode_t submode;
+	Submode_t submode = SUBMODE_NONE;
 
-	bool childrenChangedMode;
+	bool childrenChangedMode = false;
 
 	/**
 	 * Always check this against <=0, so you are robust against too many replies

tasks/TaskFactory.h

@@ -22,9 +22,26 @@ public:
 	/**
 	 * Creates a new periodic task and returns the interface pointer.
 	 * @param name_ Name of the task
-	 * @param taskPriority_ Priority of the task
+	 * @param taskPriority_
+	 * Priority of the task. This value might have different ranges for the various OSALs.
+	 *  - Linux     Value ranging from 0 to 99 with 99 being the highest value.
+	 *  - Host      For Windows, the value can be retrieved by using the #tasks::makeWinPriority
+	 *              function. For Linux, same priority system as specified above. MacOS not tested
+	 *              yet
+	 *  - FreeRTOS  Value depends on the FreeRTOS configuration, higher number means higher priority
+	 *  - RTEMS     Values ranging from 0 to 99 with 99 being the highest value.
+	 *
 	 * @param stackSize_ Stack Size of the task
+	 * This value might have different recommended ranges for the various OSALs.
+	 *  - Linux     Lowest limit is the PeriodicTaskIF::MINIMUM_STACK_SIZE value
+	 *  - Host      Value is ignored for now because the C++ threading abstraction layer is used.
+	 *  - FreeRTOS  Stack size in bytes. It is recommended to specify at least 1kB of stack for
+	 *              FSFW tasks, but the lowest possible size is specified in the
+	 *              FreeRTOSConfig.h file.
+	 *  - RTEMS     Lowest limit is specified the PeriodicTaskIF::MINIMUM_STACK_SIZE value.
+	 *
 	 * @param period_ Period of the task
+	 *
 	 * @param deadLineMissedFunction_ Function to be called if a deadline was missed
 	 * @return PeriodicTaskIF* Pointer to the newly created Task
 	 */
@@ -34,7 +51,8 @@ public:
 			TaskDeadlineMissedFunction deadLineMissedFunction_);
 
 	/**
-	 *
+	 * The meaning for the variables for fixed timeslot tasks is the same as for periodic tasks.
+	 * See #createPeriodicTask documentation.
 	 * @param name_ Name of the task
 	 * @param taskPriority_ Priority of the task
 	 * @param stackSize_ Stack Size of the task

tasks/Typedef.h

@@ -2,7 +2,7 @@
 #define FRAMEWORK_TASKS_TYPEDEF_H_
 
 typedef const char* TaskName;
-typedef uint8_t TaskPriority;
+typedef uint32_t TaskPriority;
 typedef size_t TaskStackSize;
 typedef double TaskPeriod;
 typedef void (*TaskDeadlineMissedFunction)();

tcdistribution/CCSDSDistributor.cpp

@@ -1,8 +1,10 @@
 #include "CCSDSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 
+#define CCSDS_DISTRIBUTOR_DEBUGGING     0
+
 CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 		object_id_t setObjectId):
 		TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
@@ -11,26 +13,36 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 CCSDSDistributor::~CCSDSDistributor() {}
 
 TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
+#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif::debug << "CCSDSDistributor::selectDestination received: " <<
-//			this->currentMessage.getStorageId().pool_index << ", " <<
-//			this->currentMessage.getStorageId().packet_index << std::endl;
+	sif::debug << "CCSDSDistributor::selectDestination received: " <<
+			this->currentMessage.getStorageId().poolIndex << ", " <<
+			this->currentMessage.getStorageId().packetIndex << std::endl;
+#else
+	sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
+	        currentMessage.getStorageId().poolIndex, currentMessage.getStorageId().packetIndex);
+#endif
 #endif
 	const uint8_t* packet = nullptr;
 	size_t size = 0;
 	ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
 			&packet, &size );
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CCSDSDistributor::selectDestination: Getting data from"
 				" store failed!" << std::endl;
+#else
+		sif::printError("CCSDSDistributor::selectDestination: Getting data from"
+                " store failed!\n");
+#endif
 #endif
 	}
 	SpacePacketBase currentPacket(packet);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif:: info << "CCSDSDistributor::selectDestination has packet with APID "
-//			<< std::hex << currentPacket.getAPID() << std::dec << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
+	sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex <<
+	        currentPacket.getAPID() << std::dec << std::endl;
 #endif
 	TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
 	if ( position != this->queueMap.end() ) {
@@ -76,9 +88,14 @@ ReturnValue_t CCSDSDistributor::initialize() {
 	ReturnValue_t status = this->TcDistributor::initialize();
 	this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
 	if (this->tcStore == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CCSDSDistributor::initialize: Could not initialize"
 	            " TC store!" << std::endl;
+#else
+	    sif::printError("CCSDSDistributor::initialize: Could not initialize"
+                " TC store!\n");
+#endif
 #endif
 	    status = RETURN_FAILED;
 	}

tcdistribution/PUSDistributor.cpp

@@ -1,22 +1,24 @@
 #include "CCSDSDistributorIF.h"
 #include "PUSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
+#define PUS_DISTRIBUTOR_DEBUGGING     0
+
 PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
-		object_id_t setPacketSource) :
-		TcDistributor(setObjectId), checker(setApid), verifyChannel(),
-		tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
+        object_id_t setPacketSource) :
+        TcDistributor(setObjectId), checker(setApid), verifyChannel(),
+        tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
 
 PUSDistributor::~PUSDistributor() {}
 
 PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    // sif:: debug << "PUSDistributor::handlePacket received: "
-    //          << this->current_packet_id.store_index << ", "
-    //          << this->current_packet_id.packet_index << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
+    store_address_t storeId = this->currentMessage.getStorageId());
+    sif:: debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " <<
+            storeId.packetIndex << std::endl;
 #endif
     TcMqMapIter queueMapIt = this->queueMap.end();
     if(this->currentPacket == nullptr) {
@@ -25,15 +27,17 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
     this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
     if (currentPacket->getWholeData() != nullptr) {
         tcStatus = checker.checkPacket(currentPacket);
-#ifdef DEBUG
         if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::debug << "PUSDistributor::handlePacket: Packet format "
-                    << "invalid, code "<< static_cast<int>(tcStatus)
-                    << std::endl;
+            sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
+                    static_cast<int>(tcStatus) << std::endl;
+#else
+            sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
+                    static_cast<int>(tcStatus));
+#endif
 #endif
         }
-#endif
         uint32_t queue_id = currentPacket->getService();
         queueMapIt = this->queueMap.find(queue_id);
     }
@@ -43,11 +47,12 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
     if (queueMapIt == this->queueMap.end()) {
         tcStatus = DESTINATION_NOT_FOUND;
-#ifdef DEBUG
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "PUSDistributor::handlePacket: Destination not found, "
-                << "code "<< static_cast<int>(tcStatus) << std::endl;
-#endif
+        sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
+#else
+        sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
+#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif
     }
 
@@ -62,46 +67,54 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
 
 ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
-	uint16_t serviceId = service->getIdentifier();
+    uint16_t serviceId = service->getIdentifier();
+#if PUS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	// sif::info << "Service ID: " << (int)serviceId << std::endl;
+    sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
+#else
+    sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
 #endif
-	MessageQueueId_t queue = service->getRequestQueue();
-	auto returnPair = queueMap.emplace(serviceId, queue);
-	if (not returnPair.second) {
+#endif
+    MessageQueueId_t queue = service->getRequestQueue();
+    auto returnPair = queueMap.emplace(serviceId, queue);
+    if (not returnPair.second) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::registerService: Service ID already"
-				" exists in map." << std::endl;
+        sif::error << "PUSDistributor::registerService: Service ID already"
+                " exists in map" << std::endl;
+#else
+        sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
 #endif
-		return SERVICE_ID_ALREADY_EXISTS;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+#endif
+        return SERVICE_ID_ALREADY_EXISTS;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueId_t PUSDistributor::getRequestQueue() {
-	return tcQueue->getId();
+    return tcQueue->getId();
 }
 
 ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
-	if (queueStatus != RETURN_OK) {
-		tcStatus = queueStatus;
-	}
-	if (tcStatus != RETURN_OK) {
-		this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
-				currentPacket, tcStatus);
-		// A failed packet is deleted immediately after reporting,
-		// otherwise it will block memory.
-		currentPacket->deletePacket();
-		return RETURN_FAILED;
-	} else {
-		this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
-				currentPacket);
-		return RETURN_OK;
-	}
+    if (queueStatus != RETURN_OK) {
+        tcStatus = queueStatus;
+    }
+    if (tcStatus != RETURN_OK) {
+        this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
+                currentPacket, tcStatus);
+        // A failed packet is deleted immediately after reporting,
+        // otherwise it will block memory.
+        currentPacket->deletePacket();
+        return RETURN_FAILED;
+    } else {
+        this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
+                currentPacket);
+        return RETURN_OK;
+    }
 }
 
 uint16_t PUSDistributor::getIdentifier() {
-	return checker.getApid();
+    return checker.getApid();
 }
 
 ReturnValue_t PUSDistributor::initialize() {
@@ -111,15 +124,17 @@ ReturnValue_t PUSDistributor::initialize() {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
 
-	CCSDSDistributorIF* ccsdsDistributor =
-			objectManager->get<CCSDSDistributorIF>(packetSource);
-	if (ccsdsDistributor == nullptr) {
+    CCSDSDistributorIF* ccsdsDistributor =
+            objectManager->get<CCSDSDistributorIF>(packetSource);
+    if (ccsdsDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::initialize: Packet source invalid."
-		        << " Make sure it exists and implements CCSDSDistributorIF!"
-		        << std::endl;
+        sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
+        sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
+#else
+        sif::printError("PUSDistributor::initialize: Packet source invalid\n");
+        sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
 #endif
-	    return RETURN_FAILED;
-	}
-	return ccsdsDistributor->registerApplication(this);
+        return RETURN_FAILED;
+    }
+    return ccsdsDistributor->registerApplication(this);
 }

thermal/Heater.cpp

@@ -1,350 +1,352 @@
-#include "../devicehandlers/DeviceHandlerFailureIsolation.h"
 #include "Heater.h"
 
+#include "../devicehandlers/DeviceHandlerFailureIsolation.h"
 #include "../power/Fuse.h"
 #include "../ipc/QueueFactory.h"
 
 Heater::Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1) :
-		HealthDevice(objectId, 0), internalState(STATE_OFF), powerSwitcher(
-				NULL), pcduQueueId(0), switch0(switch0), switch1(switch1), wasOn(
-				false), timedOut(false), reactedToBeingFaulty(false), passive(
-				false), eventQueue(NULL), heaterOnCountdown(10800000)/*about two orbits*/, parameterHelper(
-				this), lastAction(CLEAR) {
-	eventQueue = QueueFactory::instance()->createMessageQueue();
+HealthDevice(objectId, 0), internalState(STATE_OFF), switch0(switch0), switch1(switch1),
+        heaterOnCountdown(10800000)/*about two orbits*/,
+		parameterHelper(this) {
+    eventQueue = QueueFactory::instance()->createMessageQueue();
 }
 
 Heater::~Heater() {
-	QueueFactory::instance()->deleteMessageQueue(eventQueue);
+    QueueFactory::instance()->deleteMessageQueue(eventQueue);
 }
 
 ReturnValue_t Heater::set() {
-	passive = false;
-	//wait for clear before doing anything
-	if (internalState == STATE_WAIT) {
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	if (healthHelper.healthTable->isHealthy(getObjectId())) {
-		doAction(SET);
-		if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)){
-			return HasReturnvaluesIF::RETURN_FAILED;
-		} else {
-			return HasReturnvaluesIF::RETURN_OK;
-		}
-	} else {
-		if (healthHelper.healthTable->isFaulty(getObjectId())) {
-			if (!reactedToBeingFaulty) {
-				reactedToBeingFaulty = true;
-				doAction(CLEAR);
-			}
-		}
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
+    passive = false;
+    //wait for clear before doing anything
+    if (internalState == STATE_WAIT) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    if (healthHelper.healthTable->isHealthy(getObjectId())) {
+        doAction(SET);
+        if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)){
+            return HasReturnvaluesIF::RETURN_FAILED;
+        } else {
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+    } else {
+        if (healthHelper.healthTable->isFaulty(getObjectId())) {
+            if (!reactedToBeingFaulty) {
+                reactedToBeingFaulty = true;
+                doAction(CLEAR);
+            }
+        }
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 }
 
 void Heater::clear(bool passive) {
-	this->passive = passive;
-	//Force switching off
-	if (internalState == STATE_WAIT) {
-		internalState = STATE_ON;
-	}
-	if (healthHelper.healthTable->isHealthy(getObjectId())) {
-		doAction(CLEAR);
-	} else if (healthHelper.healthTable->isFaulty(getObjectId())) {
-		if (!reactedToBeingFaulty) {
-			reactedToBeingFaulty = true;
-			doAction(CLEAR);
-		}
-	}
+    this->passive = passive;
+    //Force switching off
+    if (internalState == STATE_WAIT) {
+        internalState = STATE_ON;
+    }
+    if (healthHelper.healthTable->isHealthy(getObjectId())) {
+        doAction(CLEAR);
+    } else if (healthHelper.healthTable->isFaulty(getObjectId())) {
+        if (!reactedToBeingFaulty) {
+            reactedToBeingFaulty = true;
+            doAction(CLEAR);
+        }
+    }
 }
 
 void Heater::doAction(Action action) {
-	//only act if we are not in the right state or in a transition
-	if (action == SET) {
-		if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)
-				|| (internalState == STATE_EXTERNAL_CONTROL)) {
-			switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
-			internalState = STATE_WAIT_FOR_SWITCHES_ON;
-			powerSwitcher->sendSwitchCommand(switch0, PowerSwitchIF::SWITCH_ON);
-			powerSwitcher->sendSwitchCommand(switch1, PowerSwitchIF::SWITCH_ON);
-		}
-	} else { //clear
-		if ((internalState == STATE_ON) || (internalState == STATE_FAULTY)
-				|| (internalState == STATE_EXTERNAL_CONTROL)) {
-			internalState = STATE_WAIT_FOR_SWITCHES_OFF;
-			switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
-			powerSwitcher->sendSwitchCommand(switch0,
-					PowerSwitchIF::SWITCH_OFF);
-			powerSwitcher->sendSwitchCommand(switch1,
-					PowerSwitchIF::SWITCH_OFF);
-		}
-	}
+    //only act if we are not in the right state or in a transition
+    if (action == SET) {
+        if ((internalState == STATE_OFF) || (internalState == STATE_PASSIVE)
+                || (internalState == STATE_EXTERNAL_CONTROL)) {
+            switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
+            internalState = STATE_WAIT_FOR_SWITCHES_ON;
+            powerSwitcher->sendSwitchCommand(switch0, PowerSwitchIF::SWITCH_ON);
+            powerSwitcher->sendSwitchCommand(switch1, PowerSwitchIF::SWITCH_ON);
+        }
+    } else { //clear
+        if ((internalState == STATE_ON) || (internalState == STATE_FAULTY)
+                || (internalState == STATE_EXTERNAL_CONTROL)) {
+            internalState = STATE_WAIT_FOR_SWITCHES_OFF;
+            switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
+            powerSwitcher->sendSwitchCommand(switch0,
+                    PowerSwitchIF::SWITCH_OFF);
+            powerSwitcher->sendSwitchCommand(switch1,
+                    PowerSwitchIF::SWITCH_OFF);
+        }
+    }
 }
 
 void Heater::setPowerSwitcher(PowerSwitchIF* powerSwitch) {
-	this->powerSwitcher = powerSwitch;
+    this->powerSwitcher = powerSwitch;
 }
 
 ReturnValue_t Heater::performOperation(uint8_t opCode) {
-	handleQueue();
-	handleEventQueue();
+    handleQueue();
+    handleEventQueue();
 
-	if (!healthHelper.healthTable->isFaulty(getObjectId())) {
-		reactedToBeingFaulty = false;
-	}
+    if (!healthHelper.healthTable->isFaulty(getObjectId())) {
+        reactedToBeingFaulty = false;
+    }
 
-	switch (internalState) {
-	case STATE_ON:
-		if ((powerSwitcher->getSwitchState(switch0) == PowerSwitchIF::SWITCH_OFF)
-				|| (powerSwitcher->getSwitchState(switch1)
-						== PowerSwitchIF::SWITCH_OFF)) {
-			//switch went off on its own
-			//trigger event. FDIR can confirm if it is caused by MniOps and decide on the action
-			//do not trigger FD events when under external control
-			if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
-				triggerEvent(PowerSwitchIF::SWITCH_WENT_OFF);
-			} else {
-				internalState = STATE_EXTERNAL_CONTROL;
-			}
-		}
-		break;
-	case STATE_OFF:
-		//check if heater is on, ie both switches are on
-		//if so, just command it to off, to resolve the situation or force a switch stayed on event
-		//But, only do anything if not already faulty (state off is the stable point for being faulty)
-		if ((!healthHelper.healthTable->isFaulty(getObjectId()))
-				&& (powerSwitcher->getSwitchState(switch0)
-						== PowerSwitchIF::SWITCH_ON)
-				&& (powerSwitcher->getSwitchState(switch1)
-						== PowerSwitchIF::SWITCH_ON)) {
-			//do not trigger FD events when under external control
-			if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
-				internalState = STATE_WAIT_FOR_SWITCHES_OFF;
-				switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
-				powerSwitcher->sendSwitchCommand(switch0,
-						PowerSwitchIF::SWITCH_OFF);
-				powerSwitcher->sendSwitchCommand(switch1,
-						PowerSwitchIF::SWITCH_OFF);
-			} else {
-				internalState = STATE_EXTERNAL_CONTROL;
-			}
-		}
-		break;
-	case STATE_PASSIVE:
-		break;
-	case STATE_WAIT_FOR_SWITCHES_ON:
-		if (switchCountdown.hasTimedOut()) {
-			if ((powerSwitcher->getSwitchState(switch0)
-					== PowerSwitchIF::SWITCH_OFF)
-					|| (powerSwitcher->getSwitchState(switch1)
-							== PowerSwitchIF::SWITCH_OFF)) {
-				triggerEvent(HEATER_STAYED_OFF);
-				internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
-			} else {
-				triggerEvent(HEATER_ON);
-				internalState = STATE_ON;
-			}
-		}
-		break;
-	case STATE_WAIT_FOR_SWITCHES_OFF:
-		if (switchCountdown.hasTimedOut()) {
-			//only check for both being on (ie heater still on)
-			if ((powerSwitcher->getSwitchState(switch0)
-					== PowerSwitchIF::SWITCH_ON)
-					&& (powerSwitcher->getSwitchState(switch1)
-							== PowerSwitchIF::SWITCH_ON)) {
-				if (healthHelper.healthTable->isFaulty(getObjectId())) {
-					if (passive) {
-						internalState = STATE_PASSIVE;
-					} else {
-						internalState = STATE_OFF; //just accept it
-					}
-					triggerEvent(HEATER_ON); //but throw an event to make it more visible
-					break;
-				}
-				triggerEvent(HEATER_STAYED_ON);
-				internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
-			} else {
-				triggerEvent(HEATER_OFF);
-				if (passive) {
-					internalState = STATE_PASSIVE;
-				} else {
-					internalState = STATE_OFF;
-				}
-			}
-		}
-		break;
-	default:
-		break;
-	}
+    switch (internalState) {
+    case STATE_ON:
+        if ((powerSwitcher->getSwitchState(switch0) == PowerSwitchIF::SWITCH_OFF)
+                || (powerSwitcher->getSwitchState(switch1)
+                        == PowerSwitchIF::SWITCH_OFF)) {
+            //switch went off on its own
+            //trigger event. FDIR can confirm if it is caused by MniOps and decide on the action
+            //do not trigger FD events when under external control
+            if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
+                triggerEvent(PowerSwitchIF::SWITCH_WENT_OFF);
+            } else {
+                internalState = STATE_EXTERNAL_CONTROL;
+            }
+        }
+        break;
+    case STATE_OFF:
+        //check if heater is on, ie both switches are on
+        //if so, just command it to off, to resolve the situation or force a switch stayed on event
+        //But, only do anything if not already faulty (state off is the stable point for being faulty)
+        if ((!healthHelper.healthTable->isFaulty(getObjectId()))
+                && (powerSwitcher->getSwitchState(switch0)
+                        == PowerSwitchIF::SWITCH_ON)
+                        && (powerSwitcher->getSwitchState(switch1)
+                                == PowerSwitchIF::SWITCH_ON)) {
+            //do not trigger FD events when under external control
+            if (healthHelper.getHealth() != EXTERNAL_CONTROL) {
+                internalState = STATE_WAIT_FOR_SWITCHES_OFF;
+                switchCountdown.setTimeout(powerSwitcher->getSwitchDelayMs());
+                powerSwitcher->sendSwitchCommand(switch0,
+                        PowerSwitchIF::SWITCH_OFF);
+                powerSwitcher->sendSwitchCommand(switch1,
+                        PowerSwitchIF::SWITCH_OFF);
+            } else {
+                internalState = STATE_EXTERNAL_CONTROL;
+            }
+        }
+        break;
+    case STATE_PASSIVE:
+        break;
+    case STATE_WAIT_FOR_SWITCHES_ON:
+        if (switchCountdown.hasTimedOut()) {
+            if ((powerSwitcher->getSwitchState(switch0)
+                    == PowerSwitchIF::SWITCH_OFF)
+                    || (powerSwitcher->getSwitchState(switch1)
+                            == PowerSwitchIF::SWITCH_OFF)) {
+                triggerEvent(HEATER_STAYED_OFF);
+                internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
+            } else {
+                triggerEvent(HEATER_ON);
+                internalState = STATE_ON;
+            }
+        }
+        break;
+    case STATE_WAIT_FOR_SWITCHES_OFF:
+        if (switchCountdown.hasTimedOut()) {
+            //only check for both being on (ie heater still on)
+            if ((powerSwitcher->getSwitchState(switch0)
+                    == PowerSwitchIF::SWITCH_ON)
+                    && (powerSwitcher->getSwitchState(switch1)
+                            == PowerSwitchIF::SWITCH_ON)) {
+                if (healthHelper.healthTable->isFaulty(getObjectId())) {
+                    if (passive) {
+                        internalState = STATE_PASSIVE;
+                    } else {
+                        internalState = STATE_OFF; //just accept it
+                    }
+                    triggerEvent(HEATER_ON); //but throw an event to make it more visible
+                    break;
+                }
+                triggerEvent(HEATER_STAYED_ON);
+                internalState = STATE_WAIT_FOR_FDIR; //wait before retrying or anything
+            } else {
+                triggerEvent(HEATER_OFF);
+                if (passive) {
+                    internalState = STATE_PASSIVE;
+                } else {
+                    internalState = STATE_OFF;
+                }
+            }
+        }
+        break;
+    default:
+        break;
+    }
 
-	if ((powerSwitcher->getSwitchState(switch0) == PowerSwitchIF::SWITCH_ON)
-			&& (powerSwitcher->getSwitchState(switch1)
-					== PowerSwitchIF::SWITCH_ON)) {
-		if (wasOn) {
-			if (heaterOnCountdown.hasTimedOut()) {
-				//SHOULDDO this means if a heater fails in single mode, the timeout will start again
-				//I am not sure if this is a bug, but atm I have no idea how to fix this and think
-				//it will be ok. whatcouldpossiblygowrong™
-				if (!timedOut) {
-					triggerEvent(HEATER_TIMEOUT);
-					timedOut = true;
-				}
-			}
-		} else {
-			wasOn = true;
-			heaterOnCountdown.resetTimer();
-			timedOut = false;
-		}
-	} else {
-		wasOn = false;
-	}
+    if ((powerSwitcher->getSwitchState(switch0) == PowerSwitchIF::SWITCH_ON)
+            && (powerSwitcher->getSwitchState(switch1)
+                    == PowerSwitchIF::SWITCH_ON)) {
+        if (wasOn) {
+            if (heaterOnCountdown.hasTimedOut()) {
+                //SHOULDDO this means if a heater fails in single mode, the timeout will start again
+                //I am not sure if this is a bug, but atm I have no idea how to fix this and think
+                //it will be ok. whatcouldpossiblygowrong™
+                if (!timedOut) {
+                    triggerEvent(HEATER_TIMEOUT);
+                    timedOut = true;
+                }
+            }
+        } else {
+            wasOn = true;
+            heaterOnCountdown.resetTimer();
+            timedOut = false;
+        }
+    } else {
+        wasOn = false;
+    }
 
-	return HasReturnvaluesIF::RETURN_OK;
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void Heater::setSwitch(uint8_t number, ReturnValue_t state,
-		uint32_t* uptimeOfSwitching) {
-	if (powerSwitcher == NULL) {
-		return;
-	}
-	if (powerSwitcher->getSwitchState(number) == state) {
-		*uptimeOfSwitching = INVALID_UPTIME;
-	} else {
-		if ((*uptimeOfSwitching == INVALID_UPTIME)) {
-			powerSwitcher->sendSwitchCommand(number, state);
-			Clock::getUptime(uptimeOfSwitching);
-		} else {
-			uint32_t currentUptime;
-			Clock::getUptime(&currentUptime);
-			if (currentUptime - *uptimeOfSwitching
-					> powerSwitcher->getSwitchDelayMs()) {
-				*uptimeOfSwitching = INVALID_UPTIME;
-				if (healthHelper.healthTable->isHealthy(getObjectId())) {
-					if (state == PowerSwitchIF::SWITCH_ON) {
-						triggerEvent(HEATER_STAYED_OFF);
-					} else {
-						triggerEvent(HEATER_STAYED_ON);
-					}
-				}
-				//SHOULDDO MiniOps during switch timeout leads to a faulty switch
-			}
-		}
-	}
+        uint32_t* uptimeOfSwitching) {
+    if (powerSwitcher == NULL) {
+        return;
+    }
+    if (powerSwitcher->getSwitchState(number) == state) {
+        *uptimeOfSwitching = INVALID_UPTIME;
+    } else {
+        if ((*uptimeOfSwitching == INVALID_UPTIME)) {
+            powerSwitcher->sendSwitchCommand(number, state);
+            Clock::getUptime(uptimeOfSwitching);
+        } else {
+            uint32_t currentUptime;
+            Clock::getUptime(&currentUptime);
+            if (currentUptime - *uptimeOfSwitching
+                    > powerSwitcher->getSwitchDelayMs()) {
+                *uptimeOfSwitching = INVALID_UPTIME;
+                if (healthHelper.healthTable->isHealthy(getObjectId())) {
+                    if (state == PowerSwitchIF::SWITCH_ON) {
+                        triggerEvent(HEATER_STAYED_OFF);
+                    } else {
+                        triggerEvent(HEATER_STAYED_ON);
+                    }
+                }
+            }
+        }
+    }
 }
 
 MessageQueueId_t Heater::getCommandQueue() const {
-	return commandQueue->getId();
+    return commandQueue->getId();
 }
 
 ReturnValue_t Heater::initialize() {
-	ReturnValue_t result = SystemObject::initialize();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+    ReturnValue_t result = SystemObject::initialize();
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	EventManagerIF* manager = objectManager->get<EventManagerIF>(
-			objects::EVENT_MANAGER);
-	if (manager == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	result = manager->registerListener(eventQueue->getId());
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+    EventManagerIF* manager = objectManager->get<EventManagerIF>(
+            objects::EVENT_MANAGER);
+    if (manager == NULL) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    result = manager->registerListener(eventQueue->getId());
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	ConfirmsFailuresIF* pcdu = objectManager->get<ConfirmsFailuresIF>(
-			DeviceHandlerFailureIsolation::powerConfirmationId);
-	if (pcdu == NULL) {
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	pcduQueueId = pcdu->getEventReceptionQueue();
+    ConfirmsFailuresIF* pcdu = objectManager->get<ConfirmsFailuresIF>(
+            DeviceHandlerFailureIsolation::powerConfirmationId);
+    if (pcdu == NULL) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    pcduQueueId = pcdu->getEventReceptionQueue();
 
-	result = manager->subscribeToAllEventsFrom(eventQueue->getId(),
-			getObjectId());
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+    result = manager->subscribeToAllEventsFrom(eventQueue->getId(),
+            getObjectId());
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	result = parameterHelper.initialize();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+    result = parameterHelper.initialize();
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	result = healthHelper.initialize();
-	if (result != HasReturnvaluesIF::RETURN_OK) {
-		return result;
-	}
+    result = healthHelper.initialize();
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
 
-	return HasReturnvaluesIF::RETURN_OK;
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void Heater::handleQueue() {
-	CommandMessage command;
-	ReturnValue_t result = commandQueue->receiveMessage(&command);
-	if (result == HasReturnvaluesIF::RETURN_OK) {
-		result = healthHelper.handleHealthCommand(&command);
-		if (result == HasReturnvaluesIF::RETURN_OK) {
-			return;
-		}
-		parameterHelper.handleParameterMessage(&command);
-	}
+    CommandMessage command;
+    ReturnValue_t result = commandQueue->receiveMessage(&command);
+    if (result == HasReturnvaluesIF::RETURN_OK) {
+        result = healthHelper.handleHealthCommand(&command);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
+        result = parameterHelper.handleParameterMessage(&command);
+        if (result == HasReturnvaluesIF::RETURN_OK) {
+            return;
+        }
+    }
 }
 
 ReturnValue_t Heater::getParameter(uint8_t domainId, uint8_t uniqueId,
         ParameterWrapper* parameterWrapper, const ParameterWrapper* newValues,
-		uint16_t startAtIndex) {
-	if (domainId != DOMAIN_ID_BASE) {
-		return INVALID_DOMAIN_ID;
-	}
-	switch (uniqueId) {
-	case 0:
-		parameterWrapper->set(heaterOnCountdown.timeout);
-		break;
-	default:
-		return INVALID_IDENTIFIER_ID;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+        uint16_t startAtIndex) {
+    if (domainId != DOMAIN_ID_BASE) {
+        return INVALID_DOMAIN_ID;
+    }
+    switch (uniqueId) {
+    case 0:
+        parameterWrapper->set(heaterOnCountdown.timeout);
+        break;
+    default:
+        return INVALID_IDENTIFIER_ID;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void Heater::handleEventQueue() {
-	EventMessage event;
-	for (ReturnValue_t result = eventQueue->receiveMessage(&event);
-			result == HasReturnvaluesIF::RETURN_OK;
-			result = eventQueue->receiveMessage(&event)) {
-		switch (event.getMessageId()) {
-		case EventMessage::EVENT_MESSAGE:
-			switch (event.getEvent()) {
-			case Fuse::FUSE_WENT_OFF:
-			case HEATER_STAYED_OFF:
-			case HEATER_STAYED_ON://Setting it faulty does not help, but we need to reach a stable state and can check for being faulty before throwing this event again.
-				if (healthHelper.healthTable->isCommandable(getObjectId())) {
-					healthHelper.setHealth(HasHealthIF::FAULTY);
-					internalState = STATE_FAULTY;
-				}
-				break;
-			case PowerSwitchIF::SWITCH_WENT_OFF:
-				internalState = STATE_WAIT;
-				event.setMessageId(EventMessage::CONFIRMATION_REQUEST);
-				if (pcduQueueId != 0) {
-					eventQueue->sendMessage(pcduQueueId, &event);
-				} else {
-					healthHelper.setHealth(HasHealthIF::FAULTY);
-					internalState = STATE_FAULTY;
-				}
-				break;
-			default:
-				return;
-			}
-			break;
-		case EventMessage::YOUR_FAULT:
-			healthHelper.setHealth(HasHealthIF::FAULTY);
-			internalState = STATE_FAULTY;
-			break;
-		case EventMessage::MY_FAULT:
-			//do nothing, we are already in STATE_WAIT and wait for a clear()
-			break;
-		default:
-			return;
-		}
-	}
+    EventMessage event;
+    for (ReturnValue_t result = eventQueue->receiveMessage(&event);
+            result == HasReturnvaluesIF::RETURN_OK;
+            result = eventQueue->receiveMessage(&event)) {
+        switch (event.getMessageId()) {
+        case EventMessage::EVENT_MESSAGE:
+            switch (event.getEvent()) {
+            case Fuse::FUSE_WENT_OFF:
+            case HEATER_STAYED_OFF:
+            // HEATER_STAYED_ON is a setting if faulty does not help, but we need to reach a stable state and can check
+            // for being faulty before throwing this event again.
+            case HEATER_STAYED_ON:
+                if (healthHelper.healthTable->isCommandable(getObjectId())) {
+                    healthHelper.setHealth(HasHealthIF::FAULTY);
+                    internalState = STATE_FAULTY;
+                }
+                break;
+            case PowerSwitchIF::SWITCH_WENT_OFF:
+                internalState = STATE_WAIT;
+                event.setMessageId(EventMessage::CONFIRMATION_REQUEST);
+                if (pcduQueueId != 0) {
+                    eventQueue->sendMessage(pcduQueueId, &event);
+                } else {
+                    healthHelper.setHealth(HasHealthIF::FAULTY);
+                    internalState = STATE_FAULTY;
+                }
+                break;
+            default:
+                return;
+            }
+            break;
+            case EventMessage::YOUR_FAULT:
+                healthHelper.setHealth(HasHealthIF::FAULTY);
+                internalState = STATE_FAULTY;
+                break;
+            case EventMessage::MY_FAULT:
+                //do nothing, we are already in STATE_WAIT and wait for a clear()
+                break;
+            default:
+                return;
+        }
+    }
 }

thermal/Heater.h

@@ -1,90 +1,93 @@
-#ifndef FRAMEWORK_THERMAL_HEATER_H_
-#define FRAMEWORK_THERMAL_HEATER_H_
+#ifndef FSFW_THERMAL_HEATER_H_
+#define FSFW_THERMAL_HEATER_H_
 
 #include "../devicehandlers/HealthDevice.h"
 #include "../parameters/ParameterHelper.h"
 #include "../power/PowerSwitchIF.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../timemanager/Countdown.h"
-#include <stdint.h>
-//class RedundantHeater;
+#include <cstdint>
+
 
 class Heater: public HealthDevice, public ReceivesParameterMessagesIF {
-	friend class RedundantHeater;
+    friend class RedundantHeater;
 public:
 
-	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::HEATER;
-	static const Event HEATER_ON = MAKE_EVENT(0, severity::INFO);
-	static const Event HEATER_OFF = MAKE_EVENT(1, severity::INFO);
-	static const Event HEATER_TIMEOUT = MAKE_EVENT(2, severity::LOW);
-	static const Event HEATER_STAYED_ON = MAKE_EVENT(3, severity::LOW);
-	static const Event HEATER_STAYED_OFF = MAKE_EVENT(4, severity::LOW);
+    static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::HEATER;
+    static const Event HEATER_ON = MAKE_EVENT(0, severity::INFO);
+    static const Event HEATER_OFF = MAKE_EVENT(1, severity::INFO);
+    static const Event HEATER_TIMEOUT = MAKE_EVENT(2, severity::LOW);
+    static const Event HEATER_STAYED_ON = MAKE_EVENT(3, severity::LOW);
+    static const Event HEATER_STAYED_OFF = MAKE_EVENT(4, severity::LOW);
 
-	Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1);
-	virtual ~Heater();
+    Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1);
+    virtual ~Heater();
 
-	ReturnValue_t performOperation(uint8_t opCode);
+    ReturnValue_t performOperation(uint8_t opCode);
 
-	ReturnValue_t initialize();
+    ReturnValue_t initialize();
 
-	ReturnValue_t set();
-	void clear(bool passive);
+    ReturnValue_t set();
+    void clear(bool passive);
 
-	void setPowerSwitcher(PowerSwitchIF *powerSwitch);
+    void setPowerSwitcher(PowerSwitchIF *powerSwitch);
 
-	MessageQueueId_t getCommandQueue() const;
+    MessageQueueId_t getCommandQueue() const;
 
-	ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId,
-	        ParameterWrapper *parameterWrapper,
-			const ParameterWrapper *newValues, uint16_t startAtIndex);
+    ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId,
+            ParameterWrapper *parameterWrapper,
+            const ParameterWrapper *newValues, uint16_t startAtIndex);
 
 protected:
-	static const uint32_t INVALID_UPTIME = 0;
+    static const uint32_t INVALID_UPTIME = 0;
 
-	enum InternalState {
-		STATE_ON,
-		STATE_OFF,
-		STATE_PASSIVE,
-		STATE_WAIT_FOR_SWITCHES_ON,
-		STATE_WAIT_FOR_SWITCHES_OFF,
-		STATE_WAIT_FOR_FDIR, //used to avoid doing anything until fdir decided what to do
-		STATE_FAULTY,
-		STATE_WAIT, //used when waiting for system to recover from miniops
-		STATE_EXTERNAL_CONTROL //entered when under external control and a fdir reaction would be triggered. This is useful when leaving external control into an unknown state
-							   //if no fdir reaction is triggered under external control the state is still ok and no need for any special treatment is needed
-	} internalState;
+    enum InternalState {
+        STATE_ON,
+        STATE_OFF,
+        STATE_PASSIVE,
+        STATE_WAIT_FOR_SWITCHES_ON,
+        STATE_WAIT_FOR_SWITCHES_OFF,
+        STATE_WAIT_FOR_FDIR, // Used to avoid doing anything until fdir decided what to do
+        STATE_FAULTY,
+        STATE_WAIT, // Used when waiting for system to recover from miniops
+        // Entered when under external control and a fdir reaction would be triggered.
+        // This is useful when leaving external control into an unknown state
+        STATE_EXTERNAL_CONTROL
+        // If no fdir reaction is triggered under external control the state is still ok and
+        // no need for any special treatment is needed
+    } internalState;
 
-	PowerSwitchIF *powerSwitcher;
-	MessageQueueId_t pcduQueueId;
+    PowerSwitchIF *powerSwitcher = nullptr;
+    MessageQueueId_t pcduQueueId = MessageQueueIF::NO_QUEUE;
 
-	uint8_t switch0;
-	uint8_t switch1;
+    uint8_t switch0;
+    uint8_t switch1;
 
-	bool wasOn;
+    bool wasOn = false;
 
-	bool timedOut;
+    bool timedOut = false;
 
-	bool reactedToBeingFaulty;
+    bool reactedToBeingFaulty = false;
 
-	bool passive;
+    bool passive = false;
 
-	MessageQueueIF* eventQueue;
-	Countdown heaterOnCountdown;
-	Countdown switchCountdown;
-	ParameterHelper parameterHelper;
+    MessageQueueIF* eventQueue = nullptr;
+    Countdown heaterOnCountdown;
+    Countdown switchCountdown;
+    ParameterHelper parameterHelper;
 
-	enum Action {
-		SET, CLEAR
-	} lastAction;
+    enum Action {
+        SET, CLEAR
+    } lastAction = CLEAR;
 
-	void doAction(Action action);
+    void doAction(Action action);
 
-	void setSwitch(uint8_t number, ReturnValue_t state,
-			uint32_t *upTimeOfSwitching);
+    void setSwitch(uint8_t number, ReturnValue_t state,
+            uint32_t *upTimeOfSwitching);
 
-	void handleQueue();
+    void handleQueue();
 
-	void handleEventQueue();
+    void handleEventQueue();
 };
 
-#endif /* FRAMEWORK_THERMAL_HEATER_H_ */
+#endif /* FSFW_THERMAL_HEATER_H_ */

thermal/TemperatureSensor.h

@@ -1,11 +1,14 @@
-#ifndef TEMPERATURESENSOR_H_
-#define TEMPERATURESENSOR_H_
+#ifndef FSFW_THERMAL_TEMPERATURESENSOR_H_
+#define FSFW_THERMAL_TEMPERATURESENSOR_H_
 
-#include "../thermal/AbstractTemperatureSensor.h"
-#include "../datapoolglob/GlobalDataSet.h"
-#include "../datapoolglob/GlobalPoolVariable.h"
+#include "tcsDefinitions.h"
+#include "AbstractTemperatureSensor.h"
+
+#include "../datapoollocal/LocalPoolDataSetBase.h"
+#include "../datapoollocal/LocalPoolVariable.h"
 #include "../monitoring/LimitMonitor.h"
 
+
 /**
  * @brief 	This building block handles non-linear value conversion and
  * 			range checks for analog temperature sensors.
@@ -57,27 +60,25 @@ public:
 
 	/**
 	 * Instantiate Temperature Sensor Object.
-	 * @param setObjectid objectId of the sensor object
-	 * @param inputValue Input value which is converted to a temperature
-	 * @param poolVariable Pool Variable to store the temperature value
-	 * @param vectorIndex Vector Index for the sensor monitor
-	 * @param parameters Calculation parameters, temperature limits, gradient limit
-	 * @param datapoolId Datapool ID of the output temperature
-	 * @param outputSet Output dataset for the output temperature to fetch it with read()
-	 * @param thermalModule respective thermal module, if it has one
+	 * @param setObjectid       objectId of the sensor object
+	 * @param inputTemperature  Pointer to a raw input value which is converted to an floating
+	 *                          point C output temperature
+	 * @param outputGpid        Global Pool ID of the output value
+	 * @param vectorIndex       Vector Index for the sensor monitor
+	 * @param parameters        Calculation parameters, temperature limits, gradient limit
+	 * @param outputSet         Output dataset for the output temperature to fetch it with read()
+	 * @param thermalModule     Respective thermal module, if it has one
 	 */
-	TemperatureSensor(object_id_t setObjectid,
-			inputType *inputValue, PoolVariableIF *poolVariable,
-			uint8_t vectorIndex, uint32_t datapoolId, Parameters parameters = {0, 0, 0, 0, 0, 0},
-			GlobDataSet *outputSet = NULL, ThermalModuleIF *thermalModule = NULL) :
+	TemperatureSensor(object_id_t setObjectid,lp_var_t<limitType>* inputTemperature,
+			gp_id_t outputGpid, uint8_t vectorIndex, Parameters parameters = {0, 0, 0, 0, 0, 0},
+			LocalPoolDataSetBase *outputSet = nullptr, ThermalModuleIF *thermalModule = nullptr) :
 			AbstractTemperatureSensor(setObjectid, thermalModule), parameters(parameters),
-			inputValue(inputValue), poolVariable(poolVariable),
-			outputTemperature(datapoolId, outputSet, PoolVariableIF::VAR_WRITE),
-			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR,
-				GlobalDataPool::poolIdAndPositionToPid(poolVariable->getDataPoolId(), vectorIndex),
+			inputTemperature(inputTemperature),
+			outputTemperature(outputGpid, outputSet, PoolVariableIF::VAR_WRITE),
+			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR, outputGpid,
 				DEFAULT_CONFIRMATION_COUNT, parameters.lowerLimit, parameters.upperLimit,
 				TEMP_SENSOR_LOW, TEMP_SENSOR_HIGH),
-			oldTemperature(20), uptimeOfOldTemperature( { INVALID_TEMPERATURE, 0 }) {
+			oldTemperature(20), uptimeOfOldTemperature({ thermal::INVALID_TEMPERATURE, 0 }) {
 	}
 
 
@@ -98,7 +99,7 @@ protected:
 
 private:
 	void setInvalid() {
-		outputTemperature = INVALID_TEMPERATURE;
+		outputTemperature = thermal::INVALID_TEMPERATURE;
 		outputTemperature.setValid(false);
 		uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 		sensorMonitor.setToInvalid();
@@ -108,11 +109,8 @@ protected:
 
 	UsedParameters parameters;
 
-	inputType * inputValue;
-
-	PoolVariableIF *poolVariable;
-
-	gp_float_t outputTemperature;
+	lp_var_t<limitType>* inputTemperature;
+	lp_var_t<float> outputTemperature;
 
 	LimitMonitor<limitType> sensorMonitor;
 
@@ -120,22 +118,27 @@ protected:
 	timeval uptimeOfOldTemperature;
 
 	void doChildOperation() {
-		if (!poolVariable->isValid()
-				|| !healthHelper.healthTable->isHealthy(getObjectId())) {
+		ReturnValue_t result = inputTemperature->read(MutexIF::TimeoutType::WAITING, 20);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+			return;
+		}
+
+		if ((not inputTemperature->isValid()) or
+		        (not healthHelper.healthTable->isHealthy(getObjectId()))) {
 			setInvalid();
 			return;
 		}
 
-		outputTemperature = calculateOutputTemperature(*inputValue);
+		outputTemperature = calculateOutputTemperature(inputTemperature->value);
 		outputTemperature.setValid(PoolVariableIF::VALID);
 
 		timeval uptime;
 		Clock::getUptime(&uptime);
 
 		if (uptimeOfOldTemperature.tv_sec != INVALID_UPTIME) {
-			//In theory, we could use an AbsValueMonitor to monitor the gradient.
-			//But this would require storing the maxGradient in DP and quite some overhead.
-			//The concept of delta limits is a bit strange anyway.
+			// In theory, we could use an AbsValueMonitor to monitor the gradient.
+			// But this would require storing the maxGradient in DP and quite some overhead.
+			// The concept of delta limits is a bit strange anyway.
 			float deltaTime;
 			float deltaTemp;
 
@@ -148,17 +151,17 @@ protected:
 			}
 			if (parameters.gradient < deltaTemp / deltaTime) {
 				triggerEvent(TEMP_SENSOR_GRADIENT);
-				//Don't set invalid, as we did not recognize it as invalid with full authority, let FDIR handle it
+				// Don't set invalid, as we did not recognize it as invalid with full authority,
+				// let FDIR handle it
 			}
 		}
 
-		//Check is done against raw limits. SHOULDDO: Why? Using <20>C would be more easy to handle.
 		sensorMonitor.doCheck(outputTemperature.value);
 
 		if (sensorMonitor.isOutOfLimits()) {
 			uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 			outputTemperature.setValid(PoolVariableIF::INVALID);
-			outputTemperature = INVALID_TEMPERATURE;
+			outputTemperature = thermal::INVALID_TEMPERATURE;
 		} else {
 			oldTemperature = outputTemperature;
 			uptimeOfOldTemperature = uptime;
@@ -179,7 +182,10 @@ public:
 	static const uint16_t ADDRESS_C = 2;
 	static const uint16_t ADDRESS_GRADIENT = 3;
 
-	static const uint16_t DEFAULT_CONFIRMATION_COUNT = 1; //!< Changed due to issue with later temperature checking even tough the sensor monitor was confirming already (Was 10 before with comment = Correlates to a 10s confirmation time. Chosen rather large, should not be so bad for components and helps survive glitches.)
+	//! Changed due to issue with later temperature checking even tough the sensor monitor was
+	//! confirming already (Was 10 before with comment = Correlates to a 10s confirmation time.
+	//! Chosen rather large, should not be so bad for components and helps survive glitches.)
+	static const uint16_t DEFAULT_CONFIRMATION_COUNT = 1;
 
 	static const uint8_t DOMAIN_ID_SENSOR = 1;
 
@@ -219,4 +225,4 @@ public:
 
 };
 
-#endif /* TEMPERATURESENSOR_H_ */
+#endif /* FSFW_THERMAL_TEMPERATURESENSOR_H_ */

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 = 8;
+	static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;
+
 	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,119 +3,66 @@
 #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) :
-		SpacePacketBase(setData) {
-	tmData = reinterpret_cast<TmPacketPointer*>(setData);
+TmPacketBase::TmPacketBase(uint8_t* setData):
+        SpacePacketBase(setData) {
 }
 
 TmPacketBase::~TmPacketBase() {
-	//Nothing to do.
+    //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)
-			- CRC_SIZE + 1;
+    return getPacketDataLength() - getDataFieldSize() - CRC_SIZE + 1;
 }
 
 uint16_t TmPacketBase::getErrorControl() {
-	uint32_t size = getSourceDataSize() + CRC_SIZE;
-	uint8_t* p_to_buffer = &tmData->data;
-	return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1];
+    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
+    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
 }
 
-void TmPacketBase::setData(const uint8_t* p_Data) {
-	SpacePacketBase::setData(p_Data);
-	tmData = (TmPacketPointer*) p_Data;
+ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const {
+    size_t tempSize = 0;
+    return CCSDSTime::convertFromCcsds(timestamp, getPacketTimeRaw(),
+            &tempSize, getTimestampSize());
 }
 
 void TmPacketBase::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	 sif::debug << "TmPacketBase::print: " << std::endl;
+    sif::debug << "TmPacketBase::print: " << std::endl;
 #endif
-	 arrayprinter::print(getWholeData(), getFullSize());
+    arrayprinter::print(getWholeData(), getFullSize());
 }
 
 bool TmPacketBase::checkAndSetStamper() {
-	if (timeStamper == NULL) {
-		timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
-		if (timeStamper == NULL) {
+    if (timeStamper == NULL) {
+        timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
+        if (timeStamper == NULL) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
-					<< std::endl;
+            sif::warning << "TmPacketBase::checkAndSetStamper: Stamper not found!" << std::endl;
+#else
+            sif::printWarning("TmPacketBase::checkAndSetStamper: Stamper not found!\n");
 #endif
-			return false;
-		}
-	}
-	return true;
+            return false;
+        }
+    }
+    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.
@@ -49,61 +23,83 @@ struct TmPacketPointer {
  * @ingroup tmtcpackets
  */
 class TmPacketBase : public SpacePacketBase {
-	friend void (Factory::setStaticFrameworkObjectIds)();
+    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.
-	//! TODO: Maybe also support PUS-C via config?
-	static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 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();
+    //! 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 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();
-	/**
-	 * 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.
-	 */
-	uint8_t* getSourceData();
-	/**
-	 * 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)
-	 */
-	uint16_t getSourceDataSize();
+    /**
+     * 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
@@ -111,79 +107,24 @@ public:
      * 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();
-
-	/**
-	 * 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;
+    /**
+     * 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:
-	/**
-	 * 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.
-	 */
-	static TimeStamperIF* timeStamper;
-	//! 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.
+     * The timeStamper is responsible for adding a timestamp to the packet.
+     * It is initialized lazy.
      */
-    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);
+    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.

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"
-#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 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();
-};
+#if FSFW_USE_PUS_C_TELEMETRY == 1
+#include "TmPacketStoredPusC.h"
+#else
+#include "TmPacketStoredPusA.h"
+#endif
 
 
 #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,68 @@
+#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 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_ */

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/TmTcBridge.cpp

@@ -4,6 +4,8 @@
 #include "../serviceinterface/ServiceInterface.h"
 #include "../globalfunctions/arrayprinter.h"
 
+#define TMTCBRIDGE_WIRETAPPING 0
+
 TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination,
 		object_id_t tmStoreId, object_id_t tcStoreId):
 		SystemObject(objectId),tmStoreId(tmStoreId), tcStoreId(tcStoreId),
@@ -14,7 +16,9 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination,
             createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
 }
 
-TmTcBridge::~TmTcBridge() {}
+TmTcBridge::~TmTcBridge() {
+    QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue);
+}
 
 ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(
 		uint8_t sentPacketsPerCycle) {
@@ -234,17 +238,11 @@ ReturnValue_t TmTcBridge::handleStoredTm() {
 
 void TmTcBridge::registerCommConnect() {
 	if(not communicationLinkUp) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		//sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
-#endif
 		communicationLinkUp = true;
 	}
 }
 
 void TmTcBridge::registerCommDisconnect() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-	//sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
-#endif
 	if(communicationLinkUp) {
 		communicationLinkUp = false;
 	}

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.