diff --git a/container/FixedOrderedMultimap.h b/container/FixedOrderedMultimap.h
index 96bc0073..acf2368a 100644
--- a/container/FixedOrderedMultimap.h
+++ b/container/FixedOrderedMultimap.h
@@ -3,6 +3,7 @@
 
 #include "ArrayList.h"
 #include <cstring>
+#include <functional>
 
 /**
  * @brief  An associative container which allows multiple entries of the same key.
diff --git a/datapool/PoolDataSetBase.cpp b/datapool/PoolDataSetBase.cpp
index 5fb1c3c7..05797012 100644
--- a/datapool/PoolDataSetBase.cpp
+++ b/datapool/PoolDataSetBase.cpp
@@ -38,11 +38,12 @@ ReturnValue_t PoolDataSetBase::registerVariable(
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
-ReturnValue_t PoolDataSetBase::read(uint32_t lockTimeout) {
+ReturnValue_t PoolDataSetBase::read(MutexIF::TimeoutType timeoutType,
+		uint32_t lockTimeout) {
 	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
 	ReturnValue_t error = result;
 	if (state == States::STATE_SET_UNINITIALISED) {
-		lockDataPool(lockTimeout);
+		lockDataPool(timeoutType, lockTimeout);
 		for (uint16_t count = 0; count < fillCount; count++) {
 			result = readVariable(count);
 			if(result != RETURN_OK) {
@@ -86,7 +87,8 @@ ReturnValue_t PoolDataSetBase::readVariable(uint16_t count) {
 				!= PoolVariableIF::NO_PARAMETER)
 	{
 		if(protectEveryReadCommitCall) {
-			result = registeredVariables[count]->read(mutexTimeout);
+			result = registeredVariables[count]->read(
+					MutexIF::TimeoutType::WAITING, mutexTimeout);
 		}
 		else {
 			result = registeredVariables[count]->readWithoutLock();
@@ -99,25 +101,28 @@ ReturnValue_t PoolDataSetBase::readVariable(uint16_t count) {
 	return result;
 }
 
-ReturnValue_t PoolDataSetBase::commit(uint32_t lockTimeout) {
+ReturnValue_t PoolDataSetBase::commit(MutexIF::TimeoutType timeoutType,
+		uint32_t lockTimeout) {
 	if (state == States::STATE_SET_WAS_READ) {
-		handleAlreadyReadDatasetCommit(lockTimeout);
+		handleAlreadyReadDatasetCommit(timeoutType, lockTimeout);
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	else {
-		return handleUnreadDatasetCommit(lockTimeout);
+		return handleUnreadDatasetCommit(timeoutType, lockTimeout);
 	}
 }
 
-void PoolDataSetBase::handleAlreadyReadDatasetCommit(uint32_t lockTimeout) {
-	lockDataPool(lockTimeout);
+void PoolDataSetBase::handleAlreadyReadDatasetCommit(
+		MutexIF::TimeoutType timeoutType, uint32_t lockTimeout) {
+	lockDataPool(timeoutType, lockTimeout);
 	for (uint16_t count = 0; count < fillCount; count++) {
 		if (registeredVariables[count]->getReadWriteMode()
 				!= PoolVariableIF::VAR_READ
 				&& registeredVariables[count]->getDataPoolId()
 				!= PoolVariableIF::NO_PARAMETER) {
 			if(protectEveryReadCommitCall) {
-				registeredVariables[count]->commit(mutexTimeout);
+				registeredVariables[count]->commit(
+						MutexIF::TimeoutType::WAITING, mutexTimeout);
 			}
 			else {
 				registeredVariables[count]->commitWithoutLock();
@@ -128,16 +133,18 @@ void PoolDataSetBase::handleAlreadyReadDatasetCommit(uint32_t lockTimeout) {
 	unlockDataPool();
 }
 
-ReturnValue_t PoolDataSetBase::handleUnreadDatasetCommit(uint32_t lockTimeout) {
+ReturnValue_t PoolDataSetBase::handleUnreadDatasetCommit(
+		MutexIF::TimeoutType timeoutType, uint32_t lockTimeout) {
 	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
-	lockDataPool(lockTimeout);
+	lockDataPool(timeoutType, lockTimeout);
 	for (uint16_t count = 0; count < fillCount; count++) {
 		if (registeredVariables[count]->getReadWriteMode()
 				== PoolVariableIF::VAR_WRITE
 				&& registeredVariables[count]->getDataPoolId()
 				!= PoolVariableIF::NO_PARAMETER) {
 			if(protectEveryReadCommitCall) {
-				result = registeredVariables[count]->commit(mutexTimeout);
+				result = registeredVariables[count]->commit(
+						MutexIF::TimeoutType::WAITING, mutexTimeout);
 			}
 			else {
 				result = registeredVariables[count]->commitWithoutLock();
@@ -160,7 +167,8 @@ ReturnValue_t PoolDataSetBase::handleUnreadDatasetCommit(uint32_t lockTimeout) {
 }
 
 
-ReturnValue_t PoolDataSetBase::lockDataPool(uint32_t timeoutMs) {
+ReturnValue_t PoolDataSetBase::lockDataPool(MutexIF::TimeoutType timeoutType,
+		uint32_t lockTimeout) {
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
diff --git a/datapool/PoolDataSetBase.h b/datapool/PoolDataSetBase.h
index 7c1d84a4..0528f40f 100644
--- a/datapool/PoolDataSetBase.h
+++ b/datapool/PoolDataSetBase.h
@@ -62,8 +62,9 @@ public:
 	 * - @c SET_WAS_ALREADY_READ if read() is called twice without calling
 	 *      commit() in between
 	 */
-	virtual ReturnValue_t read(uint32_t lockTimeout =
-			MutexIF::BLOCKING) override;
+	virtual ReturnValue_t read(
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t lockTimeout = 20) override;
 	/**
 	 * @brief	The commit call initializes writing back the registered variables.
 	 * @details
@@ -82,8 +83,9 @@ public:
 	 * 			- @c COMMITING_WITHOUT_READING if set was not read yet and
 	 * 			  contains non write-only variables
 	 */
-	virtual ReturnValue_t commit(uint32_t lockTimeout =
-			MutexIF::BLOCKING) override;
+	virtual ReturnValue_t commit(
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t lockTimeout = 20) override;
 
 	/**
 	 * Register the passed pool variable instance into the data set.
@@ -97,8 +99,9 @@ public:
 	 * thread-safety. Default implementation is empty
 	 * @return Always returns -@c RETURN_OK
 	 */
-	virtual ReturnValue_t lockDataPool(uint32_t timeoutMs =
-			MutexIF::BLOCKING) override;
+	virtual ReturnValue_t lockDataPool(
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) override;
 	/**
 	 * Provides the means to unlock the underlying data structure to ensure
 	 * thread-safety. Default implementation is empty
@@ -160,8 +163,12 @@ private:
 	uint32_t mutexTimeout = 20;
 
 	ReturnValue_t readVariable(uint16_t count);
-	void handleAlreadyReadDatasetCommit(uint32_t lockTimeout);
-	ReturnValue_t handleUnreadDatasetCommit(uint32_t lockTimeout);
+	void handleAlreadyReadDatasetCommit(
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20);
+	ReturnValue_t handleUnreadDatasetCommit(
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20);
 };
 
 #endif /* FSFW_DATAPOOL_POOLDATASETBASE_H_ */
diff --git a/datapool/PoolDataSetIF.h b/datapool/PoolDataSetIF.h
index 99c06cfd..1f52871d 100644
--- a/datapool/PoolDataSetIF.h
+++ b/datapool/PoolDataSetIF.h
@@ -18,7 +18,10 @@ public:
      *          thread-safety
      * @return Lock operation result
      */
-    virtual ReturnValue_t lockDataPool(dur_millis_t timeoutMs) = 0;
+    virtual ReturnValue_t lockDataPool(
+    		MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) = 0;
+
     /**
      * @brief   Unlock call corresponding to the lock call.
      * @return Unlock operation result
diff --git a/datapool/PoolEntry.cpp b/datapool/PoolEntry.cpp
index 58375162..1e99a28c 100644
--- a/datapool/PoolEntry.cpp
+++ b/datapool/PoolEntry.cpp
@@ -1,32 +1,24 @@
 #include "PoolEntry.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../globalfunctions/arrayprinter.h"
 #include <cstring>
 #include <algorithm>
 
 template <typename T>
-PoolEntry<T>::PoolEntry(std::initializer_list<T> initValue, uint8_t setLength,
-		bool setValid ) : length(setLength), valid(setValid) {
+PoolEntry<T>::PoolEntry(std::initializer_list<T> initValue, bool setValid ):
+		length(initValue.size()), valid(setValid) {
 	this->address = new T[this->length];
 	if(initValue.size() == 0) {
 		std::memset(this->address, 0, this->getByteSize());
 	}
-	else if (initValue.size() != setLength){
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "PoolEntry: setLength is not equal to initializer list"
-				"length! Performing zero initialization with given setLength"
-				<< std::endl;
-#endif
-		std::memset(this->address, 0, this->getByteSize());
-	}
 	else {
 		std::copy(initValue.begin(), initValue.end(), this->address);
 	}
 }
 
 template <typename T>
-PoolEntry<T>::PoolEntry( T* initValue, uint8_t setLength, bool setValid ) :
+PoolEntry<T>::PoolEntry(T* initValue, uint8_t setLength, bool setValid):
 		length(setLength), valid(setValid) {
 	this->address = new T[this->length];
 	if (initValue != nullptr) {
@@ -70,14 +62,26 @@ bool PoolEntry<T>::getValid() {
 
 template <typename T>
 void PoolEntry<T>::print() {
+	const char* validString = nullptr;
+	if(valid) {
+		validString = "Valid";
+	}
+	else {
+		validString = "Invalid";
+	}
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	 sif::debug << "Pool Entry Validity: " <<
-			 (this->valid? " (valid) " : " (invalid) ") << std::endl;
-#endif
-	arrayprinter::print(reinterpret_cast<uint8_t*>(address), length);
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-	sif::debug << std::dec << std::endl;
+	 sif::info << "PoolEntry information." << std::endl;
+	 sif::info << "PoolEntry validity: " << validString << std::endl;
+#else
+	 fsfw::printInfo("PoolEntry information.\n");
+	 fsfw::printInfo("PoolEntry validity: %s\n", validString);
 #endif
+	arrayprinter::print(reinterpret_cast<uint8_t*>(address), getByteSize());
+}
+
+template<typename T>
+inline T* PoolEntry<T>::getDataPtr() {
+	return this->address;
 }
 
 template<typename T>
@@ -88,8 +92,10 @@ Type PoolEntry<T>::getType() {
 template class PoolEntry<uint8_t>;
 template class PoolEntry<uint16_t>;
 template class PoolEntry<uint32_t>;
+template class PoolEntry<uint64_t>;
 template class PoolEntry<int8_t>;
 template class PoolEntry<int16_t>;
 template class PoolEntry<int32_t>;
+template class PoolEntry<int64_t>;
 template class PoolEntry<float>;
 template class PoolEntry<double>;
diff --git a/datapool/PoolEntry.h b/datapool/PoolEntry.h
index 033db40d..30940320 100644
--- a/datapool/PoolEntry.h
+++ b/datapool/PoolEntry.h
@@ -35,24 +35,22 @@ public:
 				"uint8_t");
 	/**
 	 * @brief	In the classe's constructor, space is allocated on the heap and
-	 * 			potential init values are copied to that space.
+	 * 			potential initialization values are copied to that space.
 	 * @details
 	 * Not passing any arguments will initialize an non-array pool entry
-	 * (setLength = 1) with an initial invalid state.
-	 * Please note that if an initializer list is passed, the correct
-	 * corresponding length should be passed too, otherwise a zero
-	 * initialization will be performed with the given setLength.
+	 * with an initial invalid state and the value 0.
+	 * Please note that if an initializer list is passed, the length of the
+	 * initializer list needs to be correct for vector entries because
+	 * required allocated space will be deduced from the initializer list length
+	 * and the pool entry type.
 	 * @param initValue
-	 * Initializer list with values to initialize with, for example {0,0} to
-	 * initialize the two entries to zero.
-	 * @param setLength
-	 * Defines the array length of this entry. Should be equal to the
-	 * intializer list length.
+	 * Initializer list with values to initialize with, for example {0, 0} to
+	 * initialize the a pool entry of a vector with two entries to 0.
 	 * @param setValid
 	 * Sets the initialization flag. It is invalid by default.
 	 */
-	PoolEntry(std::initializer_list<T> initValue = {}, uint8_t setLength = 1,
-			bool setValid = false);
+	PoolEntry(std::initializer_list<T> initValue = {0}, bool setValid = false);
+
 	/**
 	 * @brief	In the classe's constructor, space is allocated on the heap and
 	 * 			potential init values are copied to that space.
@@ -66,9 +64,9 @@ public:
 	 */
 	PoolEntry(T* initValue, uint8_t setLength = 1, bool setValid = false);
 
-	//! Explicitely deleted copy ctor, copying is not allowed!
+	//! Explicitely deleted copy ctor, copying is not allowed.
 	PoolEntry(const PoolEntry&) = delete;
-	//! Explicitely deleted copy assignment, copying is not allowed!
+	//! Explicitely deleted copy assignment, copying is not allowed.
 	PoolEntry& operator=(const PoolEntry&) = delete;
 
 	/**
@@ -82,21 +80,16 @@ public:
 	~PoolEntry();
 
 	/**
-	 * @brief	This is the address pointing to the allocated memory.
+	 * Return typed pointer to start of data.
+	 * @return
 	 */
-	T* address;
-	/**
-	 * @brief	This attribute stores the length information.
-	 */
-	uint8_t length;
-	/**
-	 * @brief	Here, the validity information for a variable is stored.
-	 * 			Every entry (single variable or vector) has one valid flag.
-	 */
-	uint8_t valid;
+	T* getDataPtr();
+
 	/**
 	 * @brief	getSize returns the array size of the entry.
-	 * @details	A single parameter has size 1.
+	 * @details
+	 * For non-array pool entries return type size, for vector entries
+	 * return type size times the number of entries.
 	 */
 	uint8_t getSize();
 	/**
@@ -123,8 +116,22 @@ public:
 	 * 			information to the screen. It prints all array entries in a row.
 	 */
 	void print();
-
 	Type getType();
+
+private:
+	/**
+	 * @brief	This attribute stores the length information.
+	 */
+	uint8_t length;
+	/**
+	 * @brief	Here, the validity information for a variable is stored.
+	 * 			Every entry (single variable or vector) has one valid flag.
+	 */
+	uint8_t valid;
+	/**
+	 * @brief	This is the address pointing to the allocated memory.
+	 */
+	T* address;
 };
 
 #endif /* FSFW_DATAPOOL_POOLENTRY_H_ */
diff --git a/datapool/PoolVariableIF.h b/datapool/PoolVariableIF.h
index 9740fc12..444e19d0 100644
--- a/datapool/PoolVariableIF.h
+++ b/datapool/PoolVariableIF.h
@@ -25,6 +25,7 @@ class PoolVariableIF : public SerializeIF,
 public:
 	static constexpr uint8_t INTERFACE_ID = CLASS_ID::POOL_VARIABLE_IF;
 	static constexpr ReturnValue_t INVALID_READ_WRITE_MODE = MAKE_RETURN_CODE(0xA0);
+	static constexpr ReturnValue_t INVALID_POOL_ENTRY = MAKE_RETURN_CODE(0xA1);
 
 	static constexpr bool VALID = 1;
 	static constexpr bool INVALID = 0;
diff --git a/datapool/ReadCommitIF.h b/datapool/ReadCommitIF.h
index 0cdce371..efe51594 100644
--- a/datapool/ReadCommitIF.h
+++ b/datapool/ReadCommitIF.h
@@ -2,6 +2,7 @@
 #define FSFW_DATAPOOL_READCOMMITIF_H_
 
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <fsfw/ipc/MutexIF.h>
 
 /**
  * @brief 	Common interface for all software objects which employ read-commit
@@ -10,8 +11,10 @@
 class ReadCommitIF {
 public:
 	virtual ~ReadCommitIF() {}
-	virtual ReturnValue_t read(uint32_t mutexTimeout) = 0;
-	virtual ReturnValue_t commit(uint32_t mutexTimeout) = 0;
+	virtual ReturnValue_t read(MutexIF::TimeoutType timeoutType,
+			uint32_t timeoutMs) = 0;
+	virtual ReturnValue_t commit(MutexIF::TimeoutType timeoutType,
+			uint32_t timeoutMs) = 0;
 
 protected:
 
@@ -19,11 +22,11 @@ protected:
 	//! members with commit and read semantics where the lock is only necessary
 	//! once.
 	virtual ReturnValue_t readWithoutLock()  {
-		return read(20);
+		return read(MutexIF::TimeoutType::WAITING, 20);
 	}
 
 	virtual ReturnValue_t commitWithoutLock() {
-		return commit(20);
+		return commit(MutexIF::TimeoutType::WAITING, 20);
 	}
 };
 
diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 24516aad..0ab6f150 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -5,7 +5,6 @@
 #include "../housekeeping/HousekeepingPacketUpdate.h"
 #include "../housekeeping/HousekeepingSetPacket.h"
 #include "../housekeeping/AcceptsHkPacketsIF.h"
-
 #include "../timemanager/CCSDSTime.h"
 #include "../ipc/MutexFactory.h"
 #include "../ipc/MutexHelper.h"
@@ -21,19 +20,17 @@ LocalDataPoolManager::LocalDataPoolManager(HasLocalDataPoolIF* owner,
         MessageQueueIF* queueToUse, bool appendValidityBuffer):
                         appendValidityBuffer(appendValidityBuffer) {
     if(owner == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::LocalDataPoolManager: "
-                << "Invalid supplied owner!" << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+				"Invalid supplied owner");
         return;
     }
     this->owner = owner;
     mutex = MutexFactory::instance()->createMutex();
     if(mutex == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::LocalDataPoolManager: "
-                << "Could not create mutex." << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+    			"LocalDataPoolManager", HasReturnvaluesIF::RETURN_FAILED,
+				"Could not create mutex");
     }
 
     hkQueue = queueToUse;
@@ -43,21 +40,18 @@ LocalDataPoolManager::~LocalDataPoolManager() {}
 
 ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
     if(queueToUse == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::initialize: "
-                << std::hex << "0x" << owner->getObjectId() << ". Supplied "
-                << "queue invalid!" << std::dec << std::endl;
-#endif
+        // error, all destinations invalid
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+    			"initialize", QUEUE_OR_DESTINATION_INVALID);
     }
     hkQueue = queueToUse;
 
     ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
     if(ipcStore == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::initialize: "
-                << std::hex << "0x" << owner->getObjectId() << ": Could not "
-                << "set IPC store." <<std::dec << std::endl;
-#endif
+        // error, all destinations invalid
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+    			"initialize", HasReturnvaluesIF::RETURN_FAILED,
+				"Could not set IPC store.");
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
@@ -69,11 +63,9 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
             hkDestinationId = hkPacketReceiver->getHkQueue();
         }
         else {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::error << "LocalDataPoolManager::LocalDataPoolManager: "
-                    << "Default HK destination object is invalid!" << std::endl;
-#endif
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+        			"initialize", QUEUE_OR_DESTINATION_INVALID);
+            return QUEUE_OR_DESTINATION_INVALID;
         }
     }
 
@@ -95,10 +87,10 @@ ReturnValue_t LocalDataPoolManager::initializeHousekeepingPoolEntriesOnce() {
         }
         return result;
     }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "HousekeepingManager: The map should only be initialized "
-            << "once!" << std::endl;
-#endif
+
+	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+			"initialize", HasReturnvaluesIF::RETURN_FAILED,
+			"The map should only be initialized once");
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -163,7 +155,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(
         LocalPoolObjectBase* poolObj = owner->getPoolObjectHandle(
                 receiver.dataId.localPoolId);
         if(poolObj == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+        			"handleNotificationUpdate", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
         }
         if(poolObj->hasChanged()) {
             // prepare and send update notification.
@@ -183,7 +177,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(
         LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(
                 receiver.dataId.sid);
         if(dataSet == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+        			"handleNotificationUpdate", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
         }
         if(dataSet->hasChanged()) {
             // prepare and send update notification
@@ -213,7 +209,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
         LocalPoolObjectBase* poolObj = owner->getPoolObjectHandle(
                 receiver.dataId.localPoolId);
         if(poolObj == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+        			"handleNotificationSnapshot", POOLOBJECT_NOT_FOUND);
+            return POOLOBJECT_NOT_FOUND;
         }
 
         if (not poolObj->hasChanged()) {
@@ -249,7 +247,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
         LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(
                 receiver.dataId.sid);
         if(dataSet == nullptr) {
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+        			"handleNotificationSnapshot", DATASET_NOT_FOUND);
+            return DATASET_NOT_FOUND;
         }
 
         if(not dataSet->hasChanged()) {
@@ -351,11 +351,9 @@ ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid,
     AcceptsHkPacketsIF* hkReceiverObject =
             objectManager->get<AcceptsHkPacketsIF>(packetDestination);
     if(hkReceiverObject == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::subscribeForPeriodicPacket:"
-                << " Invalid receiver!"<< std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_OK;
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
     }
 
     struct HkReceiver hkReceiver;
@@ -383,11 +381,9 @@ ReturnValue_t LocalDataPoolManager::subscribeForUpdatePackets(sid_t sid,
     AcceptsHkPacketsIF* hkReceiverObject =
             objectManager->get<AcceptsHkPacketsIF>(packetDestination);
     if(hkReceiverObject == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "LocalDataPoolManager::subscribeForPeriodicPacket:"
-                << " Invalid receiver!"<< std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_OK;
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"subscribeForPeriodicPacket", QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
     }
 
     struct HkReceiver hkReceiver;
@@ -591,10 +587,8 @@ ReturnValue_t LocalDataPoolManager::printPoolEntry(
         lp_id_t localPoolId) {
     auto poolIter = localPoolMap.find(localPoolId);
     if (poolIter == localPoolMap.end()) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "HousekeepingManager::fechPoolEntry:"
-                << " Pool entry not found." << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING, "printPoolEntry",
+    			 HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND);
         return HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND;
     }
     poolIter->second->print();
@@ -614,11 +608,10 @@ ReturnValue_t LocalDataPoolManager::generateHousekeepingPacket(sid_t sid,
         MessageQueueId_t destination) {
     if(dataSet == nullptr) {
         // Configuration error.
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "HousekeepingManager::generateHousekeepingPacket:"
-                << " Set ID not found or dataset not assigned!" << std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_FAILED;
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"generateHousekeepingPacket",
+    			DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
     }
 
     store_address_t storeId;
@@ -640,12 +633,18 @@ ReturnValue_t LocalDataPoolManager::generateHousekeepingPacket(sid_t sid,
     }
 
     if(hkQueue == nullptr) {
-        return QUEUE_OR_DESTINATION_NOT_SET;
+        // error, all destinations invalid
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"generateHousekeepingPacket",
+    			QUEUE_OR_DESTINATION_INVALID);
+        return QUEUE_OR_DESTINATION_INVALID;
     }
     if(destination == MessageQueueIF::NO_QUEUE) {
         if(hkDestinationId == MessageQueueIF::NO_QUEUE) {
             // error, all destinations invalid
-            return HasReturnvaluesIF::RETURN_FAILED;
+        	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+        			"generateHousekeepingPacket",
+        			QUEUE_OR_DESTINATION_INVALID);
         }
         destination = hkDestinationId;
     }
@@ -681,6 +680,13 @@ void LocalDataPoolManager::setNonDiagnosticIntervalFactor(
 void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
     sid_t sid = receiver.dataId.sid;
     LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
+    if(dataSet == nullptr) {
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"performPeriodicHkGeneration",
+    			DATASET_NOT_FOUND);
+    	return;
+    }
+
     if(not dataSet->getReportingEnabled()) {
         return;
     }
@@ -699,10 +705,11 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
     if(result != HasReturnvaluesIF::RETURN_OK) {
         // configuration error
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "LocalDataPoolManager::performHkOperation:"
-                << "0x" << std::hex << std::setfill('0') << std::setw(8)
-                << owner->getObjectId() << " Error generating "
-                << "HK packet" << std::setfill(' ') << std::dec << std::endl;
+        sif::warning << "LocalDataPoolManager::performHkOperation: "
+                << "HK generation failed." << std::endl;
+#else
+        fsfw::printWarning("LocalDataPoolManager::performHkOperation: "
+        		"HK generation failed.\n");
 #endif
     }
 }
@@ -748,11 +755,10 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
     // Get and check dataset first.
     LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
     if(dataSet == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "HousekeepingManager::generateHousekeepingPacket:"
-                << " Set ID not found" << std::endl;
-#endif
-        return HasReturnvaluesIF::RETURN_FAILED;
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"performPeriodicHkGeneration",
+    			DATASET_NOT_FOUND);
+        return DATASET_NOT_FOUND;
     }
 
 
@@ -776,10 +782,9 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
     ReturnValue_t result = ipcStore->getFreeElement(&storeId,
             expectedSize,&storePtr);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "HousekeepingManager::generateHousekeepingPacket: "
-                << "Could not get free element from IPC store." << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+    			"generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+				"Could not get free element from IPC store.");
         return result;
     }
 
@@ -788,10 +793,9 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
     result = setPacket.serialize(&storePtr, &size, expectedSize,
             SerializeIF::Endianness::BIG);
     if(expectedSize != size) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::error << "HousekeepingManager::generateSetStructurePacket: "
-                << "Expected size is not equal to serialized size" << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+    			"generateSetStructurePacket", HasReturnvaluesIF::RETURN_FAILED,
+				"Expected size is not equal to serialized size");
     }
 
     // Send structure reporting reply.
@@ -808,3 +812,58 @@ ReturnValue_t LocalDataPoolManager::generateSetStructurePacket(sid_t sid,
     hkQueue->reply(&reply);
     return result;
 }
+
+void LocalDataPoolManager::printWarningOrError(fsfw::OutputTypes outputType,
+		const char* functionName, ReturnValue_t error, const char* errorPrint) {
+	if(errorPrint == nullptr) {
+		if(error == DATASET_NOT_FOUND) {
+			errorPrint = "Dataset not found";
+		}
+		else if(error == POOLOBJECT_NOT_FOUND) {
+			errorPrint = "Pool Object not found";
+		}
+		else if(error == HasReturnvaluesIF::RETURN_FAILED) {
+			if(outputType == fsfw::OutputTypes::OUT_WARNING) {
+				errorPrint = "Generic Warning";
+			}
+			else {
+				errorPrint = "Generic error";
+			}
+		}
+		else if(error == QUEUE_OR_DESTINATION_INVALID) {
+			errorPrint = "Queue or destination not set";
+		}
+		else if(error == HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT) {
+			errorPrint = "Pool entry type conflict";
+		}
+		else if(error == HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND) {
+			errorPrint = "Pool entry not found";
+		}
+		else {
+			errorPrint = "Unknown error";
+		}
+	}
+
+	if(outputType == fsfw::OutputTypes::OUT_WARNING) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "LocalDataPoolManager::" << functionName
+				<< ": Object ID " << std::setw(8) << std::setfill('0')
+				<< std::hex << owner->getObjectId() << " | " << errorPrint
+				<< std::dec << std::setfill(' ') << std::endl;
+#else
+		fsfw::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+				owner->getObjectId(), errorPrint);
+#endif
+	}
+	else if(outputType == fsfw::OutputTypes::OUT_ERROR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "LocalDataPoolManager::" << functionName
+				<< ": Object ID " << std::setw(8) << std::setfill('0')
+				<< std::hex << owner->getObjectId() << " | " << errorPrint
+				<< std::dec << std::setfill(' ') << std::endl;
+#else
+		fsfw::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
+				owner->getObjectId(), errorPrint);
+#endif
+	}
+}
diff --git a/datapoollocal/LocalDataPoolManager.h b/datapoollocal/LocalDataPoolManager.h
index 0e98b90c..eee4c593 100644
--- a/datapoollocal/LocalDataPoolManager.h
+++ b/datapoollocal/LocalDataPoolManager.h
@@ -3,6 +3,7 @@
 
 #include "HasLocalDataPoolIF.h"
 
+#include "../serviceinterface/ServiceInterface.h"
 #include "../housekeeping/HousekeepingPacketDownlink.h"
 #include "../housekeeping/HousekeepingMessage.h"
 #include "../housekeeping/PeriodicHousekeepingHelper.h"
@@ -55,11 +56,13 @@ class LocalDataPoolManager {
 public:
 	static constexpr uint8_t INTERFACE_ID = CLASS_ID::HOUSEKEEPING_MANAGER;
 
-    static constexpr ReturnValue_t QUEUE_OR_DESTINATION_NOT_SET = MAKE_RETURN_CODE(0x0);
+    static constexpr ReturnValue_t QUEUE_OR_DESTINATION_INVALID = MAKE_RETURN_CODE(0);
 
-    static constexpr ReturnValue_t WRONG_HK_PACKET_TYPE = MAKE_RETURN_CODE(0x01);
-    static constexpr ReturnValue_t REPORTING_STATUS_UNCHANGED = MAKE_RETURN_CODE(0x02);
-    static constexpr ReturnValue_t PERIODIC_HELPER_INVALID = MAKE_RETURN_CODE(0x03);
+    static constexpr ReturnValue_t WRONG_HK_PACKET_TYPE = MAKE_RETURN_CODE(1);
+    static constexpr ReturnValue_t REPORTING_STATUS_UNCHANGED = MAKE_RETURN_CODE(2);
+    static constexpr ReturnValue_t PERIODIC_HELPER_INVALID = MAKE_RETURN_CODE(3);
+    static constexpr ReturnValue_t POOLOBJECT_NOT_FOUND = MAKE_RETURN_CODE(4);
+    static constexpr ReturnValue_t DATASET_NOT_FOUND = MAKE_RETURN_CODE(5);
 
     /**
      * This constructor is used by a class which wants to implement
@@ -367,6 +370,11 @@ private:
             ReturnValue_t& status);
 	ReturnValue_t addUpdateToStore(HousekeepingPacketUpdate& updatePacket,
 	        store_address_t& storeId);
+
+	void printWarningOrError(fsfw::OutputTypes outputType,
+			const char* functionName,
+			ReturnValue_t errorCode = HasReturnvaluesIF::RETURN_FAILED,
+			const char* errorPrint = nullptr);
 };
 
 
@@ -375,19 +383,15 @@ ReturnValue_t LocalDataPoolManager::fetchPoolEntry(lp_id_t localPoolId,
 		PoolEntry<T> **poolEntry) {
 	auto poolIter = localPoolMap.find(localPoolId);
 	if (poolIter == localPoolMap.end()) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "HousekeepingManager::fechPoolEntry: Pool entry "
-		        "not found." << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "fetchPoolEntry",
+				HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND);
 		return HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND;
 	}
 
 	*poolEntry = dynamic_cast< PoolEntry<T>* >(poolIter->second);
 	if(*poolEntry == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "HousekeepingManager::fetchPoolEntry:"
-				" Pool entry not found." << std::endl;
-#endif
+    	printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "fetchPoolEntry",
+    			HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT);
 		return HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT;
 	}
 	return HasReturnvaluesIF::RETURN_OK;
diff --git a/datapoollocal/LocalPoolDataSetBase.cpp b/datapoollocal/LocalPoolDataSetBase.cpp
index dbe34dfd..af492146 100644
--- a/datapoollocal/LocalPoolDataSetBase.cpp
+++ b/datapoollocal/LocalPoolDataSetBase.cpp
@@ -1,4 +1,6 @@
 #include "LocalPoolDataSetBase.h"
+
+#include "../serviceinterface/ServiceInterface.h"
 #include "../datapoollocal/LocalDataPoolManager.h"
 #include "../housekeeping/PeriodicHousekeepingHelper.h"
 #include "../serialize/SerializeAdapter.h"
@@ -15,7 +17,10 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(HasLocalDataPoolIF *hkOwner,
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
                 << "invalid!" << std::endl;
-#endif
+#else
+        fsfw::printError("LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
+                "invalid!\n\r");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
         return;
     }
     hkManager = hkOwner->getHkManagerHandle();
@@ -44,13 +49,26 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(sid_t sid,
     mutex = MutexFactory::instance()->createMutex();
 }
 
+LocalPoolDataSetBase::LocalPoolDataSetBase(
+		PoolVariableIF **registeredVariablesArray,
+		const size_t maxNumberOfVariables, bool protectFunctions):
+		PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables) {
+	if(protectFunctions) {
+	    mutex = MutexFactory::instance()->createMutex();
+	}
+
+}
+
+
 LocalPoolDataSetBase::~LocalPoolDataSetBase() {
 }
 
-ReturnValue_t LocalPoolDataSetBase::lockDataPool(uint32_t timeoutMs) {
+ReturnValue_t LocalPoolDataSetBase::lockDataPool(
+		MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
 	if(hkManager != nullptr) {
-	    MutexIF* mutex = hkManager->getMutexHandle();
-	    return mutex->lockMutex(MutexIF::TimeoutType::WAITING, timeoutMs);
+	    MutexIF* poolMutex = hkManager->getMutexHandle();
+	    return poolMutex->lockMutex(timeoutType, timeoutMs);
 	}
 	return HasReturnvaluesIF::RETURN_OK;
 }
@@ -150,9 +168,12 @@ ReturnValue_t LocalPoolDataSetBase::serializeLocalPoolIds(uint8_t** buffer,
                 size, maxSize, streamEndianness);
         if(result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::warning << "LocalDataSet::serializeLocalPoolIds: Serialization"
-                    " error!" << std::endl;
-#endif
+            sif::warning << "LocalPoolDataSetBase::serializeLocalPoolIds: "
+            		<< "Serialization error!" << std::endl;
+#else
+            fsfw::printWarning("LocalPoolDataSetBase::serializeLocalPoolIds: "
+            		"Serialization error!\n\r");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
             return result;
         }
     }
@@ -211,8 +232,11 @@ ReturnValue_t LocalPoolDataSetBase::serialize(uint8_t **buffer, size_t *size,
 void LocalPoolDataSetBase::bitSetter(uint8_t* byte, uint8_t position) const {
     if(position > 7) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "Pool Raw Access: Bit setting invalid position"
+        sif::warning << "LocalPoolDataSetBase::bitSetter: Invalid position!"
                 << std::endl;
+#else
+        fsfw::printWarning("LocalPoolDataSetBase::bitSetter: "
+        		"Invalid position!\n\r");
 #endif
         return;
     }
@@ -244,14 +268,19 @@ void LocalPoolDataSetBase::initializePeriodicHelper(
 }
 
 void LocalPoolDataSetBase::setChanged(bool changed) {
-    // TODO: Make this configurable?
-    MutexHelper(mutex, MutexIF::TimeoutType::WAITING, 20);
+	if(mutex == nullptr) {
+		this->changed = changed;
+		return;
+	}
+    MutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeout);
     this->changed = changed;
 }
 
 bool LocalPoolDataSetBase::hasChanged() const {
-    // TODO: Make this configurable?
-    MutexHelper(mutex, MutexIF::TimeoutType::WAITING, 20);
+	if(mutex == nullptr) {
+		return changed;
+	}
+    MutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeout);
     return changed;
 }
 
@@ -273,18 +302,22 @@ bool LocalPoolDataSetBase::bitGetter(const uint8_t* byte,
 }
 
 bool LocalPoolDataSetBase::isValid() const {
+	if(mutex == nullptr) {
+		return this->valid;
+	}
     MutexHelper(mutex, MutexIF::TimeoutType::WAITING, 5);
     return this->valid;
 }
 
 void LocalPoolDataSetBase::setValidity(bool valid, bool setEntriesRecursively) {
-    MutexHelper(mutex, MutexIF::TimeoutType::WAITING, 5);
+	mutex->lockMutex(timeoutType, mutexTimeout);
     if(setEntriesRecursively) {
         for(size_t idx = 0; idx < this->getFillCount(); idx++) {
             registeredVariables[idx] -> setValid(valid);
         }
     }
     this->valid = valid;
+    mutex->unlockMutex();
 }
 
 void LocalPoolDataSetBase::setReadCommitProtectionBehaviour(
@@ -292,3 +325,9 @@ void LocalPoolDataSetBase::setReadCommitProtectionBehaviour(
 	PoolDataSetBase::setReadCommitProtectionBehaviour(protectEveryReadCommit,
 			mutexTimeout);
 }
+
+void LocalPoolDataSetBase::setDataSetMutexTimeout(
+		MutexIF::TimeoutType timeoutType, uint32_t mutexTimeout) {
+	this->timeoutType = timeoutType;
+	this->mutexTimeout = mutexTimeout;
+}
diff --git a/datapoollocal/LocalPoolDataSetBase.h b/datapoollocal/LocalPoolDataSetBase.h
index d9b6a221..b44f607f 100644
--- a/datapoollocal/LocalPoolDataSetBase.h
+++ b/datapoollocal/LocalPoolDataSetBase.h
@@ -67,6 +67,15 @@ public:
 	LocalPoolDataSetBase(sid_t sid, PoolVariableIF** registeredVariablesArray,
 	        const size_t maxNumberOfVariables);
 
+	/**
+	 * Simple constructor, if the dataset is not owner permanently by
+	 * a class with a HK manager.
+	 * @param registeredVariablesArray
+	 * @param maxNumberOfVariables
+	 */
+	LocalPoolDataSetBase(PoolVariableIF** registeredVariablesArray,
+	        const size_t maxNumberOfVariables, bool protectFunctions = true);
+
 	/**
 	 * @brief	The destructor automatically manages writing the valid
 	 * 			information of variables.
@@ -87,6 +96,9 @@ public:
 	void setReadCommitProtectionBehaviour(bool protectEveryReadCommit,
 			uint32_t mutexTimeout = 20);
 
+	void setDataSetMutexTimeout(MutexIF::TimeoutType timeoutType,
+			uint32_t mutexTimeout);
+
 	void setValidityBufferGeneration(bool withValidityBuffer);
 
 	sid_t getSid() const;
@@ -138,7 +150,12 @@ public:
 
 protected:
 	sid_t sid;
+	MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
 	uint32_t mutexTimeout = 20;
+	/**
+	 * This mutex is required because the dataset can potentially be accessed
+	 * by multiple threads for information like change status or validity.
+	 */
 	MutexIF* mutex = nullptr;
 
 	bool diagnostic = false;
@@ -183,7 +200,9 @@ protected:
 	 * @details
 	 * It makes use of the lockDataPool method offered by the DataPool class.
 	 */
-	ReturnValue_t lockDataPool(uint32_t timeoutMs) override;
+	ReturnValue_t lockDataPool(MutexIF::TimeoutType timeoutType,
+			uint32_t timeoutMs) override;
+
 	/**
 	 * @brief	This is a small helper function to facilitate
 	 * 			unlocking the global data pool
diff --git a/datapoollocal/LocalPoolObjectBase.cpp b/datapoollocal/LocalPoolObjectBase.cpp
index 4b57dede..bb7822da 100644
--- a/datapoollocal/LocalPoolObjectBase.cpp
+++ b/datapoollocal/LocalPoolObjectBase.cpp
@@ -79,3 +79,43 @@ bool LocalPoolObjectBase::hasChanged() const {
 void LocalPoolObjectBase::setReadWriteMode(pool_rwm_t newReadWriteMode) {
 	this->readWriteMode = newReadWriteMode;
 }
+
+void LocalPoolObjectBase::reportReadCommitError(const char* variableType,
+		ReturnValue_t error, bool read, object_id_t objectId, lp_id_t lpId) {
+#if FSFW_DISABLE_PRINTOUT == 0
+	const char* type = nullptr;
+	if(read) {
+		type = "read";
+	}
+	else {
+		type = "commit";
+	}
+
+	const char* errMsg = nullptr;
+	if(error == HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND) {
+		errMsg = "Pool entry not found";
+	}
+	else if(error == HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT) {
+		errMsg = "Pool entry type conflict";
+	}
+	else if(error == PoolVariableIF::INVALID_READ_WRITE_MODE) {
+		errMsg = "Pool variable wrong read-write mode";
+	}
+	else if(error == PoolVariableIF::INVALID_POOL_ENTRY) {
+		errMsg = "Pool entry invalid";
+	}
+	else {
+		errMsg = "Unknown error code";
+	}
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	sif::warning << variableType << ": " << type << " call | " << errMsg
+			<< " | Owner: 0x" << std::hex << std::setw(8)
+			<< std::setfill('0') << objectId << std::dec << " LPID: " << lpId
+			<< std::endl;
+#else
+	fsfw::printWarning("%s: %s call | %s | Owner: 0x%08x LPID: %lu\n",
+			variableType, type, errMsg, objectId, lpId);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+}
diff --git a/datapoollocal/LocalPoolObjectBase.h b/datapoollocal/LocalPoolObjectBase.h
index 7165fc24..25ca594d 100644
--- a/datapoollocal/LocalPoolObjectBase.h
+++ b/datapoollocal/LocalPoolObjectBase.h
@@ -56,6 +56,8 @@ protected:
     //! @brief  Pointer to the class which manages the HK pool.
     LocalDataPoolManager* hkManager;
 
+	void reportReadCommitError(const char* variableType,
+			ReturnValue_t error, bool read, object_id_t objectId, lp_id_t lpId);
 };
 
 
diff --git a/datapoollocal/LocalPoolVariable.h b/datapoollocal/LocalPoolVariable.h
index 84be54b1..22019c2f 100644
--- a/datapoollocal/LocalPoolVariable.h
+++ b/datapoollocal/LocalPoolVariable.h
@@ -7,6 +7,7 @@
 
 #include "../datapool/PoolVariableIF.h"
 #include "../datapool/DataSetIF.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/ObjectManagerIF.h"
 #include "../serialize/SerializeAdapter.h"
 
@@ -105,7 +106,9 @@ public:
 	 * at once to avoid the overhead of unnecessary lock und unlock operations.
 	 *
 	 */
-	ReturnValue_t read(dur_millis_t lockTimeout = MutexIF::BLOCKING) override;
+	ReturnValue_t read(MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) override;
 	/**
 	 * @brief	The commit call copies the array values back to the data pool.
 	 * @details
@@ -115,8 +118,21 @@ public:
 	 * It is recommended to use DataSets to read and commit multiple variables
 	 * at once to avoid the overhead of unnecessary lock und unlock operations.
 	 */
-	ReturnValue_t commit(dur_millis_t lockTimeout = MutexIF::BLOCKING) override;
+	ReturnValue_t commit(MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) override;
 
+	/**
+	 * @brief	This commit function can be used to set the pool variable valid
+	 * 			as well.
+	 * @param setValid
+	 * @param timeoutType
+	 * @param timeoutMs
+	 * @return
+	 */
+	ReturnValue_t commit(bool setValid, MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20);
 
 	LocalPoolVariable<T> &operator=(const T& newValue);
 	LocalPoolVariable<T> &operator=(const LocalPoolVariable<T>& newPoolVariable);
@@ -163,8 +179,6 @@ protected:
 	friend std::ostream& operator<< (std::ostream &out,
 			const LocalPoolVariable<U> &var);
 #endif
-
-private:
 };
 
 #include "LocalPoolVariable.tpp"
diff --git a/datapoollocal/LocalPoolVariable.tpp b/datapoollocal/LocalPoolVariable.tpp
index cd5117c8..ef94b620 100644
--- a/datapoollocal/LocalPoolVariable.tpp
+++ b/datapoollocal/LocalPoolVariable.tpp
@@ -24,73 +24,87 @@ inline LocalPoolVariable<T>::LocalPoolVariable(gp_id_t globalPoolId,
 
 
 template<typename T>
-inline ReturnValue_t LocalPoolVariable<T>::read(dur_millis_t lockTimeout) {
-	MutexHelper(hkManager->getMutexHandle(), MutexIF::TimeoutType::WAITING,
-			lockTimeout);
+inline ReturnValue_t LocalPoolVariable<T>::read(
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	MutexHelper(hkManager->getMutexHandle(), timeoutType, timeoutMs);
 	return readWithoutLock();
 }
 
 template<typename T>
 inline ReturnValue_t LocalPoolVariable<T>::readWithoutLock() {
 	if(readWriteMode == pool_rwm_t::VAR_WRITE) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for read() call." << std::endl;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector",
+				PoolVariableIF::INVALID_READ_WRITE_MODE, true, targetObjectId,
+				localPoolId);
 		return PoolVariableIF::INVALID_READ_WRITE_MODE;
 	}
 
 	PoolEntry<T>* poolEntry = nullptr;
 	ReturnValue_t result = hkManager->fetchPoolEntry(localPoolId, &poolEntry);
-	if(result != RETURN_OK or poolEntry == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PoolVector: Read of local pool variable of object "
-				<< std::hex << std::setw(8) << std::setfill('0')
-				<< hkManager->getOwner() << " and lp ID " << localPoolId
-				<< std::dec << " failed." << std::setfill(' ') <<  std::endl;
-#endif
+	if(result != RETURN_OK) {
+		object_id_t ownerObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVariable", result,
+				false, ownerObjectId, localPoolId);
 		return result;
 	}
-	this->value = *(poolEntry->address);
-	this->valid = poolEntry->valid;
+
+	// Actually this should never happen..
+//	if(poolEntry->address == nullptr) {
+//		result = PoolVariableIF::INVALID_POOL_ENTRY;
+//		object_id_t ownerObjectId = hkManager->getOwner()->getObjectId();
+//		reportReadCommitError("LocalPoolVariable", result,
+//				false, ownerObjectId, localPoolId);
+//		return result;
+//	}
+
+	this->value = *(poolEntry->getDataPtr());
+	this->valid = poolEntry->getValid();
 	return RETURN_OK;
 }
 
 template<typename T>
-inline ReturnValue_t LocalPoolVariable<T>::commit(dur_millis_t lockTimeout) {
-	MutexHelper(hkManager->getMutexHandle(), MutexIF::TimeoutType::WAITING,
-			lockTimeout);
+inline ReturnValue_t LocalPoolVariable<T>::commit(bool setValid,
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	this->setValid(setValid);
+	return commit(timeoutType, timeoutMs);
+}
+
+template<typename T>
+inline ReturnValue_t LocalPoolVariable<T>::commit(
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	MutexHelper(hkManager->getMutexHandle(), timeoutType, timeoutMs);
 	return commitWithoutLock();
 }
 
 template<typename T>
 inline ReturnValue_t LocalPoolVariable<T>::commitWithoutLock() {
 	if(readWriteMode == pool_rwm_t::VAR_READ) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "LocalPoolVariable: Invalid read write "
-				 "mode for commit() call." << std::endl;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector",
+				PoolVariableIF::INVALID_READ_WRITE_MODE, false, targetObjectId,
+				localPoolId);
 		return PoolVariableIF::INVALID_READ_WRITE_MODE;
 	}
+
 	PoolEntry<T>* poolEntry = nullptr;
 	ReturnValue_t result = hkManager->fetchPoolEntry(localPoolId, &poolEntry);
 	if(result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PoolVector: Read of local pool variable of object "
-				"0x" << std::hex << std::setw(8) << std::setfill('0') <<
-				hkManager->getOwner() << " and lp ID 0x" << localPoolId <<
-				std::dec << " failed.\n" << std::flush;
-#endif
+		object_id_t ownerObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVariable", result,
+				false, ownerObjectId, localPoolId);
 		return result;
 	}
-	*(poolEntry->address) = this->value;
-	poolEntry->valid = this->valid;
+
+	*(poolEntry->getDataPtr()) = this->value;
+	poolEntry->setValid(this->valid);
 	return RETURN_OK;
 }
 
 template<typename T>
-inline ReturnValue_t LocalPoolVariable<T>::serialize(uint8_t** buffer, size_t* size,
-		const size_t max_size, SerializeIF::Endianness streamEndianness) const {
+inline ReturnValue_t LocalPoolVariable<T>::serialize(uint8_t** buffer,
+		size_t* size, const size_t max_size,
+		SerializeIF::Endianness streamEndianness) const {
 	return SerializeAdapter::serialize(&value,
 			buffer, size ,max_size, streamEndianness);
 }
@@ -121,7 +135,8 @@ inline LocalPoolVariable<T>::operator T() const {
 }
 
 template<typename T>
-inline LocalPoolVariable<T> & LocalPoolVariable<T>::operator=(const T& newValue) {
+inline LocalPoolVariable<T> & LocalPoolVariable<T>::operator=(
+		const T& newValue) {
     value = newValue;
     return *this;
 }
@@ -134,7 +149,8 @@ inline LocalPoolVariable<T>& LocalPoolVariable<T>::operator =(
 }
 
 template<typename T>
-inline bool LocalPoolVariable<T>::operator ==(const LocalPoolVariable<T> &other) const {
+inline bool LocalPoolVariable<T>::operator ==(
+		const LocalPoolVariable<T> &other) const {
 	return this->value == other.value;
 }
 
@@ -145,7 +161,8 @@ inline bool LocalPoolVariable<T>::operator ==(const T &other) const {
 
 
 template<typename T>
-inline bool LocalPoolVariable<T>::operator !=(const LocalPoolVariable<T> &other) const {
+inline bool LocalPoolVariable<T>::operator !=(
+		const LocalPoolVariable<T> &other) const {
 	return not (*this == other);
 }
 
@@ -156,7 +173,8 @@ inline bool LocalPoolVariable<T>::operator !=(const T &other) const {
 
 
 template<typename T>
-inline bool LocalPoolVariable<T>::operator <(const LocalPoolVariable<T> &other) const {
+inline bool LocalPoolVariable<T>::operator <(
+		const LocalPoolVariable<T> &other) const {
 	return this->value < other.value;
 }
 
@@ -167,7 +185,8 @@ inline bool LocalPoolVariable<T>::operator <(const T &other) const {
 
 
 template<typename T>
-inline bool LocalPoolVariable<T>::operator >(const LocalPoolVariable<T> &other) const {
+inline bool LocalPoolVariable<T>::operator >(
+		const LocalPoolVariable<T> &other) const {
 	return not (*this < other);
 }
 
diff --git a/datapoollocal/LocalPoolVector.h b/datapoollocal/LocalPoolVector.h
index 58face3c..a28fe327 100644
--- a/datapoollocal/LocalPoolVector.h
+++ b/datapoollocal/LocalPoolVector.h
@@ -7,7 +7,7 @@
 #include "../datapool/PoolVariableIF.h"
 #include "../datapoollocal/LocalDataPoolManager.h"
 #include "../serialize/SerializeAdapter.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 
 /**
@@ -100,8 +100,8 @@ public:
 		return vectorSize;
 	}
 
-	T& operator [](int i);
-	const T &operator [](int i) const;
+	T& operator [](size_t i);
+	const T &operator [](size_t i) const;
 
 	virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size,
 			const size_t maxSize,
@@ -123,7 +123,10 @@ public:
 	 * It is recommended to use DataSets to read and commit multiple variables
 	 * at once to avoid the overhead of unnecessary lock und unlock operations.
 	 */
-	ReturnValue_t read(uint32_t lockTimeout = MutexIF::BLOCKING) override;
+	ReturnValue_t read(MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) override;
+
 	/**
 	 * @brief	The commit call copies the array values back to the data pool.
 	 * @details
@@ -133,7 +136,17 @@ public:
 	 * It is recommended to use DataSets to read and commit multiple variables
 	 * at once to avoid the overhead of unnecessary lock und unlock operations.
 	 */
-	ReturnValue_t commit(uint32_t lockTimeout = MutexIF::BLOCKING) override;
+	ReturnValue_t commit(MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20) override;
+
+	/**
+	 * @brief	This commit call also sets the validity of the pool entry.
+	 * @details
+	 */
+	ReturnValue_t commit(bool valid, MutexIF::TimeoutType timeoutType =
+			MutexIF::TimeoutType::WAITING,
+			uint32_t timeoutMs = 20);
 
 protected:
 	/**
@@ -157,12 +170,12 @@ protected:
 
 private:
 
-
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	// std::ostream is the type for object std::cout
 	template <typename U, uint16_t otherSize>
 	friend std::ostream& operator<< (std::ostream &out,
 	        const LocalPoolVector<U, otherSize> &var);
-
+#endif
 
 };
 
diff --git a/datapoollocal/LocalPoolVector.tpp b/datapoollocal/LocalPoolVector.tpp
index 5c3b0ce0..f311c23d 100644
--- a/datapoollocal/LocalPoolVector.tpp
+++ b/datapoollocal/LocalPoolVector.tpp
@@ -24,18 +24,18 @@ inline LocalPoolVector<T, vectorSize>::LocalPoolVector(gp_id_t globalPoolId,
 				dataSet, setReadWriteMode) {}
 
 template<typename T, uint16_t vectorSize>
-inline ReturnValue_t LocalPoolVector<T, vectorSize>::read(uint32_t lockTimeout) {
-	MutexHelper(hkManager->getMutexHandle(), MutexIF::TimeoutType::WAITING,
-			lockTimeout);
+inline ReturnValue_t LocalPoolVector<T, vectorSize>::read(
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	MutexHelper(hkManager->getMutexHandle(), timeoutType, timeoutMs);
 	return readWithoutLock();
 }
 template<typename T, uint16_t vectorSize>
 inline ReturnValue_t LocalPoolVector<T, vectorSize>::readWithoutLock() {
 	if(readWriteMode == pool_rwm_t::VAR_WRITE) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for read() call." << std::endl;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector",
+				PoolVariableIF::INVALID_READ_WRITE_MODE, true, targetObjectId,
+				localPoolId);
 		return PoolVariableIF::INVALID_READ_WRITE_MODE;
 	}
 
@@ -44,78 +44,84 @@ inline ReturnValue_t LocalPoolVector<T, vectorSize>::readWithoutLock() {
 	memset(this->value, 0, vectorSize * sizeof(T));
 
 	if(result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PoolVector: Read of local pool variable of object "
-				"0x" << std::hex << std::setw(8) << std::setfill('0') <<
-				hkManager->getOwner() << "and lp ID 0x" << localPoolId <<
-				std::dec << " failed." << std::endl;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector", result, true, targetObjectId,
+				localPoolId);
 		return result;
 	}
-	std::memcpy(this->value, poolEntry->address, poolEntry->getByteSize());
-	this->valid = poolEntry->valid;
+	std::memcpy(this->value, poolEntry->getDataPtr(), poolEntry->getByteSize());
+	this->valid = poolEntry->getValid();
 	return RETURN_OK;
 }
 
+template<typename T, uint16_t vectorSize>
+inline ReturnValue_t LocalPoolVector<T, vectorSize>::commit(bool valid,
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	this->setValid(valid);
+	return commit(timeoutType, timeoutMs);
+}
+
 template<typename T, uint16_t vectorSize>
 inline ReturnValue_t LocalPoolVector<T, vectorSize>::commit(
-		uint32_t lockTimeout) {
-	MutexHelper(hkManager->getMutexHandle(), MutexIF::TimeoutType::WAITING,
-			lockTimeout);
+		MutexIF::TimeoutType timeoutType, uint32_t timeoutMs) {
+	MutexHelper(hkManager->getMutexHandle(), timeoutType, timeoutMs);
 	return commitWithoutLock();
 }
 
 template<typename T, uint16_t vectorSize>
 inline ReturnValue_t LocalPoolVector<T, vectorSize>::commitWithoutLock() {
 	if(readWriteMode == pool_rwm_t::VAR_READ) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for commit() call." << std::endl;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector",
+				PoolVariableIF::INVALID_READ_WRITE_MODE, false, targetObjectId,
+				localPoolId);
 		return PoolVariableIF::INVALID_READ_WRITE_MODE;
 	}
 	PoolEntry<T>* poolEntry = nullptr;
 	ReturnValue_t result = hkManager->fetchPoolEntry(localPoolId, &poolEntry);
 	if(result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PoolVector: Read of local pool variable of object "
-				"0x" << std::hex << std::setw(8) << std::setfill('0') <<
-				hkManager->getOwner() << " and lp ID 0x" << localPoolId <<
-				std::dec << " failed.\n" << std::flush;
-#endif
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector", result, false, targetObjectId,
+				localPoolId);
 		return result;
 	}
-	std::memcpy(poolEntry->address, this->value, poolEntry->getByteSize());
-	poolEntry->valid = this->valid;
+	std::memcpy(poolEntry->getDataPtr(), this->value, poolEntry->getByteSize());
+	poolEntry->setValid(this->valid);
 	return RETURN_OK;
 }
 
 template<typename T, uint16_t vectorSize>
-inline T& LocalPoolVector<T, vectorSize>::operator [](int i) {
-	if(i <= vectorSize) {
+inline T& LocalPoolVector<T, vectorSize>::operator [](size_t i) {
+	if(i < vectorSize) {
 		return value[i];
 	}
 	// If this happens, I have to set some value. I consider this
 	// a configuration error, but I wont exit here.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	sif::error << "LocalPoolVector: Invalid index. Setting or returning"
+	sif::warning << "LocalPoolVector: Invalid index. Setting or returning"
 			" last value!" << std::endl;
+#else
+	fsfw::printWarning("LocalPoolVector: Invalid index. Setting or returning"
+			" last value!\n");
 #endif
-	return value[i];
+	return value[vectorSize - 1];
 }
 
 template<typename T, uint16_t vectorSize>
-inline const T& LocalPoolVector<T, vectorSize>::operator [](int i) const {
-	if(i <= vectorSize) {
+inline const T& LocalPoolVector<T, vectorSize>::operator [](size_t i) const {
+	if(i < vectorSize) {
 		return value[i];
 	}
 	// If this happens, I have to set some value. I consider this
 	// a configuration error, but I wont exit here.
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	sif::error << "LocalPoolVector: Invalid index. Setting or returning"
+	sif::warning << "LocalPoolVector: Invalid index. Setting or returning"
 			" last value!" << std::endl;
+#else
+	fsfw::printWarning("LocalPoolVector: Invalid index. Setting or returning"
+			" last value!\n");
 #endif
-	return value[i];
+	return value[vectorSize - 1];
 }
 
 template<typename T, uint16_t vectorSize>
@@ -153,6 +159,7 @@ inline ReturnValue_t LocalPoolVector<T, vectorSize>::deSerialize(
 	return result;
 }
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 template<typename T, uint16_t vectorSize>
 inline std::ostream& operator<< (std::ostream &out,
         const LocalPoolVector<T, vectorSize> &var) {
@@ -166,5 +173,6 @@ inline std::ostream& operator<< (std::ostream &out,
     out << "]";
     return out;
 }
+#endif
 
 #endif /* FSFW_DATAPOOLLOCAL_LOCALPOOLVECTOR_TPP_ */
diff --git a/datapoollocal/PoolReadHelper.h b/datapoollocal/PoolReadHelper.h
index a059685b..c89fff98 100644
--- a/datapoollocal/PoolReadHelper.h
+++ b/datapoollocal/PoolReadHelper.h
@@ -9,10 +9,12 @@
  */
 class PoolReadHelper {
 public:
-	PoolReadHelper(ReadCommitIF* readObject, uint32_t mutexTimeout = 20):
+	PoolReadHelper(ReadCommitIF* readObject,
+			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+			uint32_t mutexTimeout = 20):
 			readObject(readObject), mutexTimeout(mutexTimeout) {
 		if(readObject != nullptr) {
-			readResult = readObject->read(mutexTimeout);
+			readResult = readObject->read(timeoutType, mutexTimeout);
 #if FSFW_PRINT_VERBOSITY_LEVEL == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "PoolReadHelper: Read failed!" << std::endl;
@@ -27,7 +29,7 @@ public:
 
 	~PoolReadHelper() {
 		if(readObject != nullptr) {
-			readObject->commit(mutexTimeout);
+			readObject->commit(timeoutType, mutexTimeout);
 		}
 
 	}
@@ -35,6 +37,7 @@ public:
 private:
 	ReadCommitIF* readObject = nullptr;
 	ReturnValue_t readResult = HasReturnvaluesIF::RETURN_OK;
+	MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
 	uint32_t mutexTimeout = 20;
 };
 
diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index 261e3d6d..e251729c 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/FSFWConfig.h
@@ -9,8 +9,7 @@
 //! the C stdio functions can be used alternatively
 #define FSFW_CPP_OSTREAM_ENABLED 	1
 
-//! More FSFW related printouts.
-//! Be careful, this also turns off most diagnostic prinouts!
+//! More FSFW related printouts. Useful for development.
 #define FSFW_ENHANCED_PRINTOUT		0
 
 //! Can be used to completely disable printouts, even the C stdio ones.
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 1af6932a..0efdeba0 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -2,7 +2,7 @@
 #include "AcceptsDeviceResponsesIF.h"
 #include "DeviceTmReportingWrapper.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../objectmanager/ObjectManager.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../thermal/ThermalComponentIF.h"
@@ -13,9 +13,6 @@
 #include "../subsystem/SubsystemBase.h"
 #include "../datapoollocal/LocalPoolVariable.h"
 
-#include <iomanip>
-
-
 object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
 object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT;
 object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT;
@@ -39,13 +36,8 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId,
 	cookieInfo.state = COOKIE_UNUSED;
 	cookieInfo.pendingCommand = deviceCommandMap.end();
 	if (comCookie == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "DeviceHandlerBase: ObjectID 0x" << std::hex
-				<< std::setw(8) << std::setfill('0') << this->getObjectId()
-				<< std::dec << ": Do not pass nullptr as a cookie, consider "
-				<< std::setfill(' ') << "passing a dummy cookie instead!"
-				<< std::endl;
-#endif
+		printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "DeviceHandlerBase",
+				HasReturnvaluesIF::RETURN_FAILED, "Invalid cookie");
 	}
 	if (this->fdirInstance == nullptr) {
 		this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId,
@@ -132,30 +124,24 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 	communicationInterface = objectManager->get<DeviceCommunicationIF>(
 			deviceCommunicationId);
 	if (communicationInterface == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "DeviceHandlerBase::initialize: Communication interface "
-				"invalid." << std::endl;
-		sif::error << "Make sure it is set up properly and implements"
-				" DeviceCommunicationIF" << std::endl;
-#endif
+		printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "initialize",
+				ObjectManagerIF::CHILD_INIT_FAILED,
+				"Passed communication IF invalid");
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	result = communicationInterface->initializeInterface(comCookie);
 	if (result != RETURN_OK) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-	    sif::error << "DeviceHandlerBase::initialize: Initializing "
-	            "communication interface failed!" << std::endl;
-#endif
+		printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "initialize",
+				ObjectManagerIF::CHILD_INIT_FAILED,
+				"ComIF initialization failed");
 	    return result;
 	}
 
 	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
 	if (IPCStore == nullptr) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "DeviceHandlerBase::initialize: IPC store not set up in "
-				"factory." << std::endl;
-#endif
+		printWarningOrError(fsfw::OutputTypes::OUT_ERROR, "initialize",
+				ObjectManagerIF::CHILD_INIT_FAILED, "IPC Store not set up");
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -164,11 +150,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 				AcceptsDeviceResponsesIF>(rawDataReceiverId);
 
 		if (rawReceiver == nullptr) {
+			printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+					"initialize", ObjectManagerIF::CHILD_INIT_FAILED,
+					"Raw receiver object ID set but no valid object found.");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "DeviceHandlerBase::initialize: Raw receiver object "
-					"ID set but no valid object found." << std::endl;
 			sif::error << "Make sure the raw receiver object is set up properly"
 					" and implements AcceptsDeviceResponsesIF" << std::endl;
+#else
+			fsfw::printError("Make sure the raw receiver object is set up "
+					"properly and implements AcceptsDeviceResponsesIF\n");
 #endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 		}
@@ -178,11 +168,15 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 	if(powerSwitcherId != objects::NO_OBJECT) {
 		powerSwitcher = objectManager->get<PowerSwitchIF>(powerSwitcherId);
 		if (powerSwitcher == nullptr) {
+			printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+					"initialize", ObjectManagerIF::CHILD_INIT_FAILED,
+					"Power switcher set but no valid object found.");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "DeviceHandlerBase::initialize: Power switcher "
-					<< "object ID set but no valid object found." << std::endl;
-			sif::error << "Make sure the raw receiver object is set up properly"
-					<< " and implements PowerSwitchIF" << std::endl;
+			sif::error << "Make sure the power switcher object is set up "
+					<< "properly and implements PowerSwitchIF" << std::endl;
+#else
+			fsfw::printError("Make sure the power switcher object is set up "
+					"properly and implements PowerSwitchIF\n");
 #endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 		}
@@ -229,7 +223,8 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 		if(result == HasReturnvaluesIF::RETURN_OK) {
 			thermalSet->heaterRequest.value =
 					ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL;
-			thermalSet->commit(PoolVariableIF::VALID);
+			thermalSet->heaterRequest.setValid(true);
+			thermalSet->commit();
 		}
 
 	}
@@ -555,17 +550,17 @@ void DeviceHandlerBase::replyReturnvalueToCommand(ReturnValue_t status,
 
 void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
 		uint32_t parameter) {
-//Check if we reply to a raw command.
+	// Check if we reply to a raw command.
 	if (cookieInfo.pendingCommand->first == RAW_COMMAND_ID) {
 		if (status == NO_REPLY_EXPECTED) {
 			status = RETURN_OK;
 		}
 		replyReturnvalueToCommand(status, parameter);
-		//Always delete data from a raw command.
+		// Always delete data from a raw command.
 		IPCStore->deleteData(storedRawData);
 		return;
 	}
-//Check if we were externally commanded.
+	// Check if we were externally commanded.
 	if (cookieInfo.pendingCommand->second.sendReplyTo != NO_COMMANDER) {
 		MessageQueueId_t queueId = cookieInfo.pendingCommand->second.sendReplyTo;
 		if (status == NO_REPLY_EXPECTED) {
@@ -580,15 +575,17 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status,
 
 void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
 		ReturnValue_t status) {
-//No need to check if iter exists, as this is checked by callers. If someone else uses the method, add check.
+	// No need to check if iter exists, as this is checked by callers.
+	// If someone else uses the method, add check.
 	if (iter->second.command == deviceCommandMap.end()) {
 		//Is most likely periodic reply. Silent return.
 		return;
 	}
-//Check if more replies are expected. If so, do nothing.
+	// Check if more replies are expected. If so, do nothing.
 	DeviceCommandInfo* info = &(iter->second.command->second);
 	if (--info->expectedReplies == 0) {
-		//Check if it was transition or internal command. Don't send any replies in that case.
+		// Check if it was transition or internal command.
+		// Don't send any replies in that case.
 		if (info->sendReplyTo != NO_COMMANDER) {
 			actionHelper.finish(info->sendReplyTo, iter->first, status);
 		}
@@ -605,7 +602,7 @@ void DeviceHandlerBase::doSendWrite() {
 		if (result == RETURN_OK) {
 			cookieInfo.state = COOKIE_WRITE_SENT;
 		} else {
-			//always generate a failure event, so that FDIR knows what's up
+			// always generate a failure event, so that FDIR knows what's up
 			triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result,
 					cookieInfo.pendingCommand->first);
 			replyToCommand(result);
@@ -720,10 +717,9 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 		case RETURN_OK:
 			handleReply(receivedData, foundId, foundLen);
 			if(foundLen == 0) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			    sif::warning << "DeviceHandlerBase::parseReply: foundLen is 0!"
-			            " Packet parsing will be stuck." << std::endl;
-#endif
+				printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+						"parseReply", ObjectManagerIF::CHILD_INIT_FAILED,
+						"Found length is one, parsing might be stuck");
 			}
 			break;
 		case APERIODIC_REPLY: {
@@ -734,6 +730,9 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 			            foundId);
 			}
 			if(foundLen == 0) {
+				printWarningOrError(fsfw::OutputTypes::OUT_ERROR,
+						"parseReply", ObjectManagerIF::CHILD_INIT_FAILED,
+						"Power switcher set but no valid object found.");
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 			    sif::warning << "DeviceHandlerBase::parseReply: foundLen is 0!"
 			            " Packet parsing will be stuck." << std::endl;
@@ -746,7 +745,8 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 		case IGNORE_FULL_PACKET:
 			return;
 		default:
-			//We need to wait for timeout.. don't know what command failed and who sent it.
+			// We need to wait for timeout.. don't know what command failed
+			// and who sent it.
 			replyRawReplyIfnotWiretapped(receivedData, foundLen);
 			triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen);
 			break;
@@ -967,7 +967,8 @@ ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) {
 }
 
 Mode_t DeviceHandlerBase::getBaseMode(Mode_t transitionMode) {
-//only child action special modes are handled, as a child should never see any base action modes
+	// only child action special modes are handled, as a child should
+	// never see any base action modes
 	if (transitionMode == _MODE_START_UP) {
 		return _MODE_TO_ON;
 	}
@@ -1290,12 +1291,11 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 	if (mode == MODE_NORMAL) {
 		result = buildNormalDeviceCommand(&deviceCommandId);
 		if (result == BUSY) {
-		    //so we can track misconfigurations
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::debug << std::hex << getObjectId()
-					<< ": DHB::buildInternalCommand: Busy" << std::dec
-					<< std::endl;
-#endif
+		    // so we can track misconfigurations
+			printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+					"buildInternalCommand",
+					HasReturnvaluesIF::RETURN_FAILED,
+					"Busy.");
 			result = NOTHING_TO_SEND; //no need to report this
 		}
 	}
@@ -1319,12 +1319,14 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 		if (iter == deviceCommandMap.end()) {
 			result = COMMAND_NOT_SUPPORTED;
 		} else if (iter->second.isExecuting) {
-			//so we can track misconfigurations
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::debug << std::hex << getObjectId()
-					<< ": DHB::buildInternalCommand: Command "
-					<< deviceCommandId << " isExecuting" << std::dec
-					<< std::endl;
+#if FSFW_DISABLE_PRINTOUT == 0
+			char output[36];
+			sprintf(output, "Command 0x%08x is executing", deviceCommandId);
+			// so we can track misconfigurations
+			printWarningOrError(fsfw::OutputTypes::OUT_WARNING,
+					"buildInternalCommand",
+					HasReturnvaluesIF::RETURN_FAILED,
+					output);
 #endif
 			// this is an internal command, no need to report a failure here,
 			// missed reply will track if a reply is too late, otherwise, it's ok
@@ -1484,3 +1486,48 @@ void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() {
 		}
 	}
 }
+
+void DeviceHandlerBase::printWarningOrError(fsfw::OutputTypes errorType,
+		const char *functionName, ReturnValue_t errorCode,
+		const char *errorPrint) {
+	if(errorPrint == nullptr) {
+		if(errorCode == ObjectManagerIF::CHILD_INIT_FAILED) {
+			errorPrint = "Initialization error";
+		}
+		if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
+			if(errorType == fsfw::OutputTypes::OUT_WARNING) {
+				errorPrint = "Generic Warning";
+			}
+			else {
+				errorPrint = "Generic Error";
+			}
+		}
+		else {
+			errorPrint = "Unknown error";
+		}
+	}
+
+	if(errorType == fsfw::OutputTypes::OUT_WARNING) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "DeviceHandlerBase::" << functionName << ": Object ID "
+				<< std::hex << std::setw(8) << std::setfill('0')
+				<< this->getObjectId() << " | " << errorPrint << std::dec
+				<< std::setfill(' ') << std::endl;
+#else
+		fsfw::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
+				this->getObjectId(), errorPrint);
+#endif
+	}
+	else if(errorType == fsfw::OutputTypes::OUT_ERROR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::error << "DeviceHandlerBase::" << functionName << ": Object ID "
+				<< std::hex << std::setw(8) << std::setfill('0')
+				<< this->getObjectId() << " | " << errorPrint << std::dec
+				<< std::setfill(' ') << std::endl;
+#else
+		fsfw::printError("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
+				this->getObjectId(), errorPrint);
+#endif
+	}
+
+}
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 24ba0372..4c745b4f 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -6,6 +6,8 @@
 #include "DeviceHandlerFailureIsolation.h"
 #include "DeviceHandlerThermalSet.h"
 
+#include "../serviceinterface/ServiceInterface.h"
+#include "../serviceinterface/serviceInterfaceDefintions.h"
 #include "../objectmanager/SystemObject.h"
 #include "../tasks/ExecutableObjectIF.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
@@ -1111,7 +1113,7 @@ private:
 	/**
 	 * @brief   The mode the current transition originated from
 	 *
-	 * This is private so the child can not change it and fuck up the timeouts
+	 * This is private so the child can not change it and mess up the timeouts
 	 *
 	 * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!!
 	 * (it is _MODE_POWER_DOWN during this modes)
@@ -1190,7 +1192,8 @@ private:
 	 * Check if the RMAP sendWrite action was successful.
 	 *
 	 * Depending on the result, the following is done
-	 * - if the device command was external commanded, a reply is sent indicating the result
+	 * - if the device command was external commanded, a reply is sent
+	 *   indicating the result
 	 * - if the action was successful, the reply timout counter is initialized
 	 */
 	void doGetWrite(void);
@@ -1206,9 +1209,9 @@ private:
 	/**
 	 * Check the getRead reply and the contained data.
 	 *
-	 * If data was received scanForReply() and, if successful, handleReply() are called.
-	 * If the current mode is @c MODE_RAW, the received packet is sent to the commanding object
-	 * via commandQueue.
+	 * If data was received scanForReply() and, if successful, handleReply()
+	 * are called. If the current mode is @c MODE_RAW, the received packet
+	 * is sent to the commanding object via commandQueue.
 	 */
 	void doGetRead(void);
 
@@ -1227,7 +1230,7 @@ private:
 			uint32_t *len);
 
 	/**
-	 * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW NOTHING ELSE!!!
+	 * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW, nothing else!
 	 */
 	void setTransition(Mode_t modeTo, Submode_t submodeTo);
 
@@ -1247,6 +1250,11 @@ private:
 
     void handleTransitionToOnMode(Mode_t commandedMode,
     		Submode_t commandedSubmode);
+
+    void printWarningOrError(fsfw::OutputTypes errorType,
+    		const char* functionName,
+			ReturnValue_t errorCode = HasReturnvaluesIF::RETURN_FAILED,
+			const char* errorPrint = nullptr);
 };
 
 #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */
diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 65e19d10..e25e574f 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -162,9 +162,15 @@ void EventManager::printEvent(EventMessage* message) {
 #endif
 
 void EventManager::lockMutex() {
-	mutex->lockMutex(MutexIF::BLOCKING);
+	mutex->lockMutex(timeoutType, timeoutMs);
 }
 
 void EventManager::unlockMutex() {
 	mutex->unlockMutex();
 }
+
+void EventManager::setMutexTimeout(MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
+	this->timeoutType = timeoutType;
+	this->timeoutMs = timeoutMs;
+}
diff --git a/events/EventManager.h b/events/EventManager.h
index c6bd07be..abce9b8b 100644
--- a/events/EventManager.h
+++ b/events/EventManager.h
@@ -29,6 +29,8 @@ public:
 	EventManager(object_id_t setObjectId);
 	virtual ~EventManager();
 
+	void setMutexTimeout(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs);
+
 	MessageQueueId_t getEventReportQueue();
 
 	ReturnValue_t registerListener(MessageQueueId_t listener, bool forwardAllButSelected = false);
@@ -51,6 +53,8 @@ protected:
 	std::map<MessageQueueId_t, EventMatchTree> listenerList;
 
 	MutexIF* mutex = nullptr;
+	MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+	uint32_t timeoutMs = 20;
 
 	static const uint8_t N_POOLS = 3;
 	LocalPool factoryBackend;
diff --git a/globalfunctions/arrayprinter.cpp b/globalfunctions/arrayprinter.cpp
index c273b250..ebf21bb8 100644
--- a/globalfunctions/arrayprinter.cpp
+++ b/globalfunctions/arrayprinter.cpp
@@ -1,5 +1,5 @@
 #include "arrayprinter.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include <bitset>
 
 void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
@@ -9,6 +9,8 @@ void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
 		sif::info << "Printing data with size " << size << ": ";
 	}
 	sif::info << "[";
+#else
+	fsfw::printInfo("Printing data with size %zu: [", size);
 #endif
 	if(type == OutputType::HEX) {
 		arrayprinter::printHex(data, size, maxCharPerLine);
@@ -37,6 +39,8 @@ void arrayprinter::printHex(const uint8_t *data, size_t size,
 	}
 	sif::info << std::dec;
 	sif::info << "]" << std::endl;
+#else
+	// how much memory to reserve for printout?
 #endif
 }
 
@@ -54,6 +58,8 @@ void arrayprinter::printDec(const uint8_t *data, size_t size,
 		}
 	}
 	sif::info << "]" << std::endl;
+#else
+	// how much memory to reserve for printout?
 #endif
 }
 
@@ -65,5 +71,7 @@ void arrayprinter::printBin(const uint8_t *data, size_t size) {
                 std::bitset<8>(data[i]) << ",\n" << std::flush;
     }
     sif::info << "]" << std::endl;
+#else
+    // how much memory to reserve for printout?
 #endif
 }
diff --git a/internalError/InternalErrorReporter.cpp b/internalError/InternalErrorReporter.cpp
index 48b6f851..6842da8e 100644
--- a/internalError/InternalErrorReporter.cpp
+++ b/internalError/InternalErrorReporter.cpp
@@ -23,7 +23,7 @@ void InternalErrorReporter::setDiagnosticPrintout(bool enable) {
 }
 
 ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
-    internalErrorDataset.read(INTERNAL_ERROR_MUTEX_TIMEOUT);
+    internalErrorDataset.read(timeoutType, timeoutMs);
 
 	uint32_t newQueueHits = getAndResetQueueHits();
 	uint32_t newTmHits = getAndResetTmHits();
@@ -46,8 +46,8 @@ ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
 	internalErrorDataset.queueHits.value += newQueueHits;
 	internalErrorDataset.storeHits.value += newStoreHits;
 	internalErrorDataset.tmHits.value += newTmHits;
-
-	internalErrorDataset.commit(INTERNAL_ERROR_MUTEX_TIMEOUT);
+	internalErrorDataset.setValidity(true, true);
+	internalErrorDataset.commit(timeoutType, timeoutMs);
 
 	poolManager.performHkOperation();
 
@@ -69,7 +69,7 @@ void InternalErrorReporter::lostTm() {
 
 uint32_t InternalErrorReporter::getAndResetQueueHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = queueHits;
 	queueHits = 0;
 	mutex->unlockMutex();
@@ -78,21 +78,21 @@ uint32_t InternalErrorReporter::getAndResetQueueHits() {
 
 uint32_t InternalErrorReporter::getQueueHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = queueHits;
 	mutex->unlockMutex();
 	return value;
 }
 
 void InternalErrorReporter::incrementQueueHits() {
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	queueHits++;
 	mutex->unlockMutex();
 }
 
 uint32_t InternalErrorReporter::getAndResetTmHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = tmHits;
 	tmHits = 0;
 	mutex->unlockMutex();
@@ -101,14 +101,14 @@ uint32_t InternalErrorReporter::getAndResetTmHits() {
 
 uint32_t InternalErrorReporter::getTmHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = tmHits;
 	mutex->unlockMutex();
 	return value;
 }
 
 void InternalErrorReporter::incrementTmHits() {
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	tmHits++;
 	mutex->unlockMutex();
 }
@@ -119,7 +119,7 @@ void InternalErrorReporter::storeFull() {
 
 uint32_t InternalErrorReporter::getAndResetStoreHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = storeHits;
 	storeHits = 0;
 	mutex->unlockMutex();
@@ -128,14 +128,14 @@ uint32_t InternalErrorReporter::getAndResetStoreHits() {
 
 uint32_t InternalErrorReporter::getStoreHits() {
 	uint32_t value;
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	value = storeHits;
 	mutex->unlockMutex();
 	return value;
 }
 
 void InternalErrorReporter::incrementStoreHits() {
-	mutex->lockMutex(MutexIF::WAITING, INTERNAL_ERROR_MUTEX_TIMEOUT);
+	mutex->lockMutex(timeoutType, timeoutMs);
 	storeHits++;
 	mutex->unlockMutex();
 }
@@ -190,3 +190,8 @@ ReturnValue_t InternalErrorReporter::initializeAfterTaskCreation() {
     return poolManager.initializeAfterTaskCreation();
 }
 
+void InternalErrorReporter::setMutexTimeout(MutexIF::TimeoutType timeoutType,
+		uint32_t timeoutMs) {
+	this->timeoutType = timeoutType;
+	this->timeoutMs = timeoutMs;
+}
diff --git a/internalError/InternalErrorReporter.h b/internalError/InternalErrorReporter.h
index 8d33c06e..c85057d6 100644
--- a/internalError/InternalErrorReporter.h
+++ b/internalError/InternalErrorReporter.h
@@ -22,7 +22,6 @@ class InternalErrorReporter: public SystemObject,
 		public InternalErrorReporterIF,
 		public HasLocalDataPoolIF {
 public:
-    static constexpr uint8_t INTERNAL_ERROR_MUTEX_TIMEOUT = 20;
 
 	InternalErrorReporter(object_id_t setObjectId,
 	        uint32_t messageQueueDepth = 5);
@@ -34,6 +33,9 @@ public:
 	 */
 	void setDiagnosticPrintout(bool enable);
 
+	void setMutexTimeout(MutexIF::TimeoutType timeoutType,
+			uint32_t timeoutMs);
+
 	virtual ~InternalErrorReporter();
 
     virtual object_id_t getObjectId() const override;
@@ -61,7 +63,11 @@ protected:
 	LocalDataPoolManager poolManager;
 
 	PeriodicTaskIF* executingTask = nullptr;
+
 	MutexIF* mutex = nullptr;
+	MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+	uint32_t timeoutMs = 20;
+
 	sid_t internalErrorSid;
 	InternalErrorDataset internalErrorDataset;
 
diff --git a/ipc/MutexIF.h b/ipc/MutexIF.h
index 5673e4d7..084b702b 100644
--- a/ipc/MutexIF.h
+++ b/ipc/MutexIF.h
@@ -14,7 +14,7 @@ public:
     /**
      * Different types of timeout for the mutex lock.
      */
-    enum TimeoutType {
+    enum class TimeoutType {
         POLLING, //!< If mutex is not available, return immediately
         WAITING, //!< Wait a specified time for the mutex to become available
         BLOCKING //!< Block indefinitely until the mutex becomes available.
diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp
index d3f4e68e..9c0a0267 100644
--- a/osal/FreeRTOS/Clock.cpp
+++ b/osal/FreeRTOS/Clock.cpp
@@ -163,7 +163,7 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
 	if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
@@ -178,7 +178,7 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
 	if (timeMutex == NULL) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index 2bc78785..88bf51d2 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -202,7 +202,7 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
 	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex();
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
@@ -217,7 +217,7 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
 	if(timeMutex == nullptr){
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex();
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
diff --git a/osal/host/Mutex.cpp b/osal/host/Mutex.cpp
index ad8873df..892028b2 100644
--- a/osal/host/Mutex.cpp
+++ b/osal/host/Mutex.cpp
@@ -4,16 +4,16 @@
 Mutex::Mutex() {}
 
 ReturnValue_t Mutex::lockMutex(TimeoutType timeoutType, uint32_t timeoutMs) {
-	if(timeoutType == MutexIF::BLOCKING) {
+	if(timeoutType == TimeoutType::BLOCKING) {
 		mutex.lock();
 		return HasReturnvaluesIF::RETURN_OK;
 	}
-	else if(timeoutType == MutexIF::POLLING) {
+	else if(timeoutType == TimeoutType::POLLING) {
 		if(mutex.try_lock()) {
 			return HasReturnvaluesIF::RETURN_OK;
 		}
 	}
-	else if(timeoutMs > MutexIF::POLLING){
+	else if(timeoutType == TimeoutType::WAITING){
 		auto chronoMs = std::chrono::milliseconds(timeoutMs);
 		if(mutex.try_lock_for(chronoMs)) {
 			return HasReturnvaluesIF::RETURN_OK;
diff --git a/osal/host/Mutex.h b/osal/host/Mutex.h
index c0fa19b7..0bd93c8a 100644
--- a/osal/host/Mutex.h
+++ b/osal/host/Mutex.h
@@ -22,7 +22,6 @@ public:
 
 	std::timed_mutex* getMutexHandle();
 private:
-	//bool locked = false;
 	std::timed_mutex mutex;
 };
 
diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp
index 54dad839..35cbfae0 100644
--- a/osal/linux/Clock.cpp
+++ b/osal/linux/Clock.cpp
@@ -182,7 +182,7 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
 	if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
@@ -197,7 +197,7 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
 	if(timeMutex==NULL){
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING);
+	ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
@@ -209,13 +209,13 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
 }
 
 ReturnValue_t Clock::checkOrCreateClockMutex(){
-	if(timeMutex==NULL){
+	if(timeMutex == nullptr){
 		MutexFactory* mutexFactory = MutexFactory::instance();
-		if (mutexFactory == NULL) {
+		if (mutexFactory == nullptr) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 		timeMutex = mutexFactory->createMutex();
-		if (timeMutex == NULL) {
+		if (timeMutex == nullptr) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 	}
diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp
index dda15464..8bd7ac37 100644
--- a/osal/rtems/Clock.cpp
+++ b/osal/rtems/Clock.cpp
@@ -37,15 +37,24 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::setClock(const timeval* time) {
-	//TODO This routine uses _TOD_Set which is not
 	timespec newTime;
 	newTime.tv_sec = time->tv_sec;
+	if(time->tv_usec < 0) {
+		// better returnvalue.
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
 	newTime.tv_nsec = time->tv_usec * TOD_NANOSECONDS_PER_MICROSECOND;
-	//SHOULDDO: Not sure if we need to protect this call somehow (by thread lock or something).
-	//Uli: rtems docu says you can call this from an ISR, not sure if this means no protetion needed
-	//TODO Second parameter is ISR_lock_Context
-	_TOD_Set(&newTime,nullptr);
-	return HasReturnvaluesIF::RETURN_OK;
+
+	ISR_lock_Context context;
+	_TOD_Lock();
+	_TOD_Acquire(&context);
+	Status_Control status = _TOD_Set(&newTime, &context);
+	_TOD_Unlock();
+	if(status == STATUS_SUCCESSFUL) {
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+	// better returnvalue
+	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
 ReturnValue_t Clock::getClock_timeval(timeval* time) {
@@ -95,6 +104,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 }
 
 ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
+	// TIsn't this a bug? Are RTEMS ticks always microseconds?
 	rtems_time_of_day* timeRtems = reinterpret_cast<rtems_time_of_day*>(time);
 	rtems_status_code status = rtems_clock_get_tod(timeRtems);
 	switch (status) {
diff --git a/osal/rtems/Mutex.cpp b/osal/rtems/Mutex.cpp
index 71c5782f..7dd512fa 100644
--- a/osal/rtems/Mutex.cpp
+++ b/osal/rtems/Mutex.cpp
@@ -30,11 +30,11 @@ Mutex::~Mutex() {
 ReturnValue_t Mutex::lockMutex(TimeoutType timeoutType =
         TimeoutType::BLOCKING, uint32_t timeoutMs) {
 	rtems_status_code status = RTEMS_INVALID_ID;
-	if(timeoutMs == MutexIF::TimeoutType::BLOCKING) {
+	if(timeoutType == MutexIF::TimeoutType::BLOCKING) {
 		status = rtems_semaphore_obtain(mutexId,
 				RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 	}
-	else if(timeoutMs == MutexIF::TimeoutType::POLLING) {
+	else if(timeoutType == MutexIF::TimeoutType::POLLING) {
 		timeoutMs = RTEMS_NO_TIMEOUT;
 		status = rtems_semaphore_obtain(mutexId,
 				RTEMS_NO_WAIT, 0);
diff --git a/power/Fuse.cpp b/power/Fuse.cpp
index 1c30d83f..b6e0a7f1 100644
--- a/power/Fuse.cpp
+++ b/power/Fuse.cpp
@@ -65,8 +65,8 @@ ReturnValue_t Fuse::check() {
 	set.read();
 	if (!healthHelper.healthTable->isHealthy(getObjectId())) {
 		setAllMonitorsToUnchecked();
-		set.commit(PoolVariableIF::INVALID);
-		return RETURN_OK;
+		set.setValidity(false, true);
+		return set.commit();
 	}
 	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
 	checkFuseState();
@@ -206,7 +206,8 @@ float Fuse::getPower() {
 
 void Fuse::setDataPoolEntriesInvalid() {
 	set.read();
-	set.commit(PoolVariableIF::INVALID);
+	set.setValidity(false, true);
+	set.commit();
 }
 
 ReturnValue_t Fuse::getParameter(uint8_t domainId, uint16_t parameterId,
diff --git a/power/PowerSensor.cpp b/power/PowerSensor.cpp
index 1ef041e3..fb181d9f 100644
--- a/power/PowerSensor.cpp
+++ b/power/PowerSensor.cpp
@@ -97,7 +97,8 @@ void PowerSensor::checkCommandQueue() {
 
 void PowerSensor::setDataPoolEntriesInvalid() {
 	powerSensorSet.read();
-	powerSensorSet.commit(PoolVariableIF::INVALID);
+	powerSensorSet.setValidity(false, true);
+	powerSensorSet.commit();
 }
 
 float PowerSensor::getPower() {
diff --git a/serviceinterface/ServiceInterface.h b/serviceinterface/ServiceInterface.h
index 0c7c4383..1f7e5e84 100644
--- a/serviceinterface/ServiceInterface.h
+++ b/serviceinterface/ServiceInterface.h
@@ -2,6 +2,7 @@
 #define FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_
 
 #include <FSFWConfig.h>
+#include "serviceInterfaceDefintions.h"
 
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp
index 20ea61dd..5846d233 100644
--- a/serviceinterface/ServiceInterfaceBuffer.cpp
+++ b/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -28,18 +28,21 @@ ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage,
 	}
 
 #if FSFW_COLORED_OUTPUT == 1
-	if(setMessage.find("DEBUG")) {
+	if(setMessage.find("DEBUG") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_MAGENTA;
 	}
-	else if(setMessage.find("INFO")) {
+	else if(setMessage.find("INFO") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_GREEN;
 	}
-	else if(setMessage.find("WARNING")) {
+	else if(setMessage.find("WARNING") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_YELLOW;
 	}
-	else if(setMessage.find("ERROR")) {
+	else if(setMessage.find("ERROR") != std::string::npos) {
 		colorPrefix = fsfw::ANSI_COLOR_RED;
 	}
+	else {
+		colorPrefix = fsfw::ANSI_COLOR_RESET;
+	}
 
 #ifdef WIN32
 	HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
diff --git a/serviceinterface/ServiceInterfacePrinter.cpp b/serviceinterface/ServiceInterfacePrinter.cpp
index e8eb498f..cb3a958d 100644
--- a/serviceinterface/ServiceInterfacePrinter.cpp
+++ b/serviceinterface/ServiceInterfacePrinter.cpp
@@ -6,12 +6,14 @@
 #include <cstdarg>
 #include <cstdint>
 
-
 fsfw::PrintLevel printLevel = fsfw::PrintLevel::DEBUG;
-uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE];
 #if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
 bool consoleInitialized = false;
-#endif
+#endif /* defined(WIN32) && FSFW_COLORED_OUTPUT == 1 */
+bool addCrAtEnd = false;
+
+#if FSFW_DISABLE_PRINTOUT == 0
+uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE];
 
 void fsfwPrint(fsfw::PrintLevel printType, const char* fmt, va_list arg) {
 
@@ -78,16 +80,13 @@ void fsfwPrint(fsfw::PrintLevel printType, const char* fmt, va_list arg) {
 
     len += vsnprintf(bufferPosition + len, sizeof(printBuffer) - len, fmt, arg);
 
+    if(addCrAtEnd) {
+    	len += sprintf(bufferPosition + len, "\r");
+    }
+
     printf("%s", printBuffer);
 }
 
-void fsfw::setPrintLevel(PrintLevel printLevel_) {
-	printLevel = printLevel_;
-}
-
-fsfw::PrintLevel fsfw::getPrintLevel() {
-	return printLevel;
-}
 
 void fsfw::printInfo(const char *fmt, ...) {
     va_list args;
@@ -110,9 +109,30 @@ void fsfw::printDebug(const char *fmt, ...) {
     va_end(args);
 }
 
+void fsfw::setToAddCrAtEnd(bool addCrAtEnd_) {
+	addCrAtEnd = addCrAtEnd_;
+}
+
 void fsfw::printError(const char *fmt, ...) {
     va_list args;
     va_start(args, fmt);
     fsfwPrint(fsfw::PrintLevel::ERROR_TYPE, fmt, args);
     va_end(args);
 }
+
+#else
+
+void fsfw::printInfo(const char *fmt, ...) {}
+void fsfw::printWarning(const char *fmt, ...) {}
+void fsfw::printDebug(const char *fmt, ...) {}
+void fsfw::printError(const char *fmt, ...) {}
+
+#endif /* FSFW_DISABLE_PRINTOUT == 0 */
+
+void fsfw::setPrintLevel(PrintLevel printLevel_) {
+	printLevel = printLevel_;
+}
+
+fsfw::PrintLevel fsfw::getPrintLevel() {
+	return printLevel;
+}
diff --git a/serviceinterface/ServiceInterfacePrinter.h b/serviceinterface/ServiceInterfacePrinter.h
index 8b81ee7a..fdff797a 100644
--- a/serviceinterface/ServiceInterfacePrinter.h
+++ b/serviceinterface/ServiceInterfacePrinter.h
@@ -1,8 +1,10 @@
-#include <stdio.h>
+#if FSFW_DISABLE_PRINTOUT == 0
+#include <cstdio>
+#endif
 
 namespace fsfw {
 
-enum class PrintLevel {
+enum PrintLevel {
 	NONE = 0,
 	//! Strange error when using just ERROR..
 	ERROR_TYPE = 1,
@@ -11,9 +13,17 @@ enum class PrintLevel {
 	DEBUG = 4
 };
 
+/**
+ * Set the print level. All print types with a smaller level will be printed
+ * as well. For example, set to PrintLevel::WARNING to only enable error
+ * and warning output.
+ * @param printLevel
+ */
 void setPrintLevel(PrintLevel printLevel);
 PrintLevel getPrintLevel();
 
+void setToAddCrAtEnd(bool addCrAtEnd_);
+
 /**
  * These functions can be used like the C stdio printf and forward the
  * supplied formatted string arguments to a printf function.
diff --git a/serviceinterface/serviceInterfaceDefintions.h b/serviceinterface/serviceInterfaceDefintions.h
index 684c7366..de445907 100644
--- a/serviceinterface/serviceInterfaceDefintions.h
+++ b/serviceinterface/serviceInterfaceDefintions.h
@@ -3,6 +3,13 @@
 
 namespace fsfw {
 
+enum class OutputTypes {
+	OUT_INFO,
+	OUT_DEBUG,
+	OUT_WARNING,
+	OUT_ERROR
+};
+
 static const char* const ANSI_COLOR_RED = "\x1b[31m";
 static const char* const ANSI_COLOR_GREEN = "\x1b[32m";
 static const char* const ANSI_COLOR_YELLOW = "\x1b[33m";
diff --git a/unittest/internal/UnittDefinitions.cpp b/unittest/internal/UnittDefinitions.cpp
index 7138e6c6..d20771a4 100644
--- a/unittest/internal/UnittDefinitions.cpp
+++ b/unittest/internal/UnittDefinitions.cpp
@@ -3,7 +3,9 @@
  ReturnValue_t unitt::put_error(std::string errorId) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "Unit Tester error: Failed at test ID "
-			<< errorId << "\n" << std::flush;
-#endif
+			<< errorId << std::endl;
+#else
+	fsfw::printError("Unit Tester error: Failed at test ID 0x%08x", errorId);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
diff --git a/unittest/internal/UnittDefinitions.h b/unittest/internal/UnittDefinitions.h
index ea36fea4..3e14fec5 100644
--- a/unittest/internal/UnittDefinitions.h
+++ b/unittest/internal/UnittDefinitions.h
@@ -2,9 +2,10 @@
 #define UNITTEST_INTERNAL_UNITTDEFINITIONS_H_
 
 #include "../../returnvalues/HasReturnvaluesIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../serviceinterface/ServiceInterface.h"
 #include <cstdint>
 #include <cstddef>
+#include <string>
 
 namespace tv {
 // POD test values
diff --git a/unittest/internal/osal/IntTestMutex.cpp b/unittest/internal/osal/IntTestMutex.cpp
index 3316de74..2a1584b8 100644
--- a/unittest/internal/osal/IntTestMutex.cpp
+++ b/unittest/internal/osal/IntTestMutex.cpp
@@ -13,7 +13,7 @@
 void testmutex::testMutex() {
 	std::string id = "[testMutex]";
 	MutexIF* mutex = MutexFactory::instance()->createMutex();
-	auto result = mutex->lockMutex(MutexIF::POLLING);
+	auto result = mutex->lockMutex(MutexIF::TimeoutType::POLLING);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 		unitt::put_error(id);
 	}
diff --git a/unittest/tests/CMakeLists.txt b/unittest/tests/CMakeLists.txt
index 97074417..180e1a51 100644
--- a/unittest/tests/CMakeLists.txt
+++ b/unittest/tests/CMakeLists.txt
@@ -2,5 +2,6 @@ add_subdirectory(action)
 add_subdirectory(container)
 add_subdirectory(osal)
 add_subdirectory(serialize)
+add_subdirectory(datapoollocal)
 add_subdirectory(storagemanager)
 
diff --git a/unittest/tests/action/TestActionHelper.cpp b/unittest/tests/action/TestActionHelper.cpp
index 3109eba1..3957bba2 100644
--- a/unittest/tests/action/TestActionHelper.cpp
+++ b/unittest/tests/action/TestActionHelper.cpp
@@ -7,6 +7,8 @@
 
 #include <catch2/catch_test_macros.hpp>
 
+#include <array>
+
 
 TEST_CASE( "Action Helper" , "[ActionHelper]") {
 	ActionHelperOwnerMockBase testDhMock;
diff --git a/unittest/tests/container/TestPlacementFactory.cpp b/unittest/tests/container/TestPlacementFactory.cpp
index 5edbb9d2..14cf8eb4 100644
--- a/unittest/tests/container/TestPlacementFactory.cpp
+++ b/unittest/tests/container/TestPlacementFactory.cpp
@@ -1,45 +1,48 @@
-//#include <fsfw/container/PlacementFactory.h>
-//#include <fsfw/storagemanager/LocalPool.h>
-//#include <fsfw/returnvalues/HasReturnvaluesIF.h>
-//#include <fsfw/container/ArrayList.h>
-//
-//#include <catch2/catch.hpp>
-//#include "../../core/CatchDefinitions.h"
-//
-//TEST_CASE( "PlacementFactory Tests", "[TestPlacementFactory]") {
-//	INFO("PlacementFactory Tests");
-//
-//	const uint16_t element_sizes[3] = {sizeof(uint16_t), sizeof(uint32_t), sizeof(uint64_t)};
-//	const uint16_t n_elements[3] = {1, 1, 1};
-//	LocalPool<3> storagePool(0x1, element_sizes, n_elements, false, true);
-//	PlacementFactory factory(&storagePool);
-//
-//	SECTION("Pool overload"){
-//		store_address_t address;
-//		uint8_t* ptr = nullptr;
-//		REQUIRE(storagePool.getFreeElement(&address, sizeof(ArrayList<uint32_t, uint16_t>), &ptr)
-//				== static_cast<int>(StorageManagerIF::DATA_TOO_LARGE));
-//		ArrayList<uint32_t, uint16_t>* list2 = factory.generate<ArrayList<uint32_t, uint16_t> >(80);
-//		REQUIRE(list2 == nullptr);
-//	}
-//
-//	SECTION("Test generate and destroy"){
-//		uint64_t* number = factory.generate<uint64_t>(32000);
-//		REQUIRE(number != nullptr);
-//		REQUIRE(*number == 32000);
-//		store_address_t address;
-//		uint8_t* ptr = nullptr;
-//		REQUIRE(storagePool.getFreeElement(&address, sizeof(uint64_t), &ptr)
-//						== static_cast<int>(StorageManagerIF::DATA_TOO_LARGE));
-//		uint64_t* number2 = factory.generate<uint64_t>(12345);
-//		REQUIRE(number2 == nullptr);
-//		REQUIRE(factory.destroy(number) == static_cast<int>(HasReturnvaluesIF::RETURN_OK));
-//		REQUIRE(storagePool.getFreeElement(&address, sizeof(uint64_t), &ptr)
-//						== static_cast<int>(HasReturnvaluesIF::RETURN_OK));
-//		REQUIRE(storagePool.deleteData(address) == static_cast<int>(HasReturnvaluesIF::RETURN_OK));
-//
-//		//Check that PlacementFactory checks for nullptr
-//		ptr = nullptr;
-//		REQUIRE(factory.destroy(ptr) == static_cast<int>(HasReturnvaluesIF::RETURN_FAILED));
-//	}
-//}
+#include <fsfw/container/PlacementFactory.h>
+#include <fsfw/storagemanager/LocalPool.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <fsfw/container/ArrayList.h>
+
+#include <catch2/catch_test_macros.hpp>
+#include <unittest/core/CatchDefinitions.h>
+
+TEST_CASE( "PlacementFactory Tests", "[TestPlacementFactory]") {
+	INFO("PlacementFactory Tests");
+
+	LocalPool::LocalPoolConfig poolCfg= {{1, sizeof(uint16_t)},
+			{1, sizeof(uint32_t)}, {1, sizeof(uint64_t)}
+	};
+	//const uint16_t element_sizes[3] = {sizeof(uint16_t), sizeof(uint32_t), sizeof(uint64_t)};
+	//const uint16_t n_elements[3] = {1, 1, 1};
+	LocalPool storagePool(0x1, poolCfg, false, true);
+	PlacementFactory factory(&storagePool);
+
+	SECTION("Pool overload"){
+		store_address_t address;
+		uint8_t* ptr = nullptr;
+		REQUIRE(storagePool.getFreeElement(&address, sizeof(ArrayList<uint32_t, uint16_t>), &ptr)
+				== static_cast<int>(StorageManagerIF::DATA_TOO_LARGE));
+		ArrayList<uint32_t, uint16_t>* list2 = factory.generate<ArrayList<uint32_t, uint16_t> >(80);
+		REQUIRE(list2 == nullptr);
+	}
+
+	SECTION("Test generate and destroy"){
+		uint64_t* number = factory.generate<uint64_t>(32000);
+		REQUIRE(number != nullptr);
+		REQUIRE(*number == 32000);
+		store_address_t address;
+		uint8_t* ptr = nullptr;
+		REQUIRE(storagePool.getFreeElement(&address, sizeof(uint64_t), &ptr)
+						== static_cast<int>(StorageManagerIF::DATA_TOO_LARGE));
+		uint64_t* number2 = factory.generate<uint64_t>(12345);
+		REQUIRE(number2 == nullptr);
+		REQUIRE(factory.destroy(number) == static_cast<int>(HasReturnvaluesIF::RETURN_OK));
+		REQUIRE(storagePool.getFreeElement(&address, sizeof(uint64_t), &ptr)
+						== static_cast<int>(HasReturnvaluesIF::RETURN_OK));
+		REQUIRE(storagePool.deleteData(address) == static_cast<int>(HasReturnvaluesIF::RETURN_OK));
+
+		//Check that PlacementFactory checks for nullptr
+		ptr = nullptr;
+		REQUIRE(factory.destroy(ptr) == static_cast<int>(HasReturnvaluesIF::RETURN_FAILED));
+	}
+}
diff --git a/unittest/tests/datapoollocal/CMakeLists.txt b/unittest/tests/datapoollocal/CMakeLists.txt
new file mode 100644
index 00000000..fd7f61b8
--- /dev/null
+++ b/unittest/tests/datapoollocal/CMakeLists.txt
@@ -0,0 +1,5 @@
+target_sources(${TARGET_NAME} PRIVATE
+	LocalPoolVariableTest.cpp
+	LocalPoolVectorTest.cpp
+	DataSetTest.cpp
+)
diff --git a/unittest/tests/datapoollocal/DataSetTest.cpp b/unittest/tests/datapoollocal/DataSetTest.cpp
new file mode 100644
index 00000000..968a0b3d
--- /dev/null
+++ b/unittest/tests/datapoollocal/DataSetTest.cpp
@@ -0,0 +1,23 @@
+#include "LocalPoolOwnerBase.h"
+
+#include <catch2/catch_test_macros.hpp>
+#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <unittest/core/CatchDefinitions.h>
+
+TEST_CASE("LocalDataSet" , "[LocDataSetTest]") {
+	LocalPoolOwnerBase* poolOwner = objectManager->
+			get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
+	REQUIRE(poolOwner != nullptr);
+	REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
+	REQUIRE(poolOwner->initializeHkManagerAfterTaskCreation()
+			== retval::CATCH_OK);
+	const uint32_t setId = 0;
+	SECTION("BasicTest") {
+		StaticLocalDataSet<3> localSet = StaticLocalDataSet<3>(
+				sid_t(objects::TEST_LOCAL_POOL_OWNER_BASE, setId));
+	}
+}
+
+
+
diff --git a/unittest/tests/datapoollocal/LocalPoolOwnerBase.h b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h
new file mode 100644
index 00000000..c665cb7f
--- /dev/null
+++ b/unittest/tests/datapoollocal/LocalPoolOwnerBase.h
@@ -0,0 +1,114 @@
+#ifndef FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_
+#define FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_
+
+#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/datapoollocal/LocalPoolVector.h>
+#include <fsfw/ipc/QueueFactory.h>
+#include <testcfg/objects/systemObjectList.h>
+
+namespace lpool {
+static constexpr lp_id_t uint8VarId = 0;
+static constexpr lp_id_t floatVarId = 1;
+static constexpr lp_id_t uint32VarId = 2;
+static constexpr lp_id_t uint16Vec3Id = 3;
+static constexpr lp_id_t int64Vec2Id = 4;
+}
+
+
+
+class LocalPoolOwnerBase: public SystemObject, public HasLocalDataPoolIF {
+public:
+	LocalPoolOwnerBase(
+			object_id_t objectId = objects::TEST_LOCAL_POOL_OWNER_BASE):
+		SystemObject(objectId), hkManager(this, messageQueue)  {
+		messageQueue = QueueFactory::instance()->createMessageQueue(10);
+	}
+
+	~LocalPoolOwnerBase() {
+		QueueFactory::instance()->deleteMessageQueue(messageQueue);
+	}
+
+	object_id_t getObjectId() const override {
+		return SystemObject::getObjectId();
+	}
+
+	ReturnValue_t initializeHkManager() {
+		if(not initialized) {
+			initialized = true;
+			return hkManager.initialize(messageQueue);
+		}
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+	ReturnValue_t initializeHkManagerAfterTaskCreation() {
+		if(not initializedAfterTaskCreation) {
+			initializedAfterTaskCreation = true;
+			return hkManager.initializeAfterTaskCreation();
+		}
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+	/** Command queue for housekeeping messages. */
+	MessageQueueId_t getCommandQueue() const override {
+		return messageQueue->getId();
+	}
+
+	// This is called by initializeAfterTaskCreation of the HK manager.
+	virtual ReturnValue_t initializeLocalDataPool(
+	        LocalDataPool& localDataPoolMap,
+	        LocalDataPoolManager& poolManager) {
+		// Default initialization empty for now.
+		localDataPoolMap.emplace(lpool::uint8VarId,
+				new PoolEntry<uint8_t>({0}));
+		localDataPoolMap.emplace(lpool::floatVarId,
+				new PoolEntry<float>({0}));
+		localDataPoolMap.emplace(lpool::uint32VarId,
+				new PoolEntry<uint32_t>({0}));
+
+		localDataPoolMap.emplace(lpool::uint16Vec3Id,
+				new PoolEntry<uint16_t>({0, 0, 0}));
+		localDataPoolMap.emplace(lpool::int64Vec2Id,
+				new PoolEntry<int64_t>({0, 0}));
+		return HasReturnvaluesIF::RETURN_OK;
+	}
+
+	LocalDataPoolManager* getHkManagerHandle() override {
+		return &hkManager;
+	}
+
+	uint32_t getPeriodicOperationFrequency() const override {
+		return 0;
+	}
+
+	/**
+	 * This function is used by the pool manager to get a valid dataset
+     * from a SID
+	 * @param sid Corresponding structure ID
+	 * @return
+	 */
+	virtual LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override  {
+		// empty for now
+		return nullptr;
+	}
+private:
+
+	lp_var_t<uint8_t> testUint8 = lp_var_t<uint8_t>(this, lpool::uint8VarId);
+	lp_var_t<float> testFloat = lp_var_t<float>(this, lpool::floatVarId);
+	lp_var_t<uint32_t> testUint32 = lp_var_t<uint32_t>(this, lpool::uint32VarId);
+
+	lp_vec_t<uint16_t, 3> testUint16Vec = lp_vec_t<uint16_t, 3>(this,
+			lpool::uint16Vec3Id);
+	lp_vec_t<int64_t, 2> testInt64Vec = lp_vec_t<int64_t, 2>(this,
+			lpool::int64Vec2Id);
+
+	MessageQueueIF* messageQueue = nullptr;
+	LocalDataPoolManager hkManager;
+
+	bool initialized = false;
+	bool initializedAfterTaskCreation = false;
+
+};
+
+#endif /* FSFW_UNITTEST_TESTS_DATAPOOLLOCAL_LOCALPOOLOWNERBASE_H_ */
diff --git a/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp b/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp
new file mode 100644
index 00000000..5f7118ff
--- /dev/null
+++ b/unittest/tests/datapoollocal/LocalPoolVariableTest.cpp
@@ -0,0 +1,123 @@
+#include "LocalPoolOwnerBase.h"
+
+#include <catch2/catch_test_macros.hpp>
+#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
+#include <unittest/core/CatchDefinitions.h>
+
+
+TEST_CASE("LocalPoolVariable" , "[LocPoolVarTest]") {
+	LocalPoolOwnerBase* poolOwner = objectManager->
+			get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
+	REQUIRE(poolOwner != nullptr);
+	REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
+	REQUIRE(poolOwner->initializeHkManagerAfterTaskCreation()
+			== retval::CATCH_OK);
+
+	SECTION("Basic Tests") {
+		// very basic test.
+		lp_var_t<uint8_t> testVariable = lp_var_t<uint8_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint8VarId);
+		REQUIRE(testVariable.read() == retval::CATCH_OK);
+		CHECK(testVariable.value == 0);
+		testVariable.value = 5;
+		REQUIRE(testVariable.commit() == retval::CATCH_OK);
+		REQUIRE(testVariable.read() == retval::CATCH_OK);
+		REQUIRE(testVariable.value == 5);
+		CHECK(not testVariable.isValid());
+		testVariable.setValid(true);
+		CHECK(testVariable.isValid());
+		CHECK(testVariable.commit(true) == retval::CATCH_OK);
+
+		testVariable.setReadWriteMode(pool_rwm_t::VAR_READ);
+		CHECK(testVariable.getReadWriteMode() == pool_rwm_t::VAR_READ);
+		testVariable.setReadWriteMode(pool_rwm_t::VAR_READ_WRITE);
+
+		testVariable.setDataPoolId(22);
+		CHECK(testVariable.getDataPoolId() == 22);
+		testVariable.setDataPoolId(lpool::uint8VarId);
+
+		testVariable.setChanged(true);
+		CHECK(testVariable.hasChanged());
+		testVariable.setChanged(false);
+
+		gp_id_t globPoolId(objects::TEST_LOCAL_POOL_OWNER_BASE,
+				lpool::uint8VarId);
+		lp_var_t<uint8_t> testVariable2 = lp_var_t<uint8_t>(globPoolId);
+		REQUIRE(testVariable2.read() == retval::CATCH_OK);
+		CHECK(testVariable2 == 5);
+		CHECK(testVariable == testVariable2);
+		testVariable = 10;
+		CHECK(testVariable != 5);
+		//CHECK(not testVariable != testVariable2);
+		uint8_t variableRaw = 0;
+		uint8_t* varPtr = &variableRaw;
+		size_t maxSize = testVariable.getSerializedSize();
+		CHECK(maxSize == 1);
+		size_t serSize = 0;
+		CHECK(testVariable.serialize(&varPtr, &serSize, maxSize,
+				SerializeIF::Endianness::MACHINE) == retval::CATCH_OK);
+		CHECK(variableRaw == 10);
+		const uint8_t* varConstPtr = &variableRaw;
+		testVariable = 5;
+		CHECK(testVariable.deSerialize(&varConstPtr, &serSize,
+				SerializeIF::Endianness::MACHINE) == retval::CATCH_OK);
+		CHECK(testVariable == 10);
+		CHECK(testVariable != testVariable2);
+		CHECK(testVariable2 < testVariable);
+		CHECK(testVariable2 < 10);
+		CHECK(testVariable > 5);
+		CHECK(testVariable > testVariable2);
+		variableRaw = static_cast<uint8_t>(testVariable2);
+		CHECK(variableRaw == 5);
+
+		CHECK(testVariable == 10);
+		testVariable = testVariable2;
+		CHECK(testVariable == 5);
+	}
+
+	SECTION("ErrorHandling") {
+
+		// not try to use a local pool variable which does not exist
+		lp_var_t<uint8_t> invalidVariable = lp_var_t<uint8_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, 0xffffffff);
+		REQUIRE(invalidVariable.read() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND));
+
+		REQUIRE(invalidVariable.commit() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND));
+		// now try to access with wrong type
+		lp_var_t<int8_t> invalidVariable2 = lp_var_t<int8_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint8VarId);
+		REQUIRE(invalidVariable2.read() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT));
+
+		lp_var_t<uint8_t> readOnlyVar = lp_var_t<uint8_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint8VarId,
+				nullptr, pool_rwm_t::VAR_READ);
+		REQUIRE(readOnlyVar.commit() ==
+				static_cast<int>(PoolVariableIF::INVALID_READ_WRITE_MODE));
+		lp_var_t<uint8_t> writeOnlyVar = lp_var_t<uint8_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint8VarId,
+				nullptr, pool_rwm_t::VAR_WRITE);
+		REQUIRE(writeOnlyVar.read() == static_cast<int>(
+				PoolVariableIF::INVALID_READ_WRITE_MODE));
+
+		lp_var_t<uint32_t> uint32tVar = lp_var_t<uint32_t>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint32VarId);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::info << "LocalPoolVariable printout: " <<uint32tVar << std::endl;
+#endif
+
+		// for code coverage. If program does not crash -> OK
+		lp_var_t<uint8_t> invalidObjectVar = lp_var_t<uint8_t>(
+				0xffffffff, lpool::uint8VarId);
+		gp_id_t globPoolId(0xffffffff,
+				lpool::uint8VarId);
+		lp_var_t<uint8_t> invalidObjectVar2 = lp_var_t<uint8_t>(globPoolId);
+		lp_var_t<uint8_t> invalidObjectVar3 = lp_var_t<uint8_t>(nullptr,
+				lpool::uint8VarId);
+	}
+
+}
+
+
diff --git a/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp b/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
new file mode 100644
index 00000000..cfc2ec2e
--- /dev/null
+++ b/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
@@ -0,0 +1,120 @@
+#include "LocalPoolOwnerBase.h"
+
+#include <catch2/catch_test_macros.hpp>
+#include <fsfw/datapoollocal/HasLocalDataPoolIF.h>
+#include <unittest/core/CatchDefinitions.h>
+
+TEST_CASE("LocalPoolVector" , "[LocPoolVecTest]") {
+	LocalPoolOwnerBase* poolOwner = objectManager->
+			get<LocalPoolOwnerBase>(objects::TEST_LOCAL_POOL_OWNER_BASE);
+	REQUIRE(poolOwner != nullptr);
+	REQUIRE(poolOwner->initializeHkManager() == retval::CATCH_OK);
+	REQUIRE(poolOwner->initializeHkManagerAfterTaskCreation()
+			== retval::CATCH_OK);
+
+	SECTION("BasicTest") {
+		// very basic test.
+		lp_vec_t<uint16_t, 3> testVector = lp_vec_t<uint16_t, 3>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint16Vec3Id);
+		REQUIRE(testVector.read() == retval::CATCH_OK);
+		testVector.value[0] = 5;
+		testVector.value[1] = 232;
+		testVector.value[2] = 32023;
+
+		REQUIRE(testVector.commit(true) == retval::CATCH_OK);
+		CHECK(testVector.isValid());
+
+		testVector.value[0] = 0;
+		testVector.value[1] = 0;
+		testVector.value[2] = 0;
+
+		CHECK(testVector.read() == retval::CATCH_OK);
+		CHECK(testVector.value[0] == 5);
+		CHECK(testVector.value[1] == 232);
+		CHECK(testVector.value[2] == 32023);
+
+		CHECK(testVector[0] == 5);
+
+		// This is invalid access, so the last value will be set instead.
+		// (we can't throw exceptions)
+		testVector[4] = 12;
+		CHECK(testVector[2] == 12);
+		CHECK(testVector.commit() == retval::CATCH_OK);
+
+		// Use read-only reference.
+		const lp_vec_t<uint16_t, 3>& roTestVec = testVector;
+		uint16_t valueOne = roTestVec[0];
+		CHECK(valueOne == 5);
+
+		uint16_t lastVal = roTestVec[25];
+		CHECK(lastVal == 12);
+
+		size_t maxSize = testVector.getSerializedSize();
+		CHECK(maxSize == 6);
+
+		uint16_t serializedVector[3];
+		uint8_t* vecPtr = reinterpret_cast<uint8_t*>(serializedVector);
+		size_t serSize = 0;
+		REQUIRE(testVector.serialize(&vecPtr, &serSize,
+				maxSize, SerializeIF::Endianness::MACHINE) == retval::CATCH_OK);
+
+		CHECK(serSize == 6);
+		CHECK(serializedVector[0] == 5);
+		CHECK(serializedVector[1] == 232);
+		CHECK(serializedVector[2] == 12);
+
+		maxSize = 1;
+		REQUIRE(testVector.serialize(&vecPtr, &serSize,
+				maxSize, SerializeIF::Endianness::MACHINE) ==
+				static_cast<int>(SerializeIF::BUFFER_TOO_SHORT));
+
+		serializedVector[0] = 16;
+		serializedVector[1] = 7832;
+		serializedVector[2] = 39232;
+
+		const uint8_t* constVecPtr = reinterpret_cast<const uint8_t*>(
+				serializedVector);
+		REQUIRE(testVector.deSerialize(&constVecPtr, &serSize,
+				SerializeIF::Endianness::MACHINE) == retval::CATCH_OK);
+		CHECK(testVector[0] == 16);
+		CHECK(testVector[1] == 7832);
+		CHECK(testVector[2] == 39232);
+
+		serSize = 1;
+		REQUIRE(testVector.deSerialize(&constVecPtr, &serSize,
+				SerializeIF::Endianness::MACHINE) ==
+				static_cast<int>(SerializeIF::STREAM_TOO_SHORT));
+	}
+
+	SECTION("ErrorHandling") {
+		// not try to use a local pool variable which does not exist
+		lp_vec_t<uint16_t, 3> invalidVector = lp_vec_t<uint16_t, 3>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, 0xffffffff);
+		REQUIRE(invalidVector.read() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND));
+		REQUIRE(invalidVector.commit() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND));
+
+		// now try to access with wrong type
+		lp_vec_t<uint32_t, 3> invalidVector2 = lp_vec_t<uint32_t, 3>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint16Vec3Id);
+		REQUIRE(invalidVector2.read() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT));
+		REQUIRE(invalidVector2.commit() ==
+				static_cast<int>(HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT));
+
+		lp_vec_t<uint16_t, 3> writeOnlyVec = lp_vec_t<uint16_t, 3>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint16Vec3Id,
+				nullptr, pool_rwm_t::VAR_WRITE);
+		REQUIRE(writeOnlyVec.read() ==
+				static_cast<int>(PoolVariableIF::INVALID_READ_WRITE_MODE));
+
+		lp_vec_t<uint16_t, 3> readOnlyVec = lp_vec_t<uint16_t, 3>(
+				objects::TEST_LOCAL_POOL_OWNER_BASE, lpool::uint16Vec3Id,
+				nullptr, pool_rwm_t::VAR_READ);
+		REQUIRE(readOnlyVec.commit() ==
+				static_cast<int>(PoolVariableIF::INVALID_READ_WRITE_MODE));
+	}
+}
+
+
diff --git a/unittest/tests/storagemanager/TestNewAccessor.cpp b/unittest/tests/storagemanager/TestNewAccessor.cpp
index 7bd0dee2..10d05c6b 100644
--- a/unittest/tests/storagemanager/TestNewAccessor.cpp
+++ b/unittest/tests/storagemanager/TestNewAccessor.cpp
@@ -1,161 +1,161 @@
-//#include <fsfw/storagemanager/LocalPool.h>
-//#include <catch2/catch.hpp>
-//#include "../../core/CatchDefinitions.h"
-//#include <array>
-//
-//TEST_CASE( "New Accessor" , "[NewAccessor]") {
-//	uint16_t numberOfElements[1] = {1};
-//	uint16_t sizeofElements[1] = {10};
-//	LocalPool<1> SimplePool = LocalPool<1>(0, sizeofElements, numberOfElements);
-//	std::array<uint8_t, 20> testDataArray;
-//	std::array<uint8_t, 20> receptionArray;
-//	store_address_t testStoreId;
-//	ReturnValue_t result = retval::CATCH_FAILED;
-//
-//	for(size_t i = 0; i < testDataArray.size(); i++) {
-//		testDataArray[i] = i;
-//	}
-//	size_t size = 10;
-//
-//	SECTION ("Simple tests getter functions") {
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		REQUIRE(result == retval::CATCH_OK);
-//		auto resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == retval::CATCH_OK);
-//		resultPair.second.getDataCopy(receptionArray.data(), 20);
-//		CHECK(resultPair.second.getId() == testStoreId);
-//		CHECK(resultPair.second.size() == 10);
-//		for(size_t i = 0; i < size; i++) {
-//			CHECK(receptionArray[i] == i );
-//		}
-//
-//		std::copy(resultPair.second.data(), resultPair.second.data() +
-//				resultPair.second.size(), receptionArray.data());
-//		for(size_t i = 0; i < size; i++) {
-//			CHECK(receptionArray[i] == i );
-//		}
-//
-//		{
-//			auto resultPairLoc = SimplePool.getData(testStoreId);
-//			REQUIRE(resultPairLoc.first == retval::CATCH_OK);
-//			// data should be deleted when accessor goes out of scope.
-//		}
-//		resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
-//
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		REQUIRE(result == retval::CATCH_OK);
-//		{
-//			ConstStorageAccessor constAccessor(testStoreId);
-//			result = SimplePool.getData(testStoreId, constAccessor);
-//			REQUIRE(result == retval::CATCH_OK);
-//			constAccessor.getDataCopy(receptionArray.data(), 20);
-//			for(size_t i = 0; i < size; i++) {
-//				CHECK(receptionArray[i] == i );
-//			}
-//			// likewise, data should be deleted when accessor gets out of scope.
-//		}
-//		resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
-//
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		{
-//			resultPair = SimplePool.getData(testStoreId);
-//			REQUIRE(resultPair.first == retval::CATCH_OK);
-//			resultPair.second.release();
-//			// now data should not be deleted anymore
-//		}
-//		resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == retval::CATCH_OK);
-//		resultPair.second.getDataCopy(receptionArray.data(), 20);
-//		for(size_t i = 0; i < size; i++) {
-//			CHECK(receptionArray[i] == i );
-//		}
-//	}
-//
-//
-//	SECTION("Simple tests modify functions") {
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		REQUIRE(result == retval::CATCH_OK);
-//		{
-//			StorageAccessor accessor(testStoreId);
-//			result = SimplePool.modifyData(testStoreId, accessor);
-//			REQUIRE(result == retval::CATCH_OK);
-//			CHECK(accessor.getId() == testStoreId);
-//			CHECK(accessor.size() == 10);
-//			accessor.getDataCopy(receptionArray.data(), 20);
-//			for(size_t i = 0; i < size; i++) {
-//				CHECK(receptionArray[i] == i );
-//			}
-//			std::copy(accessor.data(), accessor.data() +
-//					accessor.size(), receptionArray.data());
-//			for(size_t i = 0; i < size; i++) {
-//				CHECK(receptionArray[i] == i );
-//			}
-//			// data should be deleted when accessor goes out of scope
-//		}
-//		auto resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
-//
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		REQUIRE(result == retval::CATCH_OK);
-//		{
-//			auto resultPairLoc = SimplePool.modifyData(testStoreId);
-//			REQUIRE(resultPairLoc.first == retval::CATCH_OK);
-//			CHECK(resultPairLoc.second.getId() == testStoreId);
-//			CHECK(resultPairLoc.second.size() == 10);
-//			resultPairLoc.second.getDataCopy(receptionArray.data(), 20);
-//			for(size_t i = 0; i < size; i++) {
-//				CHECK(receptionArray[i] == i );
-//			}
-//			std::copy(resultPairLoc.second.data(), resultPairLoc.second.data() +
-//					resultPairLoc.second.size(), receptionArray.data());
-//			for(size_t i = 0; i < size; i++) {
-//				CHECK(receptionArray[i] == i );
-//			}
-//			resultPairLoc.second.release();
-//			// data should not be deleted when accessor goes out of scope
-//		}
-//		resultPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultPair.first == retval::CATCH_OK);
-//	}
-//
-//
-//	SECTION("Write tests") {
-//		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
-//		REQUIRE(result == retval::CATCH_OK);
-//		{
-//			auto resultPair = SimplePool.modifyData(testStoreId);
-//			REQUIRE(resultPair.first == retval::CATCH_OK);
-//			testDataArray[9] = 42;
-//			resultPair.second.write(testDataArray.data(), 10, 0);
-//			// now data should not be deleted
-//			resultPair.second.release();
-//		}
-//		auto resultConstPair = SimplePool.getData(testStoreId);
-//		REQUIRE(resultConstPair.first == retval::CATCH_OK);
-//
-//		resultConstPair.second.getDataCopy(receptionArray.data(), 10);
-//		for(size_t i = 0; i < size-1; i++) {
-//			CHECK(receptionArray[i] == i );
-//		}
-//		CHECK(receptionArray[9] == 42 );
-//
-//		auto resultPair = SimplePool.modifyData(testStoreId);
-//		REQUIRE(resultPair.first == retval::CATCH_OK);
-//		result = resultPair.second.write(testDataArray.data(), 20, 0);
-//		REQUIRE(result == retval::CATCH_FAILED);
-//		result = resultPair.second.write(testDataArray.data(), 10, 5);
-//		REQUIRE(result == retval::CATCH_FAILED);
-//
-//		memset(testDataArray.data(), 42, 5);
-//		result = resultPair.second.write(testDataArray.data(), 5, 5);
-//		REQUIRE(result == retval::CATCH_OK);
-//		resultConstPair = SimplePool.getData(testStoreId);
-//		resultPair.second.getDataCopy(receptionArray.data(), 20);
-//		for(size_t i = 5; i < 10; i++) {
-//			CHECK(receptionArray[i] == 42 );
-//		}
-//
-//	}
-//}
+#include <fsfw/storagemanager/LocalPool.h>
+#include <catch2/catch_test_macros.hpp>
+#include <unittest/core/CatchDefinitions.h>
+#include <array>
+#include <cstring>
+
+TEST_CASE( "New Accessor" , "[NewAccessor]") {
+	LocalPool::LocalPoolConfig poolCfg = {{1, 10}};
+	LocalPool SimplePool = LocalPool(0, poolCfg);
+	std::array<uint8_t, 20> testDataArray;
+	std::array<uint8_t, 20> receptionArray;
+	store_address_t testStoreId;
+	ReturnValue_t result = retval::CATCH_FAILED;
+
+	for(size_t i = 0; i < testDataArray.size(); i++) {
+		testDataArray[i] = i;
+	}
+	size_t size = 10;
+
+	SECTION ("Simple tests getter functions") {
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		REQUIRE(result == retval::CATCH_OK);
+		auto resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == retval::CATCH_OK);
+		resultPair.second.getDataCopy(receptionArray.data(), 20);
+		CHECK(resultPair.second.getId() == testStoreId);
+		CHECK(resultPair.second.size() == 10);
+		for(size_t i = 0; i < size; i++) {
+			CHECK(receptionArray[i] == i );
+		}
+
+		std::copy(resultPair.second.data(), resultPair.second.data() +
+				resultPair.second.size(), receptionArray.data());
+		for(size_t i = 0; i < size; i++) {
+			CHECK(receptionArray[i] == i );
+		}
+
+		{
+			auto resultPairLoc = SimplePool.getData(testStoreId);
+			REQUIRE(resultPairLoc.first == retval::CATCH_OK);
+			// data should be deleted when accessor goes out of scope.
+		}
+		resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
+
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		REQUIRE(result == retval::CATCH_OK);
+		{
+			ConstStorageAccessor constAccessor(testStoreId);
+			result = SimplePool.getData(testStoreId, constAccessor);
+			REQUIRE(result == retval::CATCH_OK);
+			constAccessor.getDataCopy(receptionArray.data(), 20);
+			for(size_t i = 0; i < size; i++) {
+				CHECK(receptionArray[i] == i );
+			}
+			// likewise, data should be deleted when accessor gets out of scope.
+		}
+		resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
+
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		{
+			resultPair = SimplePool.getData(testStoreId);
+			REQUIRE(resultPair.first == retval::CATCH_OK);
+			resultPair.second.release();
+			// now data should not be deleted anymore
+		}
+		resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == retval::CATCH_OK);
+		resultPair.second.getDataCopy(receptionArray.data(), 20);
+		for(size_t i = 0; i < size; i++) {
+			CHECK(receptionArray[i] == i );
+		}
+	}
+
+
+	SECTION("Simple tests modify functions") {
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		REQUIRE(result == retval::CATCH_OK);
+		{
+			StorageAccessor accessor(testStoreId);
+			result = SimplePool.modifyData(testStoreId, accessor);
+			REQUIRE(result == retval::CATCH_OK);
+			CHECK(accessor.getId() == testStoreId);
+			CHECK(accessor.size() == 10);
+			accessor.getDataCopy(receptionArray.data(), 20);
+			for(size_t i = 0; i < size; i++) {
+				CHECK(receptionArray[i] == i );
+			}
+			std::copy(accessor.data(), accessor.data() +
+					accessor.size(), receptionArray.data());
+			for(size_t i = 0; i < size; i++) {
+				CHECK(receptionArray[i] == i );
+			}
+			// data should be deleted when accessor goes out of scope
+		}
+		auto resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == (int) StorageManagerIF::DATA_DOES_NOT_EXIST);
+
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		REQUIRE(result == retval::CATCH_OK);
+		{
+			auto resultPairLoc = SimplePool.modifyData(testStoreId);
+			REQUIRE(resultPairLoc.first == retval::CATCH_OK);
+			CHECK(resultPairLoc.second.getId() == testStoreId);
+			CHECK(resultPairLoc.second.size() == 10);
+			resultPairLoc.second.getDataCopy(receptionArray.data(), 20);
+			for(size_t i = 0; i < size; i++) {
+				CHECK(receptionArray[i] == i );
+			}
+			std::copy(resultPairLoc.second.data(), resultPairLoc.second.data() +
+					resultPairLoc.second.size(), receptionArray.data());
+			for(size_t i = 0; i < size; i++) {
+				CHECK(receptionArray[i] == i );
+			}
+			resultPairLoc.second.release();
+			// data should not be deleted when accessor goes out of scope
+		}
+		resultPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultPair.first == retval::CATCH_OK);
+	}
+
+
+	SECTION("Write tests") {
+		result = SimplePool.addData(&testStoreId, testDataArray.data(), size);
+		REQUIRE(result == retval::CATCH_OK);
+		{
+			auto resultPair = SimplePool.modifyData(testStoreId);
+			REQUIRE(resultPair.first == retval::CATCH_OK);
+			testDataArray[9] = 42;
+			resultPair.second.write(testDataArray.data(), 10, 0);
+			// now data should not be deleted
+			resultPair.second.release();
+		}
+		auto resultConstPair = SimplePool.getData(testStoreId);
+		REQUIRE(resultConstPair.first == retval::CATCH_OK);
+
+		resultConstPair.second.getDataCopy(receptionArray.data(), 10);
+		for(size_t i = 0; i < size-1; i++) {
+			CHECK(receptionArray[i] == i );
+		}
+		CHECK(receptionArray[9] == 42 );
+
+		auto resultPair = SimplePool.modifyData(testStoreId);
+		REQUIRE(resultPair.first == retval::CATCH_OK);
+		result = resultPair.second.write(testDataArray.data(), 20, 0);
+		REQUIRE(result == retval::CATCH_FAILED);
+		result = resultPair.second.write(testDataArray.data(), 10, 5);
+		REQUIRE(result == retval::CATCH_FAILED);
+
+		std::memset(testDataArray.data(), 42, 5);
+		result = resultPair.second.write(testDataArray.data(), 5, 5);
+		REQUIRE(result == retval::CATCH_OK);
+		resultConstPair = SimplePool.getData(testStoreId);
+		resultPair.second.getDataCopy(receptionArray.data(), 20);
+		for(size_t i = 5; i < 10; i++) {
+			CHECK(receptionArray[i] == 42 );
+		}
+
+	}
+}
diff --git a/unittest/tests/storagemanager/TestPool.cpp b/unittest/tests/storagemanager/TestPool.cpp
index dcfb6c03..50bde01a 100644
--- a/unittest/tests/storagemanager/TestPool.cpp
+++ b/unittest/tests/storagemanager/TestPool.cpp
@@ -8,8 +8,6 @@
 
 
 TEST_CASE( "Local Pool Simple Tests [1 Pool]" , "[TestPool]") {
-//	uint16_t numberOfElements[1] = {1};
-//	uint16_t sizeofElements[1] = {10};
 	LocalPool::LocalPoolConfig config = {{1, 10}};
 	LocalPool simplePool(0, config);
 	std::array<uint8_t, 20> testDataArray;
diff --git a/unittest/user/CMakeLists.txt b/unittest/user/CMakeLists.txt
index 6ec314c3..722cc424 100644
--- a/unittest/user/CMakeLists.txt
+++ b/unittest/user/CMakeLists.txt
@@ -13,7 +13,19 @@ cmake_minimum_required(VERSION 3.13)
 
 # set(CMAKE_VERBOSE TRUE)
 
-set(CMAKE_SCRIPT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/buildsystem/cmake")
+set(CMAKE_SCRIPT_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
+
+option(TMTC_TEST "Build binary for manual or automatic TMTC tests" FALSE)
+option(GENERATE_COVERAGE 
+	"Specify whether coverage data is generated with GCOV" 
+	TRUE
+)
+
+if(TMTC_TEST)
+	set(LINK_CATCH2 FALSE)
+else()
+	set(LINK_CATCH2 TRUE)
+endif()
 
 # Tests can be built with the Host OSAL or with the Linux OSAL.
 if(NOT OS_FSFW)
@@ -53,6 +65,7 @@ set(FSFW_PATH fsfw)
 set(CATCH2_PATH Catch2)
 set(FSFW_TESTS_PATH fsfw/unittest)
 set(TEST_SETUP_PATH unittest)
+set(TMTC_TEST_PATH tests)
 
 # Analyse different OS and architecture/target options and
 # determine BSP_PATH
@@ -71,6 +84,15 @@ else()
 	endif()
 endif()
 
+if(GENERATE_COVERAGE)
+	list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake/cmake-modules)
+	if(CMAKE_COMPILER_IS_GNUCXX)
+    	include(CodeCoverage)
+    	# Add compile options on target base, we don't want coverage for Catch2
+    	# append_coverage_compiler_flags()
+   	endif()
+endif()
+
 set(FSFW_CONFIG_PATH testcfg)
 
 ################################################################################
@@ -82,10 +104,16 @@ add_executable(${TARGET_NAME})
 
 # Add subdirectories
 add_subdirectory(${FSFW_PATH})
-add_subdirectory(${CATCH2_PATH})
 add_subdirectory(${FSFW_CONFIG_PATH})
-add_subdirectory(${FSFW_TESTS_PATH})
-add_subdirectory(${TEST_SETUP_PATH})
+
+if(LINK_CATCH2)
+	add_subdirectory(${CATCH2_PATH})
+	add_subdirectory(${FSFW_TESTS_PATH})
+	add_subdirectory(${TEST_SETUP_PATH})
+else()
+	add_subdirectory(${TMTC_TEST_PATH})
+endif()
+
 
 ################################################################################
 # Post-Sources preparation
@@ -94,9 +122,74 @@ add_subdirectory(${TEST_SETUP_PATH})
 # Add libraries for all sources.
 target_link_libraries(${TARGET_NAME} PRIVATE
 	${LIB_FSFW_NAME} 
-	${CATCH2_TARGET}
 )
 
+if(LINK_CATCH2)
+	target_link_libraries(${TARGET_NAME} PRIVATE
+		${CATCH2_TARGET}
+	)
+endif()
+
+if(GENERATE_COVERAGE)
+	if(CMAKE_COMPILER_IS_GNUCXX)
+		set(CODE_COVERAGE_VERBOSE TRUE)
+    	include(CodeCoverage)
+    	
+    	# Remove quotes.
+    	separate_arguments(COVERAGE_COMPILER_FLAGS 
+    		NATIVE_COMMAND "${COVERAGE_COMPILER_FLAGS}"
+    	)
+    	
+    	# Add compile options manually, we don't want coverage for Catch2
+    	target_compile_options(${TARGET_NAME} PRIVATE
+    		"${COVERAGE_COMPILER_FLAGS}"
+    	)
+    	target_compile_options(${LIB_FSFW_NAME} PRIVATE
+    		"${COVERAGE_COMPILER_FLAGS}"
+    	)
+    	
+		# Exclude internal unittest from coverage for now.
+    	if(WIN32)
+			set(GCOVR_ADDITIONAL_ARGS 
+				"--exclude-throw-branches"
+				"--exclude-unreachable-branches"
+			)
+			set(COVERAGE_EXCLUDES
+				"/c/msys64/mingw64/*" "Catch2"
+				"${CMAKE_CURRENT_SOURCE_DIR}/fsfw/unittest/internal"
+			)
+		elseif(UNIX)
+			set(COVERAGE_EXCLUDES
+				"/usr/include/*" "/usr/bin/*" "Catch2/*"
+				"fsfw/unittest/internal/*"
+			)
+		endif()
+    	
+		target_link_options(${TARGET_NAME} PRIVATE
+			-fprofile-arcs
+			-ftest-coverage
+    	)
+    	target_link_options(${LIB_FSFW_NAME} PRIVATE
+			-fprofile-arcs
+			-ftest-coverage
+    	)
+    	
+		if(WIN32)
+			setup_target_for_coverage_gcovr_html(
+				NAME ${TARGET_NAME}_coverage
+				EXECUTABLE ${TARGET_NAME}
+				DEPENDENCIES ${TARGET_NAME}
+			)
+    	else()
+			setup_target_for_coverage_lcov(
+				NAME ${TARGET_NAME}_coverage
+				EXECUTABLE ${TARGET_NAME}
+				DEPENDENCIES ${TARGET_NAME}
+			)
+		endif()
+	endif()
+endif()
+
 # Add include paths for all sources.
 target_include_directories(${TARGET_NAME} PRIVATE
 	${CMAKE_CURRENT_SOURCE_DIR}
@@ -147,8 +240,7 @@ if(NOT CMAKE_SIZE)
 	endif()
 endif()
 
-add_custom_command(
-   TARGET ${TARGET_NAME}
+add_custom_command(TARGET ${TARGET_NAME}
    POST_BUILD
    COMMAND echo "Build directory: ${CMAKE_BINARY_DIR}"
    COMMAND echo "Target OSAL: ${OS_FSFW}"
@@ -156,7 +248,7 @@ add_custom_command(
    COMMAND ${CMAKE_SIZE} ${TARGET_NAME}${FILE_SUFFIX}
 )
 
-include (${CMAKE_CURRENT_SOURCE_DIR}/buildsystem/cmake/BuildType.cmake)
+include (${CMAKE_SCRIPT_PATH}/BuildType.cmake)
 set_build_type()
 
 
diff --git a/unittest/user/testcfg/FSFWConfig.h b/unittest/user/testcfg/FSFWConfig.h
index 6b0def90..261e3d6d 100644
--- a/unittest/user/testcfg/FSFWConfig.h
+++ b/unittest/user/testcfg/FSFWConfig.h
@@ -4,17 +4,26 @@
 #include <cstddef>
 #include <cstdint>
 
-//! Used to determine whether C++ ostreams are used
-//! Those can lead to code bloat.
+//! Used to determine whether C++ ostreams are used which can increase
+//! the binary size significantly. If this is disabled,
+//! the C stdio functions can be used alternatively
 #define FSFW_CPP_OSTREAM_ENABLED 	1
 
-//! Reduced printout to further decrease code size
+//! More FSFW related printouts.
 //! Be careful, this also turns off most diagnostic prinouts!
 #define FSFW_ENHANCED_PRINTOUT		0
 
+//! Can be used to completely disable printouts, even the C stdio ones.
+#if FSFW_CPP_OSTREAM_ENABLED == 0 && FSFW_ENHANCED_PRINTOUT == 0
+	#define FSFW_DISABLE_PRINTOUT 	0
+#endif
+
 //! Can be used to enable additional debugging printouts for developing the FSFW
 #define FSFW_PRINT_VERBOSITY_LEVEL   0
 
+//! Can be used to disable the ANSI color sequences for C stdio.
+#define FSFW_COLORED_OUTPUT 		1
+
 //! If FSFW_OBJ_EVENT_TRANSLATION is set to one,
 //! additional output which requires the translation files translateObjects
 //! and translateEvents (and their compiled source files)
@@ -49,7 +58,9 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 //! also determines how many commands a CSB service can handle in one cycle
 //! simulataneously. This will increase the required RAM for
 //! each CSB service !
-static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 3;
+static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
+
+static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/unittest/user/testcfg/objects/systemObjectList.h b/unittest/user/testcfg/objects/systemObjectList.h
index 0e034aff..da95617c 100644
--- a/unittest/user/testcfg/objects/systemObjectList.h
+++ b/unittest/user/testcfg/objects/systemObjectList.h
@@ -9,7 +9,19 @@ namespace objects {
 	enum sourceObjects: uint32_t {
 		/* All addresses between start and end are reserved for the FSFW */
 		FSFW_CONFIG_RESERVED_START = PUS_SERVICE_1_VERIFICATION,
-		FSFW_CONFIG_RESERVED_END = TM_STORE
+		FSFW_CONFIG_RESERVED_END = TM_STORE,
+
+		CCSDS_DISTRIBUTOR = 10,
+		PUS_DISTRIBUTOR = 11,
+		TM_FUNNEL = 12,
+
+		UDP_BRIDGE = 15,
+		UDP_POLLING_TASK = 16,
+
+		TEST_ECHO_COM_IF = 20,
+		TEST_DEVICE = 21,
+
+		TEST_LOCAL_POOL_OWNER_BASE = 25
 	};
 }
 
diff --git a/unittest/user/unittest/core/CMakeLists.txt b/unittest/user/unittest/core/CMakeLists.txt
index d78f45f1..0989926c 100644
--- a/unittest/user/unittest/core/CMakeLists.txt
+++ b/unittest/user/unittest/core/CMakeLists.txt
@@ -5,3 +5,9 @@ target_sources(${TARGET_NAME} PRIVATE
 	CatchSetup.cpp
 	printChar.cpp
 )
+
+if(CUSTOM_UNITTEST_RUNNER)
+	target_sources(${TARGET_NAME} PRIVATE
+		CatchRunner.cpp
+	)
+endif()
\ No newline at end of file
diff --git a/unittest/user/unittest/core/CatchDefinitions.cpp b/unittest/user/unittest/core/CatchDefinitions.cpp
index 00daf015..02a935d2 100644
--- a/unittest/user/unittest/core/CatchDefinitions.cpp
+++ b/unittest/user/unittest/core/CatchDefinitions.cpp
@@ -1,4 +1,5 @@
 #include "CatchDefinitions.h"
+#include <fsfw/serviceinterface/ServiceInterface.h>
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 
 StorageManagerIF* tglob::getIpcStoreHandle() {
@@ -7,6 +8,8 @@ StorageManagerIF* tglob::getIpcStoreHandle() {
 	} else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Global object manager uninitialized" << std::endl;
+#else
+		fsfw::printError("Global object manager uninitialized");
 #endif
 		return nullptr;
 	}
diff --git a/unittest/user/unittest/core/CatchFactory.cpp b/unittest/user/unittest/core/CatchFactory.cpp
index 3177212f..0597f08c 100644
--- a/unittest/user/unittest/core/CatchFactory.cpp
+++ b/unittest/user/unittest/core/CatchFactory.cpp
@@ -1,11 +1,15 @@
+#include <fsfw/datapoollocal/LocalDataPoolManager.h>
+#include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include "CatchFactory.h"
 
 #include <fsfw/events/EventManager.h>
 #include <fsfw/health/HealthTable.h>
-
 #include <fsfw/internalError/InternalErrorReporter.h>
 #include <fsfw/objectmanager/frameworkObjects.h>
 #include <fsfw/storagemanager/PoolManager.h>
+#include <fsfw/tmtcpacket/pus/TmPacketStored.h>
+#include <fsfw/tmtcservices/CommandingServiceBase.h>
+#include <fsfw/tmtcservices/PusServiceBase.h>
 
 /**
  * @brief Produces system objects.
@@ -50,7 +54,23 @@ void Factory::produce(void) {
 }
 
 void Factory::setStaticFrameworkObjectIds() {
+	PusServiceBase::packetSource = objects::NO_OBJECT;
+	PusServiceBase::packetDestination = objects::NO_OBJECT;
 
+	CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
+	CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
+
+	VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION;
+
+	DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT;
+	DeviceHandlerBase::rawDataReceiverId = objects::PUS_SERVICE_2_DEVICE_ACCESS;
+
+	LocalDataPoolManager::defaultHkDestination = objects::NO_OBJECT;
+
+	DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT;
+
+	TmPacketStored::timeStamperId = objects::NO_OBJECT;
 }
 
 
+
diff --git a/unittest/user/unittest/core/CatchRunner.cpp b/unittest/user/unittest/core/CatchRunner.cpp
index 35c53cd4..886d641f 100644
--- a/unittest/user/unittest/core/CatchRunner.cpp
+++ b/unittest/user/unittest/core/CatchRunner.cpp
@@ -1,17 +1,16 @@
 /**
- * @file 	CatchSource.cpp
+ * @file 	CatchRunner.cpp
  * @brief 	Source file to compile catch framework.
  * @details	All tests should be written in other files.
  * For eclipse console output, install ANSI Escape in Console
  * from the eclipse market place to get colored characters.
  */
 
-#ifndef NO_UNIT_TEST_FRAMEWORK
+#include <TestsConfig.h>
 
-#define CATCH_CONFIG_RUNNER
-#include <catch2/catch.hpp>
+#define CATCH_CONFIG_COLOUR_WINDOWS
 
-#if CUSTOM_UNITTEST_RUNNER == 0
+#include <catch2/catch_session.hpp>
 
 extern int customSetup();
 
@@ -25,7 +24,3 @@ int main( int argc, char* argv[] ) {
     return result;
 }
 
-#endif
-
-
-#endif
diff --git a/unittest/user/unittest/core/CatchSetup.cpp b/unittest/user/unittest/core/CatchSetup.cpp
index c93cf032..bda31400 100644
--- a/unittest/user/unittest/core/CatchSetup.cpp
+++ b/unittest/user/unittest/core/CatchSetup.cpp
@@ -1,21 +1,20 @@
-#include <fsfw/unittest/core/CatchFactory.h>
+#include "CatchFactory.h"
 #include "CatchDefinitions.h"
-#include <testcfg/cdatapool/dataPoolInit.h>
 
 #ifdef GCOV
 #include <gcov.h>
 #endif
 
-#include "../../objectmanager/ObjectManager.h"
-#include "../../objectmanager/ObjectManagerIF.h"
-#include "../../storagemanager/StorageManagerIF.h"
-#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include <fsfw/objectmanager/ObjectManager.h>
+#include <fsfw/objectmanager/ObjectManagerIF.h>
+#include <fsfw/storagemanager/StorageManagerIF.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 
 
 /* Global instantiations normally done in main.cpp */
 /* Initialize Data Pool */
 
-
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 namespace sif {
 /* Set up output streams */
 ServiceInterfaceStream debug("DEBUG");
@@ -23,6 +22,7 @@ ServiceInterfaceStream info("INFO");
 ServiceInterfaceStream error("ERROR");
 ServiceInterfaceStream warning("WARNING");
 }
+#endif
 
 /* Global object manager */
 ObjectManagerIF *objectManager;
diff --git a/unittest/user/unittest/core/printChar.cpp b/unittest/user/unittest/core/printChar.cpp
index 755a13b2..5d4f1b17 100644
--- a/unittest/user/unittest/core/printChar.cpp
+++ b/unittest/user/unittest/core/printChar.cpp
@@ -1,4 +1,4 @@
-#include <fsfw/unittest/core/printChar.h>
+#include "printChar.h"
 #include <cstdio>
 
 void printChar(const char* character, bool errStream) {