diff --git a/CHANGELOG b/CHANGELOG
index 2d901791..298ef0d4 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -70,3 +70,15 @@ now
 ### Commanding Service Base
 
 - CSB uses the new fsfwconfig::FSFW_CSB_FIFO_DEPTH variable to determine  the FIFO depth for each CSB instance. This variable has to be set in the FSFWConfig.h file
+
+### Service Interface
+
+- Proper printf support contained in ServiceInterfacePrinter.h
+- CPP ostream support now optional (can reduce executable size by 150 - 250 kB)
+- Amalagated header which determines automatically which service interface to use depending on FSFWConfig.h configuration. 
+  Users can just use #include <fsfw/serviceinterface/ServiceInterface.h>
+- If CPP streams are excluded, sif:: calls won't work anymore and need to be replaced by their printf counterparts. 
+  For the fsfw, this can be done by checking the processor define FSFW_CPP_OSTREAM_ENABLED from FSFWConfig.h. 
+  For mission code, developers need to replace sif:: calls by the printf counterparts, but only if the CPP stream are excluded. 
+  If this is not the case, everything should work as usual.
+- 
\ No newline at end of file
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/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp
index 6ddb3d3e..769ce000 100644
--- a/container/SharedRingBuffer.cpp
+++ b/container/SharedRingBuffer.cpp
@@ -47,9 +47,11 @@ ReturnValue_t SharedRingBuffer::initialize() {
 DynamicFIFO<size_t>* SharedRingBuffer::getReceiveSizesFIFO() {
 	if(receiveSizesFIFO == nullptr) {
 		// Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "SharedRingBuffer::getReceiveSizesFIFO: Ring buffer"
 				<< " was not configured to have sizes FIFO, returning nullptr!"
 				<< std::endl;
+#endif
 	}
 	return receiveSizesFIFO;
 }
diff --git a/controller/ExtendedControllerBase.cpp b/controller/ExtendedControllerBase.cpp
index fa4f6786..017acafe 100644
--- a/controller/ExtendedControllerBase.cpp
+++ b/controller/ExtendedControllerBase.cpp
@@ -107,7 +107,9 @@ MessageQueueId_t ExtendedControllerBase::getCommandQueue() const {
 }
 
 LocalPoolDataSetBase* ExtendedControllerBase::getDataSetHandle(sid_t sid) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "ExtendedControllerBase::getDataSetHandle: No child "
             << " implementation provided, returning nullptr!" << std::endl;
+#endif
     return nullptr;
 }
diff --git a/datalinklayer/Clcw.cpp b/datalinklayer/Clcw.cpp
index dc435a2b..13971929 100644
--- a/datalinklayer/Clcw.cpp
+++ b/datalinklayer/Clcw.cpp
@@ -55,7 +55,9 @@ void Clcw::setBitLock(bool bitLock) {
 }
 
 void Clcw::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Clcw::print: Clcw is: " << std::hex << getAsWhole() << std::dec << std::endl;
+#endif
 }
 
 void Clcw::setWhole(uint32_t rawClcw) {
diff --git a/datalinklayer/DataLinkLayer.cpp b/datalinklayer/DataLinkLayer.cpp
index 75f2edda..1bdaa4f5 100644
--- a/datalinklayer/DataLinkLayer.cpp
+++ b/datalinklayer/DataLinkLayer.cpp
@@ -98,8 +98,10 @@ ReturnValue_t DataLinkLayer::processFrame(uint16_t length) {
 	receivedDataLength = length;
 	ReturnValue_t status = allFramesReception();
 	if (status != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataLinkLayer::processFrame: frame reception failed. "
 		         "Error code: " << std::hex << status << std::dec << std::endl;
+#endif
 //		currentFrame.print();
 		return status;
 	} else {
@@ -124,7 +126,9 @@ ReturnValue_t DataLinkLayer::initialize() {
 	if ( virtualChannels.begin() != virtualChannels.end() ) {
 		clcw->setVirtualChannel( virtualChannels.begin()->second->getChannelId() );
 	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataLinkLayer::initialize: No VC assigned to this DLL instance! " << std::endl;
+#endif
 		return RETURN_FAILED;
 	}
 
diff --git a/datalinklayer/MapPacketExtraction.cpp b/datalinklayer/MapPacketExtraction.cpp
index 845ed7c1..cdc9ae27 100644
--- a/datalinklayer/MapPacketExtraction.cpp
+++ b/datalinklayer/MapPacketExtraction.cpp
@@ -29,9 +29,11 @@ ReturnValue_t MapPacketExtraction::extractPackets(TcTransferFrame* frame) {
 			bufferPosition = &packetBuffer[packetLength];
 			status = RETURN_OK;
 		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error
 					<< "MapPacketExtraction::extractPackets. Packet too large! Size: "
 					<< packetLength << std::endl;
+#endif
 			clearBuffers();
 			status = CONTENT_TOO_LARGE;
 		}
@@ -51,24 +53,30 @@ ReturnValue_t MapPacketExtraction::extractPackets(TcTransferFrame* frame) {
 				}
 				status = RETURN_OK;
 			} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error
 						<< "MapPacketExtraction::extractPackets. Packet too large! Size: "
 						<< packetLength << std::endl;
+#endif
 				clearBuffers();
 				status = CONTENT_TOO_LARGE;
 			}
 		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error
 					<< "MapPacketExtraction::extractPackets. Illegal segment! Last flag: "
 					<< (int) lastSegmentationFlag << std::endl;
+#endif
 			clearBuffers();
 			status = ILLEGAL_SEGMENTATION_FLAG;
 		}
 		break;
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error
 				<< "MapPacketExtraction::extractPackets. Illegal segmentationFlag: "
 				<< (int) segmentationFlag << std::endl;
+#endif
 		clearBuffers();
 		status = DATA_CORRUPTED;
 		break;
@@ -135,10 +143,14 @@ ReturnValue_t MapPacketExtraction::initialize() {
 }
 
 void MapPacketExtraction::printPacketBuffer(void) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "DLL: packet_buffer contains: " << std::endl;
+#endif
 	for (uint32_t i = 0; i < this->packetLength; ++i) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "packet_buffer[" << std::dec << i << "]: 0x" << std::hex
 				<< (uint16_t) this->packetBuffer[i] << std::endl;
+#endif
 	}
 }
 
diff --git a/datalinklayer/TcTransferFrame.cpp b/datalinklayer/TcTransferFrame.cpp
index f1a70bdc..ee094dc3 100644
--- a/datalinklayer/TcTransferFrame.cpp
+++ b/datalinklayer/TcTransferFrame.cpp
@@ -87,16 +87,25 @@ uint8_t* TcTransferFrame::getFullDataField() {
 }
 
 void TcTransferFrame::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Raw Frame: " << std::hex << std::endl;
 	for (uint16_t count = 0; count < this->getFullSize(); count++ ) {
 		sif::debug << (uint16_t)this->getFullFrame()[count] << " ";
 	}
 	sif::debug << std::dec << std::endl;
-//	debug << "Frame Header:" << std::endl;
-//	debug << "Version Number: " << std::hex << (uint16_t)this->current_frame.getVersionNumber() << std::endl;
-//	debug << "Bypass Flag set?| Ctrl Cmd Flag set?: " << (uint16_t)this->current_frame.bypassFlagSet() << " | " << (uint16_t)this->current_frame.controlCommandFlagSet()  << std::endl;
-//	debug << "SCID : " << this->current_frame.getSpacecraftId() << std::endl;
-//	debug << "VCID : " << (uint16_t)this->current_frame.getVirtualChannelId() << std::endl;
-//	debug << "Frame length: " << std::dec << this->current_frame.getFrameLength() << std::endl;
-//	debug << "Sequence Number: " << (uint16_t)this->current_frame.getSequenceNumber() << std::endl;
+
+	sif::debug << "Frame Header:" << std::endl;
+	sif::debug << "Version Number: " << std::hex
+			<< (uint16_t)this->getVersionNumber() << std::endl;
+	sif::debug << "Bypass Flag set?| Ctrl Cmd Flag set?: "
+			<< (uint16_t)this->bypassFlagSet() << " | "
+			<< (uint16_t)this->controlCommandFlagSet()  << std::endl;
+	sif::debug << "SCID : " << this->getSpacecraftId() << std::endl;
+	sif::debug << "VCID : " << (uint16_t)this->getVirtualChannelId()
+			<< std::endl;
+	sif::debug << "Frame length: " << std::dec << this->getFrameLength()
+			<< std::endl;
+	sif::debug << "Sequence Number: " << (uint16_t)this->getSequenceNumber()
+			<< std::endl;
+#endif
 }
diff --git a/datalinklayer/TcTransferFrameLocal.cpp b/datalinklayer/TcTransferFrameLocal.cpp
index e8785c92..de8f568f 100644
--- a/datalinklayer/TcTransferFrameLocal.cpp
+++ b/datalinklayer/TcTransferFrameLocal.cpp
@@ -37,7 +37,9 @@ TcTransferFrameLocal::TcTransferFrameLocal(bool bypass, bool controlCommand, uin
 			this->getFullFrame()[getFullSize()-2] = (crc & 0xFF00) >> 8;
 			this->getFullFrame()[getFullSize()-1] = (crc & 0x00FF);
 		} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::debug << "TcTransferFrameLocal: dataSize too large: " << dataSize << std::endl;
+#endif
 		}
 	} else {
 		//No data in frame
diff --git a/datalinklayer/VirtualChannelReception.cpp b/datalinklayer/VirtualChannelReception.cpp
index cde28de3..3a56fe1e 100644
--- a/datalinklayer/VirtualChannelReception.cpp
+++ b/datalinklayer/VirtualChannelReception.cpp
@@ -102,8 +102,10 @@ uint8_t VirtualChannelReception::getChannelId() const {
 ReturnValue_t VirtualChannelReception::initialize() {
 	ReturnValue_t returnValue = RETURN_FAILED;
 	if ((slidingWindowWidth > 254) || (slidingWindowWidth % 2 != 0)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VirtualChannelReception::initialize: Illegal sliding window width: "
 				<< (int) slidingWindowWidth << std::endl;
+#endif
 		return RETURN_FAILED;
 	}
 	for (mapChannelIterator iterator = mapChannels.begin(); iterator != mapChannels.end();
diff --git a/datapool/PoolDataSetBase.cpp b/datapool/PoolDataSetBase.cpp
index 8f073359..05797012 100644
--- a/datapool/PoolDataSetBase.cpp
+++ b/datapool/PoolDataSetBase.cpp
@@ -13,18 +13,24 @@ PoolDataSetBase::~PoolDataSetBase() {}
 ReturnValue_t PoolDataSetBase::registerVariable(
 		PoolVariableIF *variable) {
 	if (state != States::STATE_SET_UNINITIALISED) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataSet::registerVariable: "
 				"Call made in wrong position." << std::endl;
+#endif
 		return DataSetIF::DATA_SET_UNINITIALISED;
 	}
 	if (variable == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataSet::registerVariable: "
 				"Pool variable is nullptr." << std::endl;
+#endif
 		return DataSetIF::POOL_VAR_NULL;
 	}
 	if (fillCount >= maxFillCount) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataSet::registerVariable: "
 				"DataSet is full." << std::endl;
+#endif
 		return DataSetIF::DATA_SET_FULL;
 	}
 	registeredVariables[fillCount] = variable;
@@ -32,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) {
@@ -47,9 +54,11 @@ ReturnValue_t PoolDataSetBase::read(uint32_t lockTimeout) {
 		unlockDataPool();
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DataSet::read(): "
 				"Call made in wrong position. Don't forget to commit"
 				" member datasets!" << std::endl;
+#endif
 		result = SET_WAS_ALREADY_READ;
 	}
 
@@ -78,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();
@@ -91,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();
@@ -120,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();
@@ -138,8 +153,10 @@ ReturnValue_t PoolDataSetBase::handleUnreadDatasetCommit(uint32_t lockTimeout) {
 		} else if (registeredVariables[count]->getDataPoolId()
 				!= PoolVariableIF::NO_PARAMETER) {
 			if (result != COMMITING_WITHOUT_READING) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "DataSet::commit(): commit-without-read call made "
 						"with non write-only variable." << std::endl;
+#endif
 				result = COMMITING_WITHOUT_READING;
 			}
 		}
@@ -150,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 fb73328c..5f5f56f2 100644
--- a/datapool/PoolEntry.cpp
+++ b/datapool/PoolEntry.cpp
@@ -3,6 +3,7 @@
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../globalfunctions/arrayprinter.h"
 #include <cstring>
+#include <algorithm>
 
 template <typename T>
 PoolEntry<T>::PoolEntry(std::initializer_list<T> initValue, uint8_t setLength,
@@ -12,9 +13,11 @@ PoolEntry<T>::PoolEntry(std::initializer_list<T> initValue, uint8_t setLength,
 		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 {
@@ -67,10 +70,14 @@ bool PoolEntry<T>::getValid() {
 
 template <typename T>
 void PoolEntry<T>::print() {
+#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;
+#endif
 }
 
 template<typename T>
@@ -81,8 +88,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/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/HasLocalDataPoolIF.h b/datapoollocal/HasLocalDataPoolIF.h
index df855110..7707fab8 100644
--- a/datapoollocal/HasLocalDataPoolIF.h
+++ b/datapoollocal/HasLocalDataPoolIF.h
@@ -89,8 +89,10 @@ public:
 	 * @return
 	 */
 	virtual LocalPoolObjectBase* getPoolObjectHandle(lp_id_t localPoolId) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::warning << "HasLocalDataPoolIF::getPoolObjectHandle: Not overriden"
 	            << ". Returning nullptr!" << std::endl;
+#endif
 	    return nullptr;
 	}
 
diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 5f0d15d3..24516aad 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -21,15 +21,19 @@ 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
         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
     }
 
     hkQueue = queueToUse;
@@ -39,17 +43,21 @@ 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
     }
     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
         return HasReturnvaluesIF::RETURN_FAILED;
     }
 
@@ -61,8 +69,10 @@ 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;
         }
     }
@@ -85,8 +95,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
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -339,8 +351,10 @@ 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;
     }
 
@@ -369,8 +383,10 @@ 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;
     }
 
@@ -575,8 +591,10 @@ 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
         return HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND;
     }
     poolIter->second->print();
@@ -596,8 +614,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;
     }
 
@@ -678,10 +698,12 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
             sid, dataSet, true);
     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;
+#endif
     }
 }
 
@@ -726,8 +748,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;
     }
 
@@ -752,8 +776,10 @@ 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
         return result;
     }
 
@@ -762,8 +788,10 @@ 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
     }
 
     // Send structure reporting reply.
diff --git a/datapoollocal/LocalDataPoolManager.h b/datapoollocal/LocalDataPoolManager.h
index cdefa2c2..bcb8f0af 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"
@@ -375,15 +376,25 @@ 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;
+#else
+		fsfw::printWarning("HousekeepingManager::fechPoolEntry: Pool entry "
+		        "not found.");
+#endif
 		return HasLocalDataPoolIF::POOL_ENTRY_NOT_FOUND;
 	}
 
 	*poolEntry = dynamic_cast< PoolEntry<T>* >(poolIter->second);
 	if(*poolEntry == nullptr) {
-		sif::debug << "HousekeepingManager::fetchPoolEntry:"
-				" Pool entry not found." << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+		sif::warning << "HousekeepingManager::fetchPoolEntry:"
+				" Pool entry type conflict." << std::endl;
+#else
+		fsfw::printWarning("HousekeepingManager::fetchPoolEntry:"
+				" Pool entry type conflict.");
+#endif
 		return HasLocalDataPoolIF::POOL_ENTRY_TYPE_CONFLICT;
 	}
 	return HasReturnvaluesIF::RETURN_OK;
diff --git a/datapoollocal/LocalPoolDataSetBase.cpp b/datapoollocal/LocalPoolDataSetBase.cpp
index 9ecad218..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"
@@ -12,8 +14,13 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(HasLocalDataPoolIF *hkOwner,
         PoolDataSetBase(registeredVariablesArray, maxNumberOfVariables) {
     if(hkOwner == nullptr) {
         // Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
                 << "invalid!" << std::endl;
+#else
+        fsfw::printError("LocalPoolDataSetBase::LocalPoolDataSetBase: Owner "
+                "invalid!\n\r");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
         return;
     }
     hkManager = hkOwner->getHkManagerHandle();
@@ -42,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;
 }
@@ -147,8 +167,13 @@ ReturnValue_t LocalPoolDataSetBase::serializeLocalPoolIds(uint8_t** buffer,
         auto result = SerializeAdapter::serialize(&currentPoolId, buffer,
                 size, maxSize, streamEndianness);
         if(result != HasReturnvaluesIF::RETURN_OK) {
-            sif::warning << "LocalDataSet::serializeLocalPoolIds: Serialization"
-                    " error!" << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            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;
         }
     }
@@ -206,8 +231,13 @@ ReturnValue_t LocalPoolDataSetBase::serialize(uint8_t **buffer, size_t *size,
 
 void LocalPoolDataSetBase::bitSetter(uint8_t* byte, uint8_t position) const {
     if(position > 7) {
-        sif::debug << "Pool Raw Access: Bit setting invalid position"
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "LocalPoolDataSetBase::bitSetter: Invalid position!"
                 << std::endl;
+#else
+        fsfw::printWarning("LocalPoolDataSetBase::bitSetter: "
+        		"Invalid position!\n\r");
+#endif
         return;
     }
     uint8_t shiftNumber = position + (7 - 2 * position);
@@ -238,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;
 }
 
@@ -256,8 +291,10 @@ sid_t LocalPoolDataSetBase::getSid() const {
 bool LocalPoolDataSetBase::bitGetter(const 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"
                 << std::endl;
+#endif
         return false;
     }
     uint8_t shiftNumber = position + (7 - 2 * position);
@@ -265,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(
@@ -284,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 b4d0e306..b84569b8 100644
--- a/datapoollocal/LocalPoolObjectBase.cpp
+++ b/datapoollocal/LocalPoolObjectBase.cpp
@@ -5,12 +5,16 @@ LocalPoolObjectBase::LocalPoolObjectBase(lp_id_t poolId,
         pool_rwm_t setReadWriteMode): localPoolId(poolId),
         readWriteMode(setReadWriteMode) {
     if(poolId == PoolVariableIF::NO_PARAMETER) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "LocalPoolVar<T>::LocalPoolVar: 0 passed as pool ID, "
                 << "which is the NO_PARAMETER value!" << std::endl;
+#endif
     }
     if(hkOwner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPoolVar<T>::LocalPoolVar: The supplied pool "
                 << "owner is a invalid!" << std::endl;
+#endif
         return;
     }
     hkManager = hkOwner->getHkManagerHandle();
@@ -23,15 +27,19 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId,
         DataSetIF *dataSet, pool_rwm_t setReadWriteMode): localPoolId(poolId),
         readWriteMode(setReadWriteMode) {
     if(poolId == PoolVariableIF::NO_PARAMETER) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "LocalPoolVar<T>::LocalPoolVar: 0 passed as pool ID, "
                 << "which is the NO_PARAMETER value!" << std::endl;
+#endif
     }
     HasLocalDataPoolIF* hkOwner =
             objectManager->get<HasLocalDataPoolIF>(poolOwner);
     if(hkOwner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPoolVariable: The supplied pool owner did not "
                 << "implement the correct interface"
                 << " HasLocalDataPoolIF!" << std::endl;
+#endif
         return;
     }
     hkManager = hkOwner->getHkManagerHandle();
@@ -71,3 +79,44 @@ 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: " << std::hex << std::setw(8)
+			<< std::setfill('0') << objectId << " LPID: 0x" << lpId
+			<< std::dec << std::endl;
+#else
+	fsfw::printWarning("LocalPoolVariable: %s of local pool variable of "
+			"object 0x%08x and lp ID 0x%08x failed.\n\r",
+			type, 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 7b7e443e..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);
@@ -157,12 +173,12 @@ protected:
 	 */
 	ReturnValue_t commitWithoutLock() override;
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	// std::ostream is the type for object std::cout
 	template <typename U>
 	friend std::ostream& operator<< (std::ostream &out,
 			const LocalPoolVariable<U> &var);
-
-private:
+#endif
 };
 
 #include "LocalPoolVariable.tpp"
diff --git a/datapoollocal/LocalPoolVariable.tpp b/datapoollocal/LocalPoolVariable.tpp
index a685ea8d..d9d45d38 100644
--- a/datapoollocal/LocalPoolVariable.tpp
+++ b/datapoollocal/LocalPoolVariable.tpp
@@ -24,65 +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) {
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for read() call." << std::endl;
+		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) {
-		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;
+	if(result != RETURN_OK) {
+		object_id_t ownerObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVariable", result,
+				false, ownerObjectId, localPoolId);
 		return result;
 	}
+
+	// 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->address);
 	this->valid = poolEntry->valid;
 	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) {
-		sif::debug << "LocalPoolVariable: Invalid read write "
-				 "mode for commit() call." << std::endl;
+		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) {
-		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;
+		object_id_t ownerObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVariable", result,
+				false, ownerObjectId, localPoolId);
 		return result;
 	}
+
 	*(poolEntry->address) = this->value;
 	poolEntry->valid = 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);
 }
@@ -98,12 +120,14 @@ inline ReturnValue_t LocalPoolVariable<T>::deSerialize(const uint8_t** buffer,
 	return SerializeAdapter::deSerialize(&value, buffer, size, streamEndianness);
 }
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 template<typename T>
 inline std::ostream& operator<< (std::ostream &out,
 		const LocalPoolVariable<T> &var) {
     out << var.value;
     return out;
 }
+#endif
 
 template<typename T>
 inline LocalPoolVariable<T>::operator T() const {
@@ -111,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;
 }
@@ -124,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;
 }
 
@@ -135,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);
 }
 
@@ -146,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;
 }
 
@@ -157,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..773a52cc 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"
 
 
 /**
@@ -123,7 +123,9 @@ 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 +135,9 @@ 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;
 
 protected:
 	/**
@@ -157,12 +161,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 46123ccc..88ba084a 100644
--- a/datapoollocal/LocalPoolVector.tpp
+++ b/datapoollocal/LocalPoolVector.tpp
@@ -24,16 +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) {
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for read() call." << std::endl;
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector",
+				PoolVariableIF::INVALID_READ_WRITE_MODE, true, targetObjectId,
+				localPoolId);
 		return PoolVariableIF::INVALID_READ_WRITE_MODE;
 	}
 
@@ -42,10 +44,9 @@ inline ReturnValue_t LocalPoolVector<T, vectorSize>::readWithoutLock() {
 	memset(this->value, 0, vectorSize * sizeof(T));
 
 	if(result != RETURN_OK) {
-		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;
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector", result, true, targetObjectId,
+				localPoolId);
 		return result;
 	}
 	std::memcpy(this->value, poolEntry->address, poolEntry->getByteSize());
@@ -55,26 +56,26 @@ inline ReturnValue_t LocalPoolVector<T, vectorSize>::readWithoutLock() {
 
 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) {
-		sif::debug << "LocalPoolVar: Invalid read write "
-				"mode for commit() call." << std::endl;
+		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) {
-		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;
+		object_id_t targetObjectId = hkManager->getOwner()->getObjectId();
+		reportReadCommitError("LocalPoolVector", result, false, targetObjectId,
+				localPoolId);
 		return result;
 	}
 	std::memcpy(poolEntry->address, this->value, poolEntry->getByteSize());
@@ -89,8 +90,10 @@ inline T& LocalPoolVector<T, vectorSize>::operator [](int 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"
 			" last value!" << std::endl;
+#endif
 	return value[i];
 }
 
@@ -101,8 +104,10 @@ inline const T& LocalPoolVector<T, vectorSize>::operator [](int i) const {
 	}
 	// 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"
 			" last value!" << std::endl;
+#endif
 	return value[i];
 }
 
@@ -141,6 +146,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) {
@@ -154,5 +160,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 4fe506ee..c89fff98 100644
--- a/datapoollocal/PoolReadHelper.h
+++ b/datapoollocal/PoolReadHelper.h
@@ -9,12 +9,16 @@
  */
 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;
+#endif
 #endif
 		}
 	}
@@ -25,7 +29,7 @@ public:
 
 	~PoolReadHelper() {
 		if(readObject != nullptr) {
-			readObject->commit(mutexTimeout);
+			readObject->commit(timeoutType, mutexTimeout);
 		}
 
 	}
@@ -33,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/CMakeLists.txt b/defaultcfg/fsfwconfig/CMakeLists.txt
index b8b41c93..cbd4ecde 100644
--- a/defaultcfg/fsfwconfig/CMakeLists.txt
+++ b/defaultcfg/fsfwconfig/CMakeLists.txt
@@ -1,15 +1,15 @@
-target_sources(${TARGET_NAME} 
-	PRIVATE
+target_sources(${LIB_FSFW_NAME} PRIVATE
+	ipc/missionMessageTypes.cpp
+	objects/FsfwFactory.cpp
+	pollingsequence/PollingSequenceFactory.cpp
 )
 
-# Add include paths for the executable
-target_include_directories(${TARGET_NAME} 
-	PUBLIC 
-		${CMAKE_CURRENT_SOURCE_DIR}
+# Should be added to include path
+target_include_directories(${TARGET_NAME} PRIVATE
+	${CMAKE_CURRENT_SOURCE_DIR}
 )
 
-# Add include paths for the FSFW library
-target_include_directories(${LIB_FSFW_NAME} 
-	PUBLIC
-		${CMAKE_CURRENT_SOURCE_DIR}
-)
\ No newline at end of file
+if(NOT FSFW_CONFIG_PATH)
+	set(FSFW_CONFIG_PATH ${CMAKE_CURRENT_SOURCE_DIR})
+endif()
+
diff --git a/defaultcfg/fsfwconfig/FSFWConfig.h b/defaultcfg/fsfwconfig/FSFWConfig.h
index e1815d05..261e3d6d 100644
--- a/defaultcfg/fsfwconfig/FSFWConfig.h
+++ b/defaultcfg/fsfwconfig/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)
@@ -50,6 +59,8 @@ static constexpr size_t FSFW_EVENTMGMR_RANGEMATCHERS   = 120;
 //! simulataneously. This will increase the required RAM for
 //! each CSB service !
 static constexpr uint8_t FSFW_CSB_FIFO_DEPTH = 6;
+
+static constexpr size_t FSFW_PRINT_BUFFER_SIZE = 124;
 }
 
 #endif /* CONFIG_FSFWCONFIG_H_ */
diff --git a/defaultcfg/fsfwconfig/devices/logicalAddresses.cpp b/defaultcfg/fsfwconfig/devices/logicalAddresses.cpp
deleted file mode 100644
index c7ce314d..00000000
--- a/defaultcfg/fsfwconfig/devices/logicalAddresses.cpp
+++ /dev/null
@@ -1,5 +0,0 @@
-#include "logicalAddresses.h"
-
-
-
-
diff --git a/defaultcfg/fsfwconfig/devices/logicalAddresses.h b/defaultcfg/fsfwconfig/devices/logicalAddresses.h
index e0827ba3..53edcd54 100644
--- a/defaultcfg/fsfwconfig/devices/logicalAddresses.h
+++ b/defaultcfg/fsfwconfig/devices/logicalAddresses.h
@@ -2,7 +2,7 @@
 #define CONFIG_DEVICES_LOGICALADDRESSES_H_
 
 #include <fsfw/devicehandlers/CookieIF.h>
-#include "../objects/systemObjectList.h"
+#include <objects/systemObjectList.h>
 #include <cstdint>
 
 /**
diff --git a/defaultcfg/fsfwconfig/devices/powerSwitcherList.cpp b/defaultcfg/fsfwconfig/devices/powerSwitcherList.cpp
deleted file mode 100644
index 343f78d0..00000000
--- a/defaultcfg/fsfwconfig/devices/powerSwitcherList.cpp
+++ /dev/null
@@ -1,4 +0,0 @@
-#include "powerSwitcherList.h"
-
-
-
diff --git a/defaultcfg/fsfwconfig/objects/Factory.cpp b/defaultcfg/fsfwconfig/objects/FsfwFactory.cpp
similarity index 60%
rename from defaultcfg/fsfwconfig/objects/Factory.cpp
rename to defaultcfg/fsfwconfig/objects/FsfwFactory.cpp
index 41333b1c..428adf1d 100644
--- a/defaultcfg/fsfwconfig/objects/Factory.cpp
+++ b/defaultcfg/fsfwconfig/objects/FsfwFactory.cpp
@@ -1,9 +1,5 @@
-#include "Factory.h"
-#include "../tmtc/apid.h"
-#include "../tmtc/pusIds.h"
-#include "../objects/systemObjectList.h"
-#include "../devices/logicalAddresses.h"
-#include "../devices/powerSwitcherList.h"
+#include "FsfwFactory.h"
+#include <OBSWConfig.h>
 
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw/events/EventManager.h>
@@ -11,24 +7,27 @@
 #include <fsfw/tmtcpacket/pus/TmPacketStored.h>
 #include <fsfw/tmtcservices/CommandingServiceBase.h>
 #include <fsfw/tmtcservices/PusServiceBase.h>
-#include <internalError/InternalErrorReporter.h>
+#include <fsfw/internalError/InternalErrorReporter.h>
 
 #include <cstdint>
 
 /**
- * This class should be used to create all system objects required for
- * the on-board software, using the object ID list from the configuration
- * folder.
+ * This function builds all system objects required for using
+ * the FSFW. It is recommended to build all other required objects
+ * in a function with an identical prototype, call this function in it and
+ * then pass the function to the object manager so it builds all system
+ * objects on software startup.
  *
- * The objects are registered in the internal object manager automatically.
- * This is used later to add objects to tasks.
+ * All system objects are registered in the internal object manager
+ * automatically. The objects should be added to tasks at a later stage, using
+ * their objects IDs.
  *
- * This file also sets static framework IDs.
+ * This function also sets static framework IDs.
  *
- * Framework objects are created first.
+ * Framework should be created first before creating mission system objects.
  * @ingroup init
  */
-void Factory::produce(void) {
+void Factory::produceFsfwObjects(void) {
 	setStaticFrameworkObjectIds();
 	new EventManager(objects::EVENT_MANAGER);
 	new HealthTable(objects::HEALTH_TABLE);
diff --git a/defaultcfg/fsfwconfig/objects/Factory.h b/defaultcfg/fsfwconfig/objects/FsfwFactory.h
similarity index 61%
rename from defaultcfg/fsfwconfig/objects/Factory.h
rename to defaultcfg/fsfwconfig/objects/FsfwFactory.h
index fe55deff..198b22f2 100644
--- a/defaultcfg/fsfwconfig/objects/Factory.h
+++ b/defaultcfg/fsfwconfig/objects/FsfwFactory.h
@@ -1,5 +1,5 @@
-#ifndef FACTORY_H_
-#define FACTORY_H_
+#ifndef FSFWCONFIG_OBJECTS_FACTORY_H_
+#define FSFWCONFIG_OBJECTS_FACTORY_H_
 
 #include <fsfw/objectmanager/SystemObjectIF.h>
 #include <cstddef>
@@ -9,9 +9,9 @@ namespace Factory {
 	 * @brief   Creates all SystemObject elements which are persistent
 	 *          during execution.
 	 */
-	void produce();
+	void produceFsfwObjects();
 	void setStaticFrameworkObjectIds();
 }
 
 
-#endif /* FACTORY_H_ */
+#endif /* FSFWCONFIG_OBJECTS_FACTORY_H_ */
diff --git a/defaultcfg/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp b/defaultcfg/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp
index f836a746..b7f1fb3e 100644
--- a/defaultcfg/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp
+++ b/defaultcfg/fsfwconfig/pollingsequence/PollingSequenceFactory.cpp
@@ -15,8 +15,10 @@ ReturnValue_t pst::pollingSequenceInitDefault(
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "pst::pollingSequenceInitDefault: Sequence invalid!"
 		        << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 }
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index d0630854..580f8c0d 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -39,11 +39,13 @@ 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
 	}
 	if (this->fdirInstance == nullptr) {
 		this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId,
@@ -130,24 +132,30 @@ 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
 		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
 	    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
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -156,10 +164,12 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 				AcceptsDeviceResponsesIF>(rawDataReceiverId);
 
 		if (rawReceiver == nullptr) {
+#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;
+#endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 		}
 		defaultRawReceiver = rawReceiver->getDeviceQueue();
@@ -168,10 +178,12 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 	if(powerSwitcherId != objects::NO_OBJECT) {
 		powerSwitcher = objectManager->get<PowerSwitchIF>(powerSwitcherId);
 		if (powerSwitcher == nullptr) {
+#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;
+#endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 		}
 	}
@@ -217,7 +229,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();
 		}
 
 	}
@@ -682,8 +695,10 @@ void DeviceHandlerBase::doGetRead() {
 		replyRawData(receivedData, receivedDataLen, requestedRawTraffic);
 	}
 
-	if (mode == MODE_RAW and defaultRawReceiver != MessageQueueIF::NO_QUEUE) {
-		replyRawReplyIfnotWiretapped(receivedData, receivedDataLen);
+	if (mode == MODE_RAW) {
+		if (defaultRawReceiver != MessageQueueIF::NO_QUEUE) {
+			replyRawReplyIfnotWiretapped(receivedData, receivedDataLen);
+		}
 	}
 	else {
 		parseReply(receivedData, receivedDataLen);
@@ -706,8 +721,10 @@ 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
 			}
 			break;
 		case APERIODIC_REPLY: {
@@ -718,8 +735,10 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 			            foundId);
 			}
 			if(foundLen == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			    sif::warning << "DeviceHandlerBase::parseReply: foundLen is 0!"
 			            " Packet parsing will be stuck." << std::endl;
+#endif
 			}
 			break;
 		}
@@ -1273,9 +1292,11 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 		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
 			result = NOTHING_TO_SEND; //no need to report this
 		}
 	}
@@ -1300,10 +1321,12 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 			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;
+#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
 			return;
diff --git a/devicehandlers/DeviceHandlerFailureIsolation.cpp b/devicehandlers/DeviceHandlerFailureIsolation.cpp
index 9d0535bf..5f7ab89f 100644
--- a/devicehandlers/DeviceHandlerFailureIsolation.cpp
+++ b/devicehandlers/DeviceHandlerFailureIsolation.cpp
@@ -169,8 +169,10 @@ void DeviceHandlerFailureIsolation::clearFaultCounters() {
 ReturnValue_t DeviceHandlerFailureIsolation::initialize() {
 	ReturnValue_t result = FailureIsolationBase::initialize();
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "DeviceHandlerFailureIsolation::initialize: Could not"
 				" initialize FailureIsolationBase." << std::endl;
+#endif
 		return result;
 	}
 	ConfirmsFailuresIF* power = objectManager->get<ConfirmsFailuresIF>(
@@ -250,8 +252,10 @@ bool DeviceHandlerFailureIsolation::isFdirInActionOrAreWeFaulty(
 
 	if (owner == nullptr) {
 	    // Configuration error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "DeviceHandlerFailureIsolation::"
 	            << "isFdirInActionOrAreWeFaulty: Owner not set!" << std::endl;
+#endif
 	    return false;
 	}
 
diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index 6cc97eb6..e25e574f 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -122,6 +122,7 @@ void EventManager::printEvent(EventMessage* message) {
 	case severity::INFO:
 #if DEBUG_INFO_EVENT == 1
 		string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::info << "EVENT: ";
 		if (string != 0) {
 			sif::info << string;
@@ -132,10 +133,12 @@ void EventManager::printEvent(EventMessage* message) {
 				<< std::dec << message->getEventId() << std::hex << ") P1: 0x"
 				<< message->getParameter1() << " P2: 0x"
 				<< message->getParameter2() << std::dec << std::endl;
-#endif
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* DEBUG_INFO_EVENT == 1 */
 		break;
 	default:
 		string = translateObject(message->getReporter());
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "EventManager: ";
 		if (string != 0) {
 			sif::debug << string;
@@ -146,22 +149,28 @@ void EventManager::printEvent(EventMessage* message) {
 		sif::debug << " reported " << translateEvents(message->getEvent())
 				<< " (" << std::dec << message->getEventId() << ") "
 				<< std::endl;
-
 		sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1()
 				<< ", P1 Dec: " << std::dec << message->getParameter1()
 				<< std::endl;
 		sif::debug << std::hex << "P2 Hex: 0x" << message->getParameter2()
 				<< ", P2 Dec: " <<  std::dec << message->getParameter2()
 				<< std::endl;
+#endif
 		break;
 	}
 }
 #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/fdir/FailureIsolationBase.cpp b/fdir/FailureIsolationBase.cpp
index a04a0313..69cb0f01 100644
--- a/fdir/FailureIsolationBase.cpp
+++ b/fdir/FailureIsolationBase.cpp
@@ -21,8 +21,10 @@ ReturnValue_t FailureIsolationBase::initialize() {
 	EventManagerIF* manager = objectManager->get<EventManagerIF>(
 			objects::EVENT_MANAGER);
 	if (manager == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "FailureIsolationBase::initialize: Event Manager has not"
 				" been initialized!" << std::endl;
+#endif
 		return RETURN_FAILED;
 	}
 	ReturnValue_t result = manager->registerListener(eventQueue->getId());
@@ -36,8 +38,10 @@ ReturnValue_t FailureIsolationBase::initialize() {
 		}
 		owner = objectManager->get<HasHealthIF>(ownerId);
 		if (owner == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "FailureIsolationBase::intialize: Owner object "
 					"invalid. Make sure it implements HasHealthIF" << std::endl;
+#endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 		}
 	}
@@ -45,10 +49,14 @@ ReturnValue_t FailureIsolationBase::initialize() {
 		ConfirmsFailuresIF* parentIF = objectManager->get<ConfirmsFailuresIF>(
 				faultTreeParent);
 		if (parentIF == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "FailureIsolationBase::intialize: Parent object"
 					<< "invalid." << std::endl;
+#endif
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "Make sure it implements ConfirmsFailuresIF."
 					<< std::endl;
+#endif
 			return ObjectManagerIF::CHILD_INIT_FAILED;
 			return RETURN_FAILED;
 		}
diff --git a/globalfunctions/arrayprinter.cpp b/globalfunctions/arrayprinter.cpp
index b1e888c5..c273b250 100644
--- a/globalfunctions/arrayprinter.cpp
+++ b/globalfunctions/arrayprinter.cpp
@@ -4,10 +4,12 @@
 
 void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
 		bool printInfo, size_t maxCharPerLine) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	if(printInfo) {
 		sif::info << "Printing data with size " << size << ": ";
 	}
 	sif::info << "[";
+#endif
 	if(type == OutputType::HEX) {
 		arrayprinter::printHex(data, size, maxCharPerLine);
 	}
@@ -21,6 +23,7 @@ void arrayprinter::print(const uint8_t *data, size_t size, OutputType type,
 
 void arrayprinter::printHex(const uint8_t *data, size_t size,
 		size_t maxCharPerLine) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::info << std::hex;
 	for(size_t i = 0; i < size; i++) {
 		sif::info << "0x" << static_cast<int>(data[i]);
@@ -28,16 +31,18 @@ void arrayprinter::printHex(const uint8_t *data, size_t size,
 			sif::info << " , ";
 			if(i > 0 and i % maxCharPerLine == 0) {
 				sif::info << std::endl;
+
 			}
 		}
-
 	}
 	sif::info << std::dec;
 	sif::info << "]" << std::endl;
+#endif
 }
 
 void arrayprinter::printDec(const uint8_t *data, size_t size,
 		size_t maxCharPerLine) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::info << std::dec;
 	for(size_t i = 0; i < size; i++) {
 		sif::info << static_cast<int>(data[i]);
@@ -49,13 +54,16 @@ void arrayprinter::printDec(const uint8_t *data, size_t size,
 		}
 	}
 	sif::info << "]" << std::endl;
+#endif
 }
 
 void arrayprinter::printBin(const uint8_t *data, size_t size) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::info << "\n" << std::flush;
     for(size_t i = 0; i < size; i++) {
         sif::info << "Byte " << i + 1 << ": 0b"<<
                 std::bitset<8>(data[i]) << ",\n" << std::flush;
     }
     sif::info << "]" << std::endl;
+#endif
 }
diff --git a/health/HealthHelper.cpp b/health/HealthHelper.cpp
index d574634d..231d616e 100644
--- a/health/HealthHelper.cpp
+++ b/health/HealthHelper.cpp
@@ -41,14 +41,18 @@ ReturnValue_t HealthHelper::initialize() {
 	eventSender = objectManager->get<EventReportingProxyIF>(objectId);
 
 	if (healthTable == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "HealthHelper::initialize: Health table object needs"
 	            "to be created in factory." << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	if(eventSender == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "HealthHelper::initialize: Owner has to implement "
 	            "ReportingProxyIF." << std::endl;
+#endif
 	    return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -79,8 +83,10 @@ void HealthHelper::informParent(HasHealthIF::HealthState health,
 			health, oldHealth);
 	if (MessageQueueSenderIF::sendMessage(parentQueue, &information,
 	        owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "HealthHelper::informParent: sending health reply failed."
 				<< std::endl;
+#endif
 	}
 }
 
@@ -98,8 +104,10 @@ void HealthHelper::handleSetHealthCommand(CommandMessage* command) {
 	}
 	if (MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
 	        owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "HealthHelper::handleHealthCommand: sending health "
 		        "reply failed." << std::endl;
+#endif
 
 	}
 }
diff --git a/health/HealthTableIF.h b/health/HealthTableIF.h
index d61e6761..727c4496 100644
--- a/health/HealthTableIF.h
+++ b/health/HealthTableIF.h
@@ -5,6 +5,8 @@
 #include "../objectmanager/ObjectManagerIF.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 
+#include <utility>
+
 class HealthTableIF: public ManagesHealthIF {
 public:
 	virtual ~HealthTableIF() {}
diff --git a/internalError/InternalErrorReporter.cpp b/internalError/InternalErrorReporter.cpp
index bfb67289..6842da8e 100644
--- a/internalError/InternalErrorReporter.cpp
+++ b/internalError/InternalErrorReporter.cpp
@@ -23,20 +23,22 @@ 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();
 	uint32_t newStoreHits = getAndResetStoreHits();
 
-#ifdef DEBUG
+#if FSFW_ENHANCED_PRINTOUT == 1
 	if(diagnosticPrintout) {
 	    if((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	        sif::debug << "InternalErrorReporter::performOperation: Errors "
 	                << "occured!" << std::endl;
 	        sif::debug << "Queue errors: " << newQueueHits << std::endl;
 	        sif::debug << "TM errors: " << newTmHits << std::endl;
 	        sif::debug << "Store errors: " << newStoreHits << std::endl;
+#endif
 	    }
 	}
 #endif
@@ -44,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();
 
@@ -67,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();
@@ -76,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();
@@ -99,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();
 }
@@ -117,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();
@@ -126,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();
 }
@@ -188,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/MessageQueueMessage.cpp b/ipc/MessageQueueMessage.cpp
index dcd4def3..e97778c3 100644
--- a/ipc/MessageQueueMessage.cpp
+++ b/ipc/MessageQueueMessage.cpp
@@ -15,8 +15,10 @@ MessageQueueMessage::MessageQueueMessage(uint8_t* data, size_t size) :
 		this->messageSize = this->HEADER_SIZE + size;
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "MessageQueueMessage: Passed size larger than maximum"
 				"allowed size! Setting content to 0" << std::endl;
+#endif
 		memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
 		this->messageSize = this->HEADER_SIZE;
 	}
@@ -52,7 +54,9 @@ void MessageQueueMessage::setSender(MessageQueueId_t setId) {
 }
 
 void MessageQueueMessage::print(bool printWholeMessage) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "MessageQueueMessage content: " << std::endl;
+#endif
 	if(printWholeMessage) {
 	    arrayprinter::print(getData(), getMaximumMessageSize());
 	}
diff --git a/ipc/MutexHelper.h b/ipc/MutexHelper.h
index 97001ade..befa69bc 100644
--- a/ipc/MutexHelper.h
+++ b/ipc/MutexHelper.h
@@ -12,12 +12,16 @@ public:
 		ReturnValue_t status = mutex->lockMutex(timeoutType,
 		        timeoutMs);
 		if(status == MutexIF::MUTEX_TIMEOUT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MutexHelper: Lock of mutex failed with timeout of "
 					<< timeoutMs << " milliseconds!" << std::endl;
+#endif
 		}
 		else if(status != HasReturnvaluesIF::RETURN_OK){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MutexHelper: Lock of Mutex failed with code "
 			        << status << std::endl;
+#endif
 		}
 	}
 
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/memory/MemoryHelper.cpp b/memory/MemoryHelper.cpp
index 6b11882f..42ac2654 100644
--- a/memory/MemoryHelper.cpp
+++ b/memory/MemoryHelper.cpp
@@ -16,7 +16,9 @@ ReturnValue_t MemoryHelper::handleMemoryCommand(CommandMessage* message) {
 	lastSender = message->getSender();
 	lastCommand = message->getCommand();
 	if (busy) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "MemHelper: Busy!" << std::endl;
+#endif
 	}
 	switch (lastCommand) {
 	case MemoryMessage::CMD_MEMORY_DUMP:
diff --git a/monitoring/MonitoringMessageContent.h b/monitoring/MonitoringMessageContent.h
index 44d32656..2822743a 100644
--- a/monitoring/MonitoringMessageContent.h
+++ b/monitoring/MonitoringMessageContent.h
@@ -72,8 +72,10 @@ private:
 		if (timeStamper == nullptr) {
 			timeStamper = objectManager->get<TimeStamperIF>( timeStamperId );
 			if ( timeStamper == nullptr ) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "MonitoringReportContent::checkAndSetStamper: "
 				        "Stamper not found!" << std::endl;
+#endif
 				return false;
 			}
 		}
diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp
index 5871bfcd..882d2762 100644
--- a/objectmanager/ObjectManager.cpp
+++ b/objectmanager/ObjectManager.cpp
@@ -1,6 +1,9 @@
 #include "ObjectManager.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 #include <iomanip>
+#endif
 #include <cstdlib>
 
 ObjectManager::ObjectManager( void (*setProducer)() ):
@@ -18,13 +21,18 @@ ObjectManager::~ObjectManager() {
 ReturnValue_t ObjectManager::insert( object_id_t id, SystemObjectIF* object) {
 	auto returnPair = objectList.emplace(id, object);
 	if (returnPair.second) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    // sif::debug << "ObjectManager::insert: Object " << std::hex
 	    //            << (int)id << std::dec << " inserted." << std::endl;
+#endif
 		return this->RETURN_OK;
 	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::insert: Object id " << std::hex
-		           << (int)id << std::dec << " is already in use!" << std::endl;
+		           << static_cast<uint32_t>(id) << std::dec
+				   << " is already in use!" << std::endl;
 		sif::error << "Terminating program." << std::endl;
+#endif
 		//This is very severe and difficult to handle in other places.
 		std::exit(INSERTION_FAILED);
 	}
@@ -33,12 +41,16 @@ ReturnValue_t ObjectManager::insert( object_id_t id, SystemObjectIF* object) {
 ReturnValue_t ObjectManager::remove( object_id_t id ) {
 	if ( this->getSystemObject(id) != NULL ) {
 		this->objectList.erase( id );
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		//sif::debug << "ObjectManager::removeObject: Object " << std::hex
 		//           << (int)id << std::dec << " removed." << std::endl;
+#endif
 		return RETURN_OK;
 	} else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::removeObject: Requested object "
 		        << std::hex << (int)id << std::dec << " not found." << std::endl;
+#endif
 		return NOT_FOUND;
 	}
 }
@@ -60,8 +72,10 @@ ObjectManager::ObjectManager() : produceObjects(nullptr) {
 
 void ObjectManager::initialize() {
 	if(produceObjects == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::initialize: Passed produceObjects "
 				"functions is nullptr!" << std::endl;
+#endif
 		return;
 	}
 	this->produceObjects();
@@ -70,38 +84,49 @@ void ObjectManager::initialize() {
 	for (auto const& it : objectList) {
 		result = it.second->initialize();
 		if ( result != RETURN_OK ) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			object_id_t var = it.first;
 			sif::error << "ObjectManager::initialize: Object 0x" << std::hex <<
 					std::setw(8) << std::setfill('0')<< var << " failed to "
 					"initialize with code 0x" << result << std::dec <<
 					std::setfill(' ') << std::endl;
+#endif
 			errorCount++;
 		}
 	}
 	if (errorCount > 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::ObjectManager: Counted " << errorCount
 		           << " failed initializations." << std::endl;
+#endif
 	}
 	//Init was successful. Now check successful interconnections.
 	errorCount = 0;
 	for (auto const& it : objectList) {
 		result = it.second->checkObjectConnections();
 		if ( result != RETURN_OK ) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "ObjectManager::ObjectManager: Object " << std::hex <<
 					(int) it.first  << " connection check failed with code 0x"
 					<< result << std::dec << std::endl;
+#endif
 			errorCount++;
 		}
 	}
 	if (errorCount > 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManager::ObjectManager: Counted " << errorCount
 		           << " failed connection checks." << std::endl;
+#endif
 	}
 }
 
 void ObjectManager::printList() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "ObjectManager: Object List contains:" << std::endl;
 	for (auto const& it : objectList) {
 		sif::debug << std::hex << it.first << " | " << it.second << std::endl;
+
 	}
+#endif
 }
diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h
index 4bd1d915..8bebb609 100644
--- a/objectmanager/ObjectManagerIF.h
+++ b/objectmanager/ObjectManagerIF.h
@@ -86,8 +86,10 @@ extern ObjectManagerIF *objectManager;
 template <typename T>
 T* ObjectManagerIF::get( object_id_t id ) {
 	if(objectManager == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectManagerIF: Global object manager has not "
 				"been initialized yet!" << std::endl;
+#endif
 	}
 	SystemObjectIF* temp = this->getSystemObject(id);
 	return dynamic_cast<T*>(temp);
diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp
index a420ec48..7d609aee 100644
--- a/osal/FreeRTOS/BinSemaphUsingTask.cpp
+++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp
@@ -8,8 +8,10 @@
 BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() {
 	handle = TaskManagement::getCurrentTaskHandle();
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Could not retrieve task handle. Please ensure the"
 				"constructor was called inside a task." << std::endl;
+#endif
 	}
 	xTaskNotifyGive(handle);
 }
diff --git a/osal/FreeRTOS/BinarySemaphore.cpp b/osal/FreeRTOS/BinarySemaphore.cpp
index 6fee5c35..c0349b7c 100644
--- a/osal/FreeRTOS/BinarySemaphore.cpp
+++ b/osal/FreeRTOS/BinarySemaphore.cpp
@@ -5,7 +5,9 @@
 BinarySemaphore::BinarySemaphore() {
 	handle = xSemaphoreCreateBinary();
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Semaphore: Binary semaph creation failure" << std::endl;
+#endif
 	}
 	// Initiated semaphore must be given before it can be taken.
 	xSemaphoreGive(handle);
@@ -18,7 +20,9 @@ BinarySemaphore::~BinarySemaphore() {
 BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) {
     handle = xSemaphoreCreateBinary();
     if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "Binary semaphore creation failure" << std::endl;
+#endif
     }
     xSemaphoreGive(handle);
 }
@@ -28,7 +32,9 @@ BinarySemaphore& BinarySemaphore::operator =(
     if(&s != this) {
         handle = xSemaphoreCreateBinary();
         if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "Binary semaphore creation failure" << std::endl;
+#endif
         }
         xSemaphoreGive(handle);
     }
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/FreeRTOS/CountingSemaphUsingTask.cpp b/osal/FreeRTOS/CountingSemaphUsingTask.cpp
index 9d309acc..750ea9d6 100644
--- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp
+++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp
@@ -9,25 +9,31 @@
 CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount,
 		uint8_t initCount): maxCount(maxCount) {
 	if(initCount > maxCount) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
 				"intial cout. Setting initial count to max count." << std::endl;
+#endif
 		initCount = maxCount;
 	}
 
 	handle = TaskManagement::getCurrentTaskHandle();
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphoreUsingTask: Could not retrieve task "
 				"handle. Please ensure the constructor was called inside a "
 				"task." << std::endl;
+#endif
 	}
 
 	uint32_t oldNotificationValue;
 	xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite,
 			&oldNotificationValue);
 	if(oldNotificationValue != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but "
 				"current notification value is not 0. Please ensure the "
 				"notification value is not used for other purposes!" << std::endl;
+#endif
 	}
 	for(int i = 0; i < initCount; i++) {
 		xTaskNotifyGive(handle);
diff --git a/osal/FreeRTOS/CountingSemaphore.cpp b/osal/FreeRTOS/CountingSemaphore.cpp
index a202e480..40884d27 100644
--- a/osal/FreeRTOS/CountingSemaphore.cpp
+++ b/osal/FreeRTOS/CountingSemaphore.cpp
@@ -10,14 +10,18 @@
 CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount):
 		maxCount(maxCount), initCount(initCount) {
 	if(initCount > maxCount) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
 				"intial cout. Setting initial count to max count." << std::endl;
+#endif
 		initCount = maxCount;
 	}
 
 	handle = xSemaphoreCreateCounting(maxCount, initCount);
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+#endif
 	}
 }
 
@@ -25,7 +29,9 @@ CountingSemaphore::CountingSemaphore(CountingSemaphore&& other):
 		maxCount(other.maxCount), initCount(other.initCount) {
 	handle = xSemaphoreCreateCounting(other.maxCount, other.initCount);
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+#endif
 	}
 }
 
@@ -33,7 +39,9 @@ CountingSemaphore& CountingSemaphore::operator =(
 		CountingSemaphore&& other) {
 	handle = xSemaphoreCreateCounting(other.maxCount, other.initCount);
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphore: Creation failure" << std::endl;
+#endif
 	}
 	return * this;
 }
diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp
index fc5b89fe..aa7e6c59 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@@ -37,15 +37,19 @@ void FixedTimeslotTask::taskEntryPoint(void* argument) {
 	}
 
 	originalTask->taskFunctionality();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Polling task " << originalTask->handle
 			<< " returned from taskFunctionality." << std::endl;
+#endif
 }
 
 void FixedTimeslotTask::missedDeadlineCounter() {
 	FixedTimeslotTask::deadlineMissedCount++;
 	if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount
 				<< " deadlines." << std::endl;
+#endif
 	}
 }
 
@@ -69,8 +73,10 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "Component " << std::hex << componentId <<
 			" not found, not adding it to pst" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index fdadf8b7..c0c82cf1 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -10,6 +10,7 @@
 MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 		maxMessageSize(maxMessageSize) {
 	handle = xQueueCreate(messageDepth, maxMessageSize);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	if (handle == nullptr) {
 		sif::error << "MessageQueue::MessageQueue:"
 		        << " Creation failed." << std::endl;
@@ -17,7 +18,9 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 		        << std::endl;
 		sif::error << "Specified Maximum Message Size: "
 		        << maxMessageSize << std::endl;
+
 	}
+#endif
 }
 
 MessageQueue::~MessageQueue() {
diff --git a/osal/FreeRTOS/Mutex.cpp b/osal/FreeRTOS/Mutex.cpp
index 9d9638b5..0b85ca13 100644
--- a/osal/FreeRTOS/Mutex.cpp
+++ b/osal/FreeRTOS/Mutex.cpp
@@ -5,7 +5,9 @@
 Mutex::Mutex() {
 	handle = xSemaphoreCreateMutex();
 	if(handle == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex::Mutex(FreeRTOS): Creation failure" << std::endl;
+#endif
 	}
 }
 
diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 2ee7bec0..3e830c7f 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -13,8 +13,10 @@ PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
 	BaseType_t status = xTaskCreate(taskEntryPoint, name,
 			stackSize, this, setPriority, &handle);
 	if(status != pdPASS){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "PeriodicTask Insufficient heap memory remaining. "
 		        "Status: " << status << std::endl;
+#endif
 	}
 
 }
@@ -41,8 +43,10 @@ void PeriodicTask::taskEntryPoint(void* argument) {
 	}
 
 	originalTask->taskFunctionality();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Polling task " << originalTask->handle
 			<< " returned from taskFunctionality." << std::endl;
+#endif
 }
 
 ReturnValue_t PeriodicTask::startTask() {
@@ -99,8 +103,10 @@ ReturnValue_t PeriodicTask::addComponent(object_id_t object) {
 	ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
 			object);
 	if (newObject == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "PeriodicTask::addComponent: Invalid object. Make sure"
 	            "it implement ExecutableObjectIF" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	objectList.push_back(newObject);
diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp
index beb0d096..df005f6a 100644
--- a/osal/FreeRTOS/SemaphoreFactory.cpp
+++ b/osal/FreeRTOS/SemaphoreFactory.cpp
@@ -32,8 +32,10 @@ SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t argument) {
 		return new BinarySemaphoreUsingTask();
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "SemaphoreFactory: Invalid argument, return regular"
 				"binary semaphore" << std::endl;
+#endif
 		return new BinarySemaphore();
 	}
 }
@@ -47,8 +49,10 @@ SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t maxCount,
 		return new CountingSemaphoreUsingTask(maxCount, initCount);
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "SemaphoreFactory: Invalid argument, return regular"
 						"binary semaphore" << std::endl;
+#endif
 		return new CountingSemaphore(maxCount, initCount);
 	}
 
diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp
index 41321eeb..88bf51d2 100644
--- a/osal/host/Clock.cpp
+++ b/osal/host/Clock.cpp
@@ -14,7 +14,9 @@ MutexIF* Clock::timeMutex = NULL;
 using SystemClock = std::chrono::system_clock;
 
 uint32_t Clock::getTicksPerSecond(void){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl;
+#endif
 	return 0;
 	//return CLOCKS_PER_SEC;
 	//uint32_t ticks = sysconf(_SC_CLK_TCK);
@@ -23,7 +25,9 @@ uint32_t Clock::getTicksPerSecond(void){
 
 ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {
 	// do some magic with chrono
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::setClock: not implemented yet" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -36,7 +40,9 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 #else
 
 #endif
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
@@ -60,7 +66,9 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 	time->tv_usec = timeUnix.tv_nsec / 1000.0;
 	return HasReturnvaluesIF::RETURN_OK;
 #else
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_FAILED;
 #endif
 
@@ -68,7 +76,9 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
 	// do some magic with chrono
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -90,7 +100,9 @@ timeval Clock::getUptime() {
 		timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6);
 	}
 #else
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl;
+#endif
 #endif
 	return timeval;
 }
@@ -126,7 +138,9 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 	auto usecond = std::chrono::duration_cast<std::chrono::microseconds>(fraction);
 	time->usecond = usecond.count();
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -148,7 +162,9 @@ ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from,
 	to->tv_usec = from->usecond;
 	//Fails in 2038..
 	return HasReturnvaluesIF::RETURN_OK;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -186,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;
 	}
@@ -201,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/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 9e892bb5..272d99b9 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -35,15 +35,19 @@ FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority,
             reinterpret_cast<HANDLE>(mainThread.native_handle()),
             ABOVE_NORMAL_PRIORITY_CLASS);
     if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
                 << GetLastError() << std::endl;
+#endif
     }
     result = SetThreadPriority(
             reinterpret_cast<HANDLE>(mainThread.native_handle()),
             THREAD_PRIORITY_NORMAL);
     if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code "
                 << GetLastError() << std::endl;
+#endif
     }
 #elif defined(LINUX)
     // TODO: we can just copy and paste the code from the linux OSAL here.
@@ -70,8 +74,10 @@ void FixedTimeslotTask::taskEntryPoint(void* argument) {
     }
 
     this->taskFunctionality();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::debug << "FixedTimeslotTask::taskEntryPoint: "
             "Returned from taskFunctionality." << std::endl;
+#endif
 }
 
 ReturnValue_t FixedTimeslotTask::startTask() {
@@ -134,8 +140,10 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
         return HasReturnvaluesIF::RETURN_OK;
     }
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Component " << std::hex << componentId <<
             " not found, not adding it to pst" << std::endl;
+#endif
     return HasReturnvaluesIF::RETURN_FAILED;
 }
 
diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index d662d782..dfc045e8 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -10,8 +10,10 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize):
 	queueLock = MutexFactory::instance()->createMutex();
 	auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::MessageQueue:"
 		        << " Could not be created" << std::endl;
+#endif
 	}
 }
 
@@ -137,8 +139,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 			targetQueue->messageQueue.push(*mqmMessage);
 		}
 		else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
 					"is not MessageQueueMessage!" << std::endl;
+#endif
 		}
 
 	}
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/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp
index bfa6c3fd..1f427546 100644
--- a/osal/host/PeriodicTask.cpp
+++ b/osal/host/PeriodicTask.cpp
@@ -33,15 +33,19 @@ PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
             reinterpret_cast<HANDLE>(mainThread.native_handle()),
             ABOVE_NORMAL_PRIORITY_CLASS);
     if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
                 << GetLastError() << std::endl;
+#endif
     }
     result = SetThreadPriority(
             reinterpret_cast<HANDLE>(mainThread.native_handle()),
             THREAD_PRIORITY_NORMAL);
     if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "PeriodicTask: Windows SetPriorityClass failed with code "
                 << GetLastError() << std::endl;
+#endif
     }
 #elif defined(LINUX)
     // we can just copy and paste the code from linux here.
@@ -69,8 +73,10 @@ void PeriodicTask::taskEntryPoint(void* argument) {
 	}
 
 	this->taskFunctionality();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "PeriodicTask::taskEntryPoint: "
 			"Returned from taskFunctionality." << std::endl;
+#endif
 }
 
 ReturnValue_t PeriodicTask::startTask() {
diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp
index 621f46bc..7ffd4758 100644
--- a/osal/host/QueueMapManager.cpp
+++ b/osal/host/QueueMapManager.cpp
@@ -26,8 +26,10 @@ ReturnValue_t QueueMapManager::addMessageQueue(
 	auto returnPair = queueMap.emplace(currentId, queueToInsert);
 	if(not returnPair.second) {
 		// this should never happen for the atomic variable.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "QueueMapManager: This ID is already inside the map!"
 				<< std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if (id != nullptr) {
@@ -44,8 +46,10 @@ MessageQueueIF* QueueMapManager::getMessageQueue(
 		return queueIter->second;
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "QueueMapManager::getQueueHandle: The ID " <<
 				messageQueueId << " does not exists in the map" << std::endl;
+#endif
 		return nullptr;
 	}
 }
diff --git a/osal/host/SemaphoreFactory.cpp b/osal/host/SemaphoreFactory.cpp
index b19b2c75..3d3fe17f 100644
--- a/osal/host/SemaphoreFactory.cpp
+++ b/osal/host/SemaphoreFactory.cpp
@@ -21,16 +21,20 @@ SemaphoreFactory* SemaphoreFactory::instance() {
 
 SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) {
 	// Just gonna wait for full C++20 for now.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet."
 			" Returning nullptr!\n" << std::flush;
+#endif
 	return nullptr;
 }
 
 SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount,
 		uint8_t initCount, uint32_t arguments) {
 	// Just gonna wait for full C++20 for now.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet."
 				" Returning nullptr!\n" << std::flush;
+#endif
 	return nullptr;
 }
 
diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp
index b81fa109..0a6bb29f 100644
--- a/osal/linux/BinarySemaphore.cpp
+++ b/osal/linux/BinarySemaphore.cpp
@@ -43,8 +43,10 @@ ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType,
 		timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000;
 	    result = sem_timedwait(&handle, &timeOut);
 	    if(result != 0 and errno == EINVAL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    	sif::debug << "BinarySemaphore::acquire: Invalid time value possible"
 	    			<< std::endl;
+#endif
 	    }
 	}
 	if(result == 0) {
@@ -62,8 +64,10 @@ ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType,
 		return SemaphoreIF::SEMAPHORE_INVALID;
 	case(EINTR):
 		// Call was interrupted by signal handler
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "BinarySemaphore::acquire: Signal handler interrupted."
 				"Code " << strerror(errno) << std::endl;
+#endif
 		/* No break */
 	default:
 		return HasReturnvaluesIF::RETURN_FAILED;
@@ -126,8 +130,10 @@ void BinarySemaphore::initSemaphore(uint8_t initCount) {
 			// Value exceeds SEM_VALUE_MAX
 		case(ENOSYS):
 		// System does not support process-shared semaphores
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "BinarySemaphore: Init failed with" << strerror(errno)
 				<< std::endl;
+#endif
 		}
 	}
 }
diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp
index 6cddda35..35cbfae0 100644
--- a/osal/linux/Clock.cpp
+++ b/osal/linux/Clock.cpp
@@ -69,7 +69,9 @@ timeval Clock::getUptime() {
 	timeval uptime;
 	auto result = getUptime(&uptime);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Clock::getUptime: Error getting uptime" << std::endl;
+#endif
 	}
 	return uptime;
 }
@@ -180,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;
 	}
@@ -195,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;
 	}
@@ -207,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/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp
index 18339399..0bb71831 100644
--- a/osal/linux/CountingSemaphore.cpp
+++ b/osal/linux/CountingSemaphore.cpp
@@ -4,8 +4,10 @@
 CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount):
 		maxCount(maxCount), initCount(initCount) {
 	if(initCount > maxCount) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CountingSemaphoreUsingTask: Max count bigger than "
 				"intial cout. Setting initial count to max count." << std::endl;
+#endif
 		initCount = maxCount;
 	}
 
diff --git a/osal/linux/FixedTimeslotTask.cpp b/osal/linux/FixedTimeslotTask.cpp
index 247a34ed..a545eeb7 100644
--- a/osal/linux/FixedTimeslotTask.cpp
+++ b/osal/linux/FixedTimeslotTask.cpp
@@ -47,8 +47,10 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "Component " << std::hex << componentId <<
 			" not found, not adding it to pst" << std::dec << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
@@ -91,7 +93,9 @@ void FixedTimeslotTask::taskFunctionality() {
 void FixedTimeslotTask::missedDeadlineCounter() {
 	FixedTimeslotTask::deadlineMissedCount++;
 	if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount
 				   << " deadlines." << std::endl;
+#endif
 	}
 }
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index cfadb793..e7b09b4f 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -10,7 +10,6 @@
 #include <errno.h>
 
 
-
 MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
 		id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
 		defaultDestination(MessageQueueIF::NO_QUEUE),
@@ -43,13 +42,17 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
 MessageQueue::~MessageQueue() {
 	int status = mq_close(this->id);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::Destructor: mq_close Failed with status: "
 				   << strerror(errno) <<std::endl;
+#endif
 	}
 	status = mq_unlink(name);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: "
 				   << strerror(errno) << std::endl;
+#endif
 	}
 }
 
@@ -57,8 +60,10 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 		uint32_t messageDepth) {
 	switch(errno) {
 	case(EINVAL): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::MessageQueue: Invalid name or attributes"
 				" for message size" << std::endl;
+#endif
 		size_t defaultMqMaxMsg = 0;
 		// Not POSIX conformant, but should work for all UNIX systems.
 		// Just an additional helpful printout :-)
@@ -79,11 +84,13 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 			Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
 			Apply changes with: sudo sysctl -p
 			*/
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::MessageQueue: Default MQ size "
 					<< defaultMqMaxMsg << " is too small for requested size "
 					<< messageDepth << std::endl;
 			sif::error << "This error can be fixed by setting the maximum "
 					"allowed message size higher!" << std::endl;
+#endif
 
 		}
 		break;
@@ -95,8 +102,10 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 		//We unlink the other queue
 		int status = mq_unlink(name);
 		if (status != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "mq_unlink Failed with status: " << strerror(errno)
 													<< std::endl;
+#endif
 		}
 		else {
 			// Successful unlinking, try to open again
@@ -114,9 +123,11 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 
 	default:
 		// Failed either the first time or the second time
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex
 		<< name << std::dec << " failed with status: "
 		<< strerror(errno) << std::endl;
+#endif
 
 	}
 	return HasReturnvaluesIF::RETURN_FAILED;
@@ -151,15 +162,19 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
 
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 	if(message == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::receiveMessage: Message is "
 				"nullptr!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
 	if(message->getMaximumMessageSize() < maxMessageSize) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::receiveMessage: Message size "
 				<< message->getMaximumMessageSize()
 				<< " too small to receive data!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -187,8 +202,10 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			return MessageQueueIF::EMPTY;
 		case EBADF:
 			//mqdes doesn't represent a valid queue open for reading.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::receive: configuration error "
 			           << strerror(errno)  << std::endl;
+#endif
 			/*NO BREAK*/
 		case EINVAL:
 			/*
@@ -200,8 +217,10 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			 *   queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
 			 *   been set in the queue's mq_flags.
 			 */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::receive: configuration error "
 					   << strerror(errno)  << std::endl;
+#endif
 			/*NO BREAK*/
 		case EMSGSIZE:
 			/*
@@ -213,8 +232,10 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			 *   given msg_len is too short for the message that would have
 			 *   been received.
 			 */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::receive: configuration error "
 			           << strerror(errno)  << std::endl;
+#endif
 			/*NO BREAK*/
 		case EINTR:
 			//The operation was interrupted by a signal.
@@ -237,8 +258,10 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
 		switch(errno){
 		case EBADF:
 			//mqdes doesn't represent a valid message queue.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::flush configuration error, "
 					"called flush with an invalid queue ID" << std::endl;
+#endif
 			/*NO BREAK*/
 		case EINVAL:
 			//mq_attr is NULL
@@ -253,8 +276,10 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) {
 		switch(errno){
 		case EBADF:
 			//mqdes doesn't represent a valid message queue.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::flush configuration error, "
 					"called flush with an invalid queue ID" << std::endl;
+#endif
 			/*NO BREAK*/
 		case EINVAL:
 			/*
@@ -306,8 +331,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 		MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
 		bool ignoreFault) {
 	if(message == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
 				"nullptr!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -335,11 +362,13 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 		case EBADF: {
 			//mq_des doesn't represent a valid message queue descriptor,
 			//or mq_des wasn't opened for writing.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::sendMessage: Configuration error, MQ"
 					<< " destination invalid."  << std::endl;
 			sif::error << strerror(errno) << " in "
 					<<"mq_send to: " << sendTo << " sent from "
 					<< sentFrom << std::endl;
+#endif
 			return DESTINVATION_INVALID;
 		}
 		case EINTR:
@@ -354,14 +383,18 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 			 * - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and
 			 *   msg_prio is greater than the priority of the calling process.
 			 */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::sendMessage: Configuration error "
 			           << strerror(errno) << " in mq_send" << std::endl;
+#endif
 			/*NO BREAK*/
 		case EMSGSIZE:
 			// The msg_len is greater than the msgsize associated with
 			//the specified queue.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "MessageQueue::sendMessage: Size error [" <<
 					strerror(errno) << "] in mq_send" << std::endl;
+#endif
 			/*NO BREAK*/
 		default:
 			return HasReturnvaluesIF::RETURN_FAILED;
diff --git a/osal/linux/Mutex.cpp b/osal/linux/Mutex.cpp
index b27ff8c0..c642b132 100644
--- a/osal/linux/Mutex.cpp
+++ b/osal/linux/Mutex.cpp
@@ -12,24 +12,32 @@ Mutex::Mutex() {
 	pthread_mutexattr_t mutexAttr;
 	int status = pthread_mutexattr_init(&mutexAttr);
 	if (status != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: Attribute init failed with: " << strerror(status) << std::endl;
+#endif
 	}
 	status = pthread_mutexattr_setprotocol(&mutexAttr, PTHREAD_PRIO_INHERIT);
 	if (status != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: Attribute set PRIO_INHERIT failed with: " << strerror(status)
 				<< std::endl;
+#endif
 	}
 	status = pthread_mutex_init(&mutex, &mutexAttr);
 	if (status != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: creation with name, id " << mutex.__data.__count
 				<< ", " << " failed with " << strerror(status) << std::endl;
+#endif
 	}
 	// After a mutex attributes object has been used to initialize one or more
 	// mutexes, any function affecting the attributes object
 	// (including destruction) shall not affect any previously initialized mutexes.
 	status = pthread_mutexattr_destroy(&mutexAttr);
 	if (status != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: Attribute destroy failed with " << strerror(status) << std::endl;
+#endif
 	}
 }
 
diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp
index 3c1df6bb..a8f2de60 100644
--- a/osal/linux/PeriodicPosixTask.cpp
+++ b/osal/linux/PeriodicPosixTask.cpp
@@ -25,8 +25,10 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) {
 	ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
 			object);
 	if (newObject == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PeriodicTask::addComponent: Invalid object. Make sure"
 				<< " it implements ExecutableObjectIF!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	objectList.push_back(newObject);
@@ -42,7 +44,9 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) {
 
 ReturnValue_t PeriodicPosixTask::startTask(void) {
 	started = true;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::info << stackSize << std::endl;
+#endif
 	PosixThread::createTask(&taskEntryPoint,this);
 	return HasReturnvaluesIF::RETURN_OK;
 }
@@ -67,12 +71,16 @@ void PeriodicPosixTask::taskFunctionality(void) {
 			char name[20] = {0};
 			int status = pthread_getname_np(pthread_self(), name, sizeof(name));
 			if(status == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "PeriodicPosixTask " << name << ": Deadline "
 						"missed." << std::endl;
+#endif
 			}
 			else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "PeriodicPosixTask X: Deadline missed. " <<
 						status << std::endl;
+#endif
 			}
 			if (this->deadlineMissedFunc != nullptr) {
 				this->deadlineMissedFunc();
diff --git a/osal/linux/PosixThread.cpp b/osal/linux/PosixThread.cpp
index 55d74de3..a0e540cf 100644
--- a/osal/linux/PosixThread.cpp
+++ b/osal/linux/PosixThread.cpp
@@ -48,8 +48,10 @@ void PosixThread::suspend() {
 	sigaddset(&waitSignal, SIGUSR1);
 	sigwait(&waitSignal, &caughtSig);
 	if (caughtSig != SIGUSR1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "FixedTimeslotTask: Unknown Signal received: " <<
 				caughtSig << std::endl;
+#endif
 	}
 }
 
@@ -118,7 +120,9 @@ uint64_t PosixThread::getCurrentMonotonicTimeMs(){
 
 
 void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::debug << "PosixThread::createTask" << std::endl;
+#endif
 	/*
 	 * The attr argument points to a pthread_attr_t structure whose contents
        are used at thread creation time to determine attributes for the new
@@ -129,53 +133,69 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
 	pthread_attr_t attributes;
 	int status = pthread_attr_init(&attributes);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread attribute init failed with: " <<
 				strerror(status) << std::endl;
+#endif
 	}
 	void* stackPointer;
 	status = posix_memalign(&stackPointer, sysconf(_SC_PAGESIZE), stackSize);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PosixThread::createTask: Stack init failed with: " <<
 				strerror(status) << std::endl;
+#endif
 		if(errno == ENOMEM) {
 			uint64_t stackMb = stackSize/10e6;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "PosixThread::createTask: Insufficient memory for"
 					" the requested " << stackMb << " MB" << std::endl;
+#endif
 		}
 		else if(errno == EINVAL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "PosixThread::createTask: Wrong alignment argument!"
 					<< std::endl;
+#endif
 		}
 		return;
 	}
 
 	status = pthread_attr_setstack(&attributes, stackPointer, stackSize);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PosixThread::createTask: pthread_attr_setstack "
 				" failed with: " << strerror(status) <<  std::endl;
 		sif::error << "Make sure the specified stack size is valid and is "
 				"larger than the minimum allowed stack size." << std::endl;
+#endif
 	}
 
 	status = pthread_attr_setinheritsched(&attributes, PTHREAD_EXPLICIT_SCHED);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "Posix Thread attribute setinheritsched failed with: " <<
 					strerror(status) << std::endl;
+#endif
 	}
 
 	// TODO FIFO -> This needs root privileges for the process
 	status = pthread_attr_setschedpolicy(&attributes,SCHED_FIFO);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread attribute schedule policy failed with: " <<
 				strerror(status) << std::endl;
+#endif
 	}
 
 	sched_param scheduleParams;
 	scheduleParams.__sched_priority = priority;
 	status = pthread_attr_setschedparam(&attributes, &scheduleParams);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread attribute schedule params failed with: " <<
 				strerror(status) << std::endl;
+#endif
 	}
 
 	//Set Signal Mask for suspend until startTask is called
@@ -184,36 +204,48 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) {
 	sigaddset(&waitSignal, SIGUSR1);
 	status = pthread_sigmask(SIG_BLOCK, &waitSignal, NULL);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread sigmask failed failed with: " <<
 				strerror(status) << " errno: " << strerror(errno) << std::endl;
+#endif
 	}
 
 
 	status = pthread_create(&thread,&attributes,fnc_,arg_);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread create failed with: " <<
 				strerror(status) << std::endl;
+#endif
 	}
 
 	status = pthread_setname_np(thread,name);
 	if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PosixThread::createTask: setname failed with: " <<
 				strerror(status) << std::endl;
+#endif
 		if(status == ERANGE) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "PosixThread::createTask: Task name length longer"
 					" than 16 chars. Truncating.." << std::endl;
+#endif
 			name[15] = '\0';
 			status = pthread_setname_np(thread,name);
 			if(status != 0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 				sif::error << "PosixThread::createTask: Setting name"
 						" did not work.." << std::endl;
+#endif
 			}
 		}
 	}
 
 	status = pthread_attr_destroy(&attributes);
 	if(status!=0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Posix Thread attribute destroy failed with: " <<
 				strerror(status) << std::endl;
+#endif
 	}
 }
diff --git a/osal/linux/TcUnixUdpPollingTask.cpp b/osal/linux/TcUnixUdpPollingTask.cpp
index af99ec91..a8387e87 100644
--- a/osal/linux/TcUnixUdpPollingTask.cpp
+++ b/osal/linux/TcUnixUdpPollingTask.cpp
@@ -39,14 +39,18 @@ ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
 				reinterpret_cast<sockaddr*>(&senderAddress), &senderSockLen);
 		if(bytesReceived < 0) {
 			// handle error
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TcSocketPollingTask::performOperation: Reception"
 					"error." << std::endl;
+#endif
 			handleReadError();
 
 			continue;
 		}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //		sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived
 //				<< " bytes received" << std::endl;
+#endif
 
 		ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
 		if(result != HasReturnvaluesIF::RETURN_FAILED) {
@@ -65,9 +69,11 @@ ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 			receptionBuffer.data(), bytesRead);
 	// arrayprinter::print(receptionBuffer.data(), bytesRead);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data "
 				"storage failed" << std::endl;
 		sif::error << "Packet size: " << bytesRead << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -75,8 +81,10 @@ ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 
 	result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Serial Polling: Sending message to queue failed"
 				<< std::endl;
+#endif
 		tcStore->deleteData(storeId);
 	}
 	return result;
@@ -85,15 +93,19 @@ ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 ReturnValue_t TcUnixUdpPollingTask::initialize() {
 	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
 	if (tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
 				<< std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	tmtcBridge = objectManager->get<TmTcUnixUdpBridge>(tmtcBridgeId);
 	if(tmtcBridge == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
 				" TMTC bridge object!" << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -115,8 +127,10 @@ void TcUnixUdpPollingTask::setTimeout(double timeoutSeconds) {
 	int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
 			&tval, sizeof(receptionTimeout));
 	if(result == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
 				"receive timeout failed with " << strerror(errno) << std::endl;
+#endif
 	}
 }
 
@@ -126,13 +140,17 @@ void TcUnixUdpPollingTask::handleReadError() {
 	case(EAGAIN): {
 		// todo: When working in timeout mode, this will occur more often
 		// and is not an error.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcUnixUdpPollingTask::handleReadError: Timeout."
 				<< std::endl;
+#endif
 		break;
 	}
 	default: {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcUnixUdpPollingTask::handleReadError: "
 				<< strerror(errno) << std::endl;
+#endif
 	}
 	}
 }
diff --git a/osal/linux/Timer.cpp b/osal/linux/Timer.cpp
index bae631d7..fe0fbebb 100644
--- a/osal/linux/Timer.cpp
+++ b/osal/linux/Timer.cpp
@@ -10,8 +10,10 @@ Timer::Timer() {
 	sigEvent.sigev_value.sival_ptr = &timerId;
 	int status = timer_create(CLOCK_MONOTONIC, &sigEvent, &timerId);
 	if(status!=0){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Timer creation failed with: " << status <<
 				" errno: " << errno << std::endl;
+#endif
 	}
 }
 
diff --git a/osal/linux/TmTcUnixUdpBridge.cpp b/osal/linux/TmTcUnixUdpBridge.cpp
index ab28623e..1d318d3b 100644
--- a/osal/linux/TmTcUnixUdpBridge.cpp
+++ b/osal/linux/TmTcUnixUdpBridge.cpp
@@ -26,8 +26,10 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
 	//clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
 	serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
 	if(serverSocket < 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open"
 				" UDP socket!" << std::endl;
+#endif
 		handleSocketError();
 		return;
 	}
@@ -51,9 +53,11 @@ TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
 			reinterpret_cast<struct sockaddr*>(&serverAddress),
 			serverAddressLen);
 	if(result == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind "
 				"local port " << setServerPort << " to server socket!"
 				<< std::endl;
+#endif
 		handleBindError();
 		return;
 	}
@@ -74,18 +78,24 @@ ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
 	}
 
 //	char ipAddress [15];
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //	sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
 //					&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#endif
 
 	ssize_t bytesSent = sendto(serverSocket, data, dataLen, flags,
 			reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
 	if(bytesSent < 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed."
 				<< std::endl;
+#endif
 		handleSendError();
 	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //	sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
 //			" sent." << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -93,10 +103,12 @@ void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in& newAddress) {
 	MutexHelper lock(mutex, MutexIF::TimeoutType::WAITING, 10);
 
 //	char ipAddress [15];
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //	sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
 //			&newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
 //	sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
 //			&clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#endif
 
 	// Set new IP address if it has changed.
 	if(clientAddress.sin_addr.s_addr != newAddress.sin_addr.s_addr) {
@@ -117,12 +129,16 @@ void TmTcUnixUdpBridge::handleSocketError() {
 	case(ENOBUFS):
 	case(ENOMEM):
 	case(EPROTONOSUPPORT):
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleSocketError: Socket creation failed"
 				<< " with " << strerror(errno) << std::endl;
+#endif
 		break;
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleSocketError: Unknown error"
 				<< std::endl;
+#endif
 		break;
 	}
 }
@@ -135,10 +151,12 @@ void TmTcUnixUdpBridge::handleBindError() {
 		 Ephermeral ports can be shown with following command:
 		 sysctl -A | grep ip_local_port_range
 		 */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleBindError: Port access issue."
 				"Ports 1-1024 are reserved on UNIX systems and require root "
 				"rights while ephermeral ports should not be used as well."
 				<< std::endl;
+#endif
 	}
 	break;
 	case(EADDRINUSE):
@@ -153,13 +171,17 @@ void TmTcUnixUdpBridge::handleBindError() {
 	case(ENOMEM):
 	case(ENOTDIR):
 	case(EROFS): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleBindError: Socket creation failed"
 				<< " with " << strerror(errno) << std::endl;
+#endif
 		break;
 	}
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleBindError: Unknown error"
 				<< std::endl;
+#endif
 		break;
 	}
 }
@@ -167,8 +189,10 @@ void TmTcUnixUdpBridge::handleBindError() {
 void TmTcUnixUdpBridge::handleSendError() {
 	switch(errno) {
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcUnixBridge::handleSendError: "
 		        << strerror(errno) << std::endl;
+#endif
 	}
 }
 
diff --git a/osal/rtems/CMakeLists.txt b/osal/rtems/CMakeLists.txt
new file mode 100644
index 00000000..bff03184
--- /dev/null
+++ b/osal/rtems/CMakeLists.txt
@@ -0,0 +1,18 @@
+target_sources(${LIB_FSFW_NAME} 
+	PRIVATE 
+		Clock.cpp
+		CpuUsage.cpp
+		InitTask.cpp
+		InternalErrorCodes.cpp
+		MessageQueue.cpp
+		MultiObjectTask.cpp
+		Mutex.cpp
+		MutexFactory.cpp
+		PollingTask.cpp
+		QueueFactory.cpp
+		RtemsBasic.cpp
+		TaskBase.cpp
+		TaskFactory.cpp
+)
+
+
diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp
index e5f37ec6..8bd7ac37 100644
--- a/osal/rtems/Clock.cpp
+++ b/osal/rtems/Clock.cpp
@@ -1,6 +1,10 @@
-#include "../../timemanager/Clock.h"
 #include "RtemsBasic.h"
+
+#include "../../timemanager/Clock.h"
+#include "../../ipc/MutexHelper.h"
+
 #include <rtems/score/todimpl.h>
+#include <rtems/rtems/clockimpl.h>
 
 uint16_t Clock::leapSeconds = 0;
 MutexIF* Clock::timeMutex = nullptr;
@@ -33,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) {
@@ -91,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/InternalErrorCodes.cpp b/osal/rtems/InternalErrorCodes.cpp
index ddf365d5..f4079814 100644
--- a/osal/rtems/InternalErrorCodes.cpp
+++ b/osal/rtems/InternalErrorCodes.cpp
@@ -34,8 +34,10 @@ ReturnValue_t InternalErrorCodes::translate(uint8_t code) {
 		return OUT_OF_PROXIES;
 	case INTERNAL_ERROR_INVALID_GLOBAL_ID:
 		return INVALID_GLOBAL_ID;
+#ifndef STM32H743ZI_NUCLEO
 	case INTERNAL_ERROR_BAD_STACK_HOOK:
 		return BAD_STACK_HOOK;
+#endif
 //	case INTERNAL_ERROR_BAD_ATTRIBUTES:
 //		return BAD_ATTRIBUTES;
 //	case INTERNAL_ERROR_IMPLEMENTATION_KEY_CREATE_INCONSISTENCY:
diff --git a/osal/rtems/MessageQueue.cpp b/osal/rtems/MessageQueue.cpp
index 839182a6..bfaf3569 100644
--- a/osal/rtems/MessageQueue.cpp
+++ b/osal/rtems/MessageQueue.cpp
@@ -9,9 +9,11 @@ MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
 	rtems_status_code status = rtems_message_queue_create(name, message_depth,
 			max_message_size, 0, &(this->id));
 	if (status != RTEMS_SUCCESSFUL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex
 				<< name << std::dec << " failed with status:"
 				<< (uint32_t) status << std::endl;
+#endif
 		this->id = 0;
 	}
 }
diff --git a/osal/rtems/MultiObjectTask.cpp b/osal/rtems/MultiObjectTask.cpp
index 970d01e1..b111f724 100644
--- a/osal/rtems/MultiObjectTask.cpp
+++ b/osal/rtems/MultiObjectTask.cpp
@@ -30,8 +30,10 @@ ReturnValue_t MultiObjectTask::startTask() {
 	rtems_status_code status = rtems_task_start(id, MultiObjectTask::taskEntryPoint,
 			rtems_task_argument((void *) this));
 	if (status != RTEMS_SUCCESSFUL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "ObjectTask::startTask for " << std::hex << this->getId()
 				<< std::dec << " failed." << std::endl;
+#endif
 	}
 	switch(status){
 	case RTEMS_SUCCESSFUL:
@@ -63,7 +65,9 @@ void MultiObjectTask::taskFunctionality() {
 			char nameSpace[8] = { 0 };
 			char* ptr = rtems_object_get_name(getId(), sizeof(nameSpace),
 					nameSpace);
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "ObjectTask: " << ptr << " Deadline missed." << std::endl;
+#endif
 			if (this->deadlineMissedFunc != nullptr) {
 				this->deadlineMissedFunc();
 			}
diff --git a/osal/rtems/Mutex.cpp b/osal/rtems/Mutex.cpp
index a5ec9635..7dd512fa 100644
--- a/osal/rtems/Mutex.cpp
+++ b/osal/rtems/Mutex.cpp
@@ -10,27 +10,31 @@ Mutex::Mutex() :
 	RTEMS_BINARY_SEMAPHORE | RTEMS_PRIORITY | RTEMS_INHERIT_PRIORITY, 0,
 			&mutexId);
 	if (status != RTEMS_SUCCESSFUL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: creation with name, id " << mutexName << ", " << mutexId
 				<< " failed with " << status << std::endl;
+#endif
 	}
 }
 
 Mutex::~Mutex() {
 	rtems_status_code status = rtems_semaphore_delete(mutexId);
 	if (status != RTEMS_SUCCESSFUL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Mutex: deletion for id " << mutexId
 				<< " failed with " << status << std::endl;
+#endif
 	}
 }
 
 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/osal/rtems/PollingTask.cpp b/osal/rtems/PollingTask.cpp
index db7864fe..0ebf63e2 100644
--- a/osal/rtems/PollingTask.cpp
+++ b/osal/rtems/PollingTask.cpp
@@ -35,15 +35,19 @@ rtems_task PollingTask::taskEntryPoint(rtems_task_argument argument) {
 	PollingTask *originalTask(reinterpret_cast<PollingTask*>(argument));
 	//The task's functionality is called.
 	originalTask->taskFunctionality();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Polling task " << originalTask->getId()
 			<< " returned from taskFunctionality." << std::endl;
+#endif
 }
 
 void PollingTask::missedDeadlineCounter() {
 	PollingTask::deadlineMissedCount++;
 	if (PollingTask::deadlineMissedCount % 10 == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PST missed " << PollingTask::deadlineMissedCount
 				<< " deadlines." << std::endl;
+#endif
 	}
 }
 
@@ -51,8 +55,10 @@ ReturnValue_t PollingTask::startTask() {
 	rtems_status_code status = rtems_task_start(id, PollingTask::taskEntryPoint,
 			rtems_task_argument((void *) this));
 	if (status != RTEMS_SUCCESSFUL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PollingTask::startTask for " << std::hex << this->getId()
 				<< std::dec << " failed." << std::endl;
+#endif
 	}
 	switch(status){
 	case RTEMS_SUCCESSFUL:
@@ -75,8 +81,10 @@ ReturnValue_t PollingTask::addSlot(object_id_t componentId,
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::error << "Component " << std::hex << componentId <<
 			" not found, not adding it to pst" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
diff --git a/osal/rtems/TaskBase.cpp b/osal/rtems/TaskBase.cpp
index 4e0c8f00..6abfcca8 100644
--- a/osal/rtems/TaskBase.cpp
+++ b/osal/rtems/TaskBase.cpp
@@ -22,9 +22,11 @@ TaskBase::TaskBase(rtems_task_priority set_priority, size_t stack_size,
 	}
 	ReturnValue_t result = convertReturnCode(status);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TaskBase::TaskBase: createTask with name " << std::hex
 				<< osalName << std::dec << " failed with return code "
 				<< (uint32_t) status << std::endl;
+#endif
 		this->id = 0;
 	}
 }
diff --git a/osal/windows/TcWinUdpPollingTask.cpp b/osal/windows/TcWinUdpPollingTask.cpp
index 06deafd1..4fd88c93 100644
--- a/osal/windows/TcWinUdpPollingTask.cpp
+++ b/osal/windows/TcWinUdpPollingTask.cpp
@@ -43,13 +43,17 @@ ReturnValue_t TcWinUdpPollingTask::performOperation(uint8_t opCode) {
 				&senderAddressSize);
 		if(bytesReceived == SOCKET_ERROR) {
 			// handle error
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TcWinUdpPollingTask::performOperation: Reception"
 					" error." << std::endl;
+#endif
 			handleReadError();
 			continue;
 		}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		//sif::debug << "TcWinUdpPollingTask::performOperation: " << bytesReceived
 		//		<< " bytes received" << std::endl;
+#endif
 
 		ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
 		if(result != HasReturnvaluesIF::RETURN_FAILED) {
@@ -68,9 +72,11 @@ ReturnValue_t TcWinUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 			receptionBuffer.data(), bytesRead);
 	// arrayprinter::print(receptionBuffer.data(), bytesRead);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data "
 				"storage failed" << std::endl;
 		sif::error << "Packet size: " << bytesRead << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -78,8 +84,10 @@ ReturnValue_t TcWinUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 
 	result  = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
 	if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Serial Polling: Sending message to queue failed"
 				<< std::endl;
+#endif
 		tcStore->deleteData(storeId);
 	}
 	return result;
@@ -88,21 +96,27 @@ ReturnValue_t TcWinUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
 ReturnValue_t TcWinUdpPollingTask::initialize() {
 	tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
 	if (tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
 				<< std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	tmtcBridge = objectManager->get<TmTcWinUdpBridge>(tmtcBridgeId);
 	if(tmtcBridge == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
 				" TMTC bridge object!" << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	serverUdpSocket = tmtcBridge->serverSocket;
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::info << "TcWinUdpPollingTask::initialize: Server UDP socket "
 	//        << serverUdpSocket << std::endl;
+#endif
 
 	return HasReturnvaluesIF::RETURN_OK;
 }
@@ -119,8 +133,10 @@ void TcWinUdpPollingTask::setTimeout(double timeoutSeconds) {
 	int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
 			reinterpret_cast<const char*>(&timeoutMs), sizeof(DWORD));
 	if(result == -1) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TcWinUdpPollingTask::TcSocketPollingTask: Setting "
 				"receive timeout failed with " << strerror(errno) << std::endl;
+#endif
 	}
 }
 
@@ -128,23 +144,31 @@ void TcWinUdpPollingTask::handleReadError() {
     int error = WSAGetLastError();
     switch(error) {
     case(WSANOTINITIALISED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TcWinUdpPollingTask::handleReadError: WSANOTINITIALISED: "
                 << "WSAStartup(...) call " << "necessary" << std::endl;
+#endif
         break;
     }
     case(WSAEFAULT): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TcWinUdpPollingTask::handleReadError: WSADEFAULT: "
                 << "Bad address " << std::endl;
+#endif
         break;
     }
     case(WSAEINVAL): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TcWinUdpPollingTask::handleReadError: WSAEINVAL: "
                 << "Invalid input parameters. " << std::endl;
+#endif
         break;
     }
     default: {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TcWinUdpPollingTask::handleReadError: Error code: "
                 << error << std::endl;
+#endif
         break;
     }
     }
diff --git a/osal/windows/TmTcWinUdpBridge.cpp b/osal/windows/TmTcWinUdpBridge.cpp
index e1ba132e..69a48f3e 100644
--- a/osal/windows/TmTcWinUdpBridge.cpp
+++ b/osal/windows/TmTcWinUdpBridge.cpp
@@ -15,8 +15,10 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId,
     if (err != 0) {
         /* Tell the user that we could not find a usable */
         /* Winsock DLL.                                  */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge:"
                 "WSAStartup failed with error: " << err << std::endl;
+#endif
         return;
     }
 
@@ -34,8 +36,10 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId,
     //clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
     serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
     if(serverSocket == INVALID_SOCKET) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not open"
                 " UDP socket!" << std::endl;
+#endif
         handleSocketError();
         return;
     }
@@ -59,9 +63,11 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId,
             reinterpret_cast<struct sockaddr*>(&serverAddress),
             serverAddressLen);
     if(result != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not bind "
                 "local port " << setServerPort << " to server socket!"
                 << std::endl;
+#endif
         handleBindError();
     }
 }
@@ -77,19 +83,25 @@ ReturnValue_t TmTcWinUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
     //clientAddressLen = sizeof(serverAddress);
 
 //  char ipAddress [15];
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //  sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
 //                  &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#endif
 
     ssize_t bytesSent = sendto(serverSocket,
             reinterpret_cast<const char*>(data), dataLen, flags,
             reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
     if(bytesSent == SOCKET_ERROR) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "TmTcWinUdpBridge::sendTm: Send operation failed."
                 << std::endl;
+#endif
         handleSendError();
     }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //  sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
 //          " sent." << std::endl;
+#endif
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -97,10 +109,12 @@ void TmTcWinUdpBridge::checkAndSetClientAddress(sockaddr_in newAddress) {
     MutexHelper lock(mutex, MutexIF::TimeoutType::WAITING, 10);
 
 //  char ipAddress [15];
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //  sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
 //          &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
 //  sif::debug << "IP Address Old: " <<  inet_ntop(AF_INET,
 //          &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
+#endif
     registerCommConnect();
 
     // Set new IP address if it has changed.
@@ -114,8 +128,10 @@ void TmTcWinUdpBridge::handleSocketError() {
     int errCode = WSAGetLastError();
     switch(errCode) {
     case(WSANOTINITIALISED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleSocketError: WSANOTINITIALISED: "
                 << "WSAStartup(...) call necessary" << std::endl;
+#endif
         break;
     }
     default: {
@@ -123,8 +139,10 @@ void TmTcWinUdpBridge::handleSocketError() {
         https://docs.microsoft.com/en-us/windows/win32/winsock/
         windows-sockets-error-codes-2
          */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleSocketError: Error code: "
                 << errCode << std::endl;
+#endif
         break;
     }
     }
@@ -134,13 +152,17 @@ void TmTcWinUdpBridge::handleBindError() {
     int errCode = WSAGetLastError();
     switch(errCode) {
     case(WSANOTINITIALISED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleBindError: WSANOTINITIALISED: "
                 << "WSAStartup(...) call " << "necessary" << std::endl;
+#endif
         break;
     }
     case(WSAEADDRINUSE): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     	sif::warning << "TmTcWinUdpBridge::handleBindError: WSAEADDRINUSE: "
     			<< "Port is already in use!" << std::endl;
+#endif
     	break;
     }
     default: {
@@ -148,8 +170,10 @@ void TmTcWinUdpBridge::handleBindError() {
         https://docs.microsoft.com/en-us/windows/win32/winsock/
         windows-sockets-error-codes-2
         */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleBindError: Error code: "
                 << errCode << std::endl;
+#endif
         break;
     }
     }
@@ -159,13 +183,17 @@ void TmTcWinUdpBridge::handleSendError() {
     int errCode = WSAGetLastError();
     switch(errCode) {
     case(WSANOTINITIALISED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleSendError: WSANOTINITIALISED: "
                 << "WSAStartup(...) call necessary" << std::endl;
+#endif
         break;
     }
     case(WSAEADDRNOTAVAIL): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleSendError: WSAEADDRNOTAVAIL: "
                 << "Check target address. " << std::endl;
+#endif
         break;
     }
     default: {
@@ -173,8 +201,10 @@ void TmTcWinUdpBridge::handleSendError() {
         https://docs.microsoft.com/en-us/windows/win32/winsock/
         windows-sockets-error-codes-2
         */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "TmTcWinUdpBridge::handleSendError: Error code: "
                 << errCode << std::endl;
+#endif
         break;
     }
     }
diff --git a/parameters/ParameterHelper.cpp b/parameters/ParameterHelper.cpp
index 23d1a1f3..4ad5cdf2 100644
--- a/parameters/ParameterHelper.cpp
+++ b/parameters/ParameterHelper.cpp
@@ -48,8 +48,10 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) {
         ConstStorageAccessor accessor(storeId);
         result = storage->getData(storeId, accessor);
         if (result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "ParameterHelper::handleParameterMessage: Getting"
                     << " store data failed for load command." << std::endl;
+#endif
             break;
         }
 
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/pus/CService201HealthCommanding.cpp b/pus/CService201HealthCommanding.cpp
index edacd73a..ca761f14 100644
--- a/pus/CService201HealthCommanding.cpp
+++ b/pus/CService201HealthCommanding.cpp
@@ -22,7 +22,9 @@ ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice)
 	case(Subservice::COMMAND_ANNOUNCE_HEALTH_ALL):
 		return RETURN_OK;
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Invalid Subservice" << std::endl;
+#endif
 		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
 	}
 }
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 86b0dcde..9e86c752 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -51,8 +51,10 @@ ReturnValue_t Service1TelecommandVerification::sendVerificationReport(
 		result = generateSuccessReport(message);
 	}
 	if(result != HasReturnvaluesIF::RETURN_OK){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Service1TelecommandVerification::sendVerificationReport: "
 		        "Sending verification packet failed !" << std::endl;
+#endif
 	}
 	return result;
 }
@@ -88,9 +90,11 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
 	AcceptsTelemetryIF* funnel = objectManager->
 			get<AcceptsTelemetryIF>(targetDestination);
 	if(funnel == nullptr){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "Service1TelecommandVerification::initialize: Specified"
 	            " TM funnel invalid. Make sure it is set up and implements"
 	            " AcceptsTelemetryIF." << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
diff --git a/pus/Service2DeviceAccess.cpp b/pus/Service2DeviceAccess.cpp
index 3648b7eb..72db82df 100644
--- a/pus/Service2DeviceAccess.cpp
+++ b/pus/Service2DeviceAccess.cpp
@@ -25,7 +25,9 @@ ReturnValue_t Service2DeviceAccess::isValidSubservice(uint8_t subservice) {
 	case Subservice::COMMAND_TOGGLE_WIRETAPPING:
 		return HasReturnvaluesIF::RETURN_OK;
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Invalid Subservice" << std::endl;
+#endif
 		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
 	}
 }
@@ -125,9 +127,11 @@ void Service2DeviceAccess::handleUnrequestedReply(CommandMessage* reply) {
 				static_cast<uint8_t>(Subservice::REPLY_RAW));
 		break;
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Unknown message in Service2DeviceAccess::"
 				"handleUnrequestedReply with command ID " <<
 				reply->getCommand() << std::endl;
+#endif
 		break;
 	}
 	//Must be reached by all cases to clear message
@@ -143,9 +147,11 @@ void Service2DeviceAccess::sendWiretappingTm(CommandMessage *reply,
 	size_t size = 0;
 	ReturnValue_t result = IPCStore->getData(storeAddress, &data, &size);
 	if(result != HasReturnvaluesIF::RETURN_OK){
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Service2DeviceAccess::sendWiretappingTm: Data Lost in "
 		        "handleUnrequestedReply with failure ID "<< result
 		        << std::endl;
+#endif
 		return;
 	}
 
diff --git a/pus/Service3Housekeeping.cpp b/pus/Service3Housekeeping.cpp
index 175af026..ba7ff6ee 100644
--- a/pus/Service3Housekeeping.cpp
+++ b/pus/Service3Housekeeping.cpp
@@ -222,8 +222,10 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
 	}
 
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Service3Housekeeping::handleReply: Invalid reply with "
 				<< "reply command " << command << "!" << std::endl;
+#endif
 		return CommandingServiceBase::INVALID_REPLY;
 	}
 	return HasReturnvaluesIF::RETURN_OK;
@@ -249,16 +251,20 @@ void Service3Housekeeping::handleUnrequestedReply(
 	}
 
 	default:
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Service3Housekeeping::handleUnrequestedReply: Invalid "
 		        << "reply with " << "reply command " << command << "!"
 		        << std::endl;
+#endif
 		return;
 	}
 
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 		// Configuration error
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "Service3Housekeeping::handleUnrequestedReply:"
 				<< "Could not generate reply!" << std::endl;
+#endif
 	}
 }
 
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index e0b34a5b..29eb7f20 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -37,8 +37,10 @@ ReturnValue_t Service5EventReporting::performService() {
 	       }
 	    }
 	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug <<  "Service5EventReporting::generateEventReport:"
 	        " Too many events" << std::endl;
+#endif
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -53,8 +55,10 @@ ReturnValue_t Service5EventReporting::generateEventReport(
 	ReturnValue_t result = tmPacket.sendPacket(
 	        requestQueue->getDefaultDestination(),requestQueue->getId());
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "Service5EventReporting::generateEventReport:"
 		        " Could not send TM packet" << std::endl;
+#endif
 	}
 	return result;
 }
diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp
index d710c56e..49fa6ebe 100644
--- a/pus/Service8FunctionManagement.cpp
+++ b/pus/Service8FunctionManagement.cpp
@@ -60,9 +60,11 @@ ReturnValue_t Service8FunctionManagement::prepareCommand(
 ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
 		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
     if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "Service8FunctionManagement::prepareDirectCommand:"
                 << " TC size smaller thant minimum size of direct command."
                 << std::endl;
+#endif
         return CommandingServiceBase::INVALID_TC;
     }
 
@@ -125,8 +127,10 @@ ReturnValue_t Service8FunctionManagement::handleDataReply(
     const uint8_t * buffer = nullptr;
     ReturnValue_t result = IPCStore->getData(storeId, &buffer, &size);
     if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "Service 8: Could not retrieve data for data reply"
                 << std::endl;
+#endif
         return result;
     }
     DataReply dataReply(objectId, actionId, buffer, size);
@@ -135,8 +139,10 @@ ReturnValue_t Service8FunctionManagement::handleDataReply(
 
     auto deletionResult = IPCStore->deleteData(storeId);
     if(deletionResult != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "Service8FunctionManagement::handleReply: Deletion"
                 << " of data in pool failed." << std::endl;
+#endif
     }
     return result;
 }
diff --git a/serialize/EndianConverter.h b/serialize/EndianConverter.h
index c888a905..8f3c7a7e 100644
--- a/serialize/EndianConverter.h
+++ b/serialize/EndianConverter.h
@@ -3,7 +3,6 @@
 
 #include "../osal/Endiness.h"
 #include <cstring>
-#include <iostream>
 
 /**
  * Helper class to convert variables or bitstreams between machine
diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp
index 1c11afd4..53b8c3d5 100644
--- a/serialize/SerialBufferAdapter.cpp
+++ b/serialize/SerialBufferAdapter.cpp
@@ -95,8 +95,10 @@ ReturnValue_t SerialBufferAdapter<count_t>::deSerialize(const uint8_t** buffer,
 template<typename count_t>
 uint8_t * SerialBufferAdapter<count_t>::getBuffer() {
 	if(buffer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "Wrong access function for stored type !"
 				 " Use getConstBuffer()." << std::endl;
+#endif
 		return nullptr;
 	}
 	return buffer;
@@ -105,8 +107,10 @@ uint8_t * SerialBufferAdapter<count_t>::getBuffer() {
 template<typename count_t>
 const uint8_t * SerialBufferAdapter<count_t>::getConstBuffer() {
 	if(constBuffer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "SerialBufferAdapter::getConstBuffer:"
 				" Buffers are unitialized!" << std::endl;
+#endif
 		return nullptr;
 	}
 	return constBuffer;
diff --git a/serviceinterface/CMakeLists.txt b/serviceinterface/CMakeLists.txt
index d84adbeb..84c79177 100644
--- a/serviceinterface/CMakeLists.txt
+++ b/serviceinterface/CMakeLists.txt
@@ -1,5 +1,5 @@
-target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		ServiceInterfaceStream.cpp
-		ServiceInterfaceBuffer.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+	ServiceInterfaceStream.cpp
+	ServiceInterfaceBuffer.cpp
+	ServiceInterfacePrinter.cpp
 )
\ No newline at end of file
diff --git a/serviceinterface/ServiceInterface.h b/serviceinterface/ServiceInterface.h
new file mode 100644
index 00000000..0c7c4383
--- /dev/null
+++ b/serviceinterface/ServiceInterface.h
@@ -0,0 +1,12 @@
+#ifndef FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_
+#define FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_
+
+#include <FSFWConfig.h>
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#else
+#include <fsfw/serviceinterface/ServiceInterfacePrinter.h>
+#endif
+
+#endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACE_H_ */
diff --git a/serviceinterface/ServiceInterfaceBuffer.cpp b/serviceinterface/ServiceInterfaceBuffer.cpp
index 68fc4dec..5846d233 100644
--- a/serviceinterface/ServiceInterfaceBuffer.cpp
+++ b/serviceinterface/ServiceInterfaceBuffer.cpp
@@ -1,8 +1,17 @@
-#include "../timemanager/Clock.h"
 #include "ServiceInterfaceBuffer.h"
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
+#include "../timemanager/Clock.h"
+
+#include "serviceInterfaceDefintions.h"
 #include <cstring>
 #include <inttypes.h>
 
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+#include "Windows.h"
+#endif
+
 // to be implemented by bsp
 extern "C" void printChar(const char*, bool errStream);
 
@@ -17,6 +26,34 @@ ServiceInterfaceBuffer::ServiceInterfaceBuffer(std::string setMessage,
 		// Set pointers if the stream is buffered.
 		setp( buf, buf + BUF_SIZE );
 	}
+
+#if FSFW_COLORED_OUTPUT == 1
+	if(setMessage.find("DEBUG") != std::string::npos) {
+		colorPrefix = fsfw::ANSI_COLOR_MAGENTA;
+	}
+	else if(setMessage.find("INFO") != std::string::npos) {
+		colorPrefix = fsfw::ANSI_COLOR_GREEN;
+	}
+	else if(setMessage.find("WARNING") != std::string::npos) {
+		colorPrefix = fsfw::ANSI_COLOR_YELLOW;
+	}
+	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);
+	DWORD dwMode = 0;
+	GetConsoleMode(hOut, &dwMode);
+	dwMode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING;
+	SetConsoleMode(hOut, dwMode);
+#endif
+
+#endif
+
 	preamble.reserve(MAX_PREAMBLE_SIZE);
 	preamble.resize(MAX_PREAMBLE_SIZE);
 }
@@ -102,6 +139,12 @@ std::string* ServiceInterfaceBuffer::getPreamble(size_t * preambleSize) {
 		currentSize += 1;
 		parsePosition += 1;
 	}
+
+#if FSFW_COLORED_OUTPUT == 1
+	currentSize += sprintf(parsePosition, "%s", colorPrefix.c_str());
+	parsePosition += colorPrefix.size();
+#endif
+
 	int32_t charCount = sprintf(parsePosition,
 			"%s: | %02" SCNu32 ":%02" SCNu32 ":%02" SCNu32 ".%03" SCNu32 " | ",
 					this->logMessage.c_str(), loggerTime.hour,
@@ -215,3 +258,5 @@ void ServiceInterfaceBuffer::initSocket() {
 }
 
 #endif //ML505
+
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
diff --git a/serviceinterface/ServiceInterfaceBuffer.h b/serviceinterface/ServiceInterfaceBuffer.h
index c5d5b258..b1a50848 100644
--- a/serviceinterface/ServiceInterfaceBuffer.h
+++ b/serviceinterface/ServiceInterfaceBuffer.h
@@ -2,6 +2,10 @@
 #define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_
 
 #include "../returnvalues/HasReturnvaluesIF.h"
+#include <FSFWConfig.h>
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
 #include <iostream>
 #include <sstream>
 #include <iomanip>
@@ -41,6 +45,11 @@ private:
 	//! For additional message information
 	std::string logMessage;
 	std::string preamble;
+
+#if FSFW_COLORED_OUTPUT == 1
+	std::string colorPrefix;
+#endif
+
 	// For EOF detection
 	typedef std::char_traits<char> Traits;
 
@@ -54,7 +63,7 @@ private:
 	bool errStream;
 
 	//! Needed for buffered mode.
-	static size_t const BUF_SIZE = 128;
+	static size_t const BUF_SIZE = fsfwconfig::FSFW_PRINT_BUFFER_SIZE;
 	char buf[BUF_SIZE];
 
 	//! In this function, the characters are parsed.
@@ -141,5 +150,6 @@ private:
 };
 #endif //ML505
 
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 
 #endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACEBUFFER_H_ */
diff --git a/serviceinterface/ServiceInterfacePrinter.cpp b/serviceinterface/ServiceInterfacePrinter.cpp
new file mode 100644
index 00000000..15e738a8
--- /dev/null
+++ b/serviceinterface/ServiceInterfacePrinter.cpp
@@ -0,0 +1,130 @@
+#include <FSFWConfig.h>
+#include "ServiceInterfacePrinter.h"
+#include "serviceInterfaceDefintions.h"
+#include "../timemanager/Clock.h"
+
+#include <cstdarg>
+#include <cstdint>
+
+fsfw::PrintLevel printLevel = fsfw::PrintLevel::DEBUG;
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+bool consoleInitialized = false;
+#endif /* defined(WIN32) && FSFW_COLORED_OUTPUT == 1 */
+
+
+#if FSFW_DISABLE_PRINTOUT == 0
+uint8_t printBuffer[fsfwconfig::FSFW_PRINT_BUFFER_SIZE];
+
+void fsfwPrint(fsfw::PrintLevel printType, const char* fmt, va_list arg) {
+
+#if defined(WIN32) && FSFW_COLORED_OUTPUT == 1
+	if(not consoleInitialized) {
+		HANDLE hOut = GetStdHandle(STD_OUTPUT_HANDLE);
+		DWORD dwMode = 0;
+		GetConsoleMode(hOut, &dwMode);
+		dwMode |= ENABLE_VIRTUAL_TERMINAL_PROCESSING;
+		SetConsoleMode(hOut, dwMode);
+	}
+	consoleInitialized = true;
+#endif
+
+    size_t len = 0;
+    char* bufferPosition = reinterpret_cast<char*>(printBuffer);
+
+    /* Check logger level */
+    if(printType == fsfw::PrintLevel::NONE or printType > printLevel) {
+        return;
+    }
+
+    /* Log message to terminal */
+
+#if FSFW_COLORED_OUTPUT == 1
+    if(printType == fsfw::PrintLevel::INFO) {
+    	len += sprintf(bufferPosition, fsfw::ANSI_COLOR_GREEN);
+    }
+    else if(printType == fsfw::PrintLevel::DEBUG) {
+    	len += sprintf(bufferPosition, fsfw::ANSI_COLOR_MAGENTA);
+    }
+    else if(printType == fsfw::PrintLevel::WARNING) {
+    	len += sprintf(bufferPosition, fsfw::ANSI_COLOR_YELLOW);
+    }
+    else if(printType == fsfw::PrintLevel::ERROR_TYPE) {
+    	len += sprintf(bufferPosition, fsfw::ANSI_COLOR_RED);
+    }
+#endif
+
+   if (printType == fsfw::PrintLevel::INFO) {
+       len += sprintf(bufferPosition + len, "INFO: ");
+   }
+   if(printType == fsfw::PrintLevel::DEBUG) {
+	   len += sprintf(bufferPosition + len, "DEBUG: ");
+   }
+   if(printType == fsfw::PrintLevel::WARNING) {
+	   len += sprintf(bufferPosition + len, "WARNING: ");
+   }
+
+   if(printType == fsfw::PrintLevel::ERROR_TYPE) {
+	   len += sprintf(bufferPosition + len, "ERROR: ");
+   }
+
+    Clock::TimeOfDay_t now;
+    Clock::getDateAndTime(&now);
+    /*
+     * Log current time to terminal if desired.
+     */
+    len += sprintf(bufferPosition + len, "| %lu:%02lu:%02lu.%03lu | ",
+    		(unsigned long) now.hour,
+			(unsigned long) now.minute,
+			(unsigned long) now.second,
+			(unsigned long) now.usecond /1000);
+
+    len += vsnprintf(bufferPosition + len, sizeof(printBuffer) - len, fmt, arg);
+
+    printf("%s", printBuffer);
+}
+
+
+void fsfw::printInfo(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(fsfw::PrintLevel::INFO, fmt, args);
+    va_end(args);
+}
+
+void fsfw::printWarning(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(fsfw::PrintLevel::WARNING, fmt, args);
+    va_end(args);
+}
+
+void fsfw::printDebug(const char *fmt, ...) {
+    va_list args;
+    va_start(args, fmt);
+    fsfwPrint(fsfw::PrintLevel::DEBUG, fmt, args);
+    va_end(args);
+}
+
+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
new file mode 100644
index 00000000..a60b6332
--- /dev/null
+++ b/serviceinterface/ServiceInterfacePrinter.h
@@ -0,0 +1,32 @@
+#if FSFW_DISABLE_PRINTOUT == 0
+#include <stdio.h>
+#endif
+
+namespace fsfw {
+
+enum class PrintLevel {
+	NONE = 0,
+	//! Strange error when using just ERROR..
+	ERROR_TYPE = 1,
+	WARNING = 2,
+	INFO = 3,
+	DEBUG = 4
+};
+
+void setPrintLevel(PrintLevel printLevel);
+PrintLevel getPrintLevel();
+
+/**
+ * These functions can be used like the C stdio printf and forward the
+ * supplied formatted string arguments to a printf function.
+ * They prepend the string with a color (if enabled), a log preamble and
+ * a timestamp.
+ * @param fmt Formatted string
+ */
+void printInfo(const char *fmt, ...);
+void printWarning(const char* fmt, ...);
+void printDebug(const char* fmt, ...);
+void printError(const char* fmt, ...);
+
+}
+
diff --git a/serviceinterface/ServiceInterfaceStream.cpp b/serviceinterface/ServiceInterfaceStream.cpp
index 5b7b9f00..59526536 100644
--- a/serviceinterface/ServiceInterfaceStream.cpp
+++ b/serviceinterface/ServiceInterfaceStream.cpp
@@ -1,5 +1,7 @@
 #include "ServiceInterfaceStream.h"
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
 ServiceInterfaceStream::ServiceInterfaceStream(std::string setMessage,
 		bool addCrToPreamble, bool buffered, bool errStream, uint16_t port) :
 		std::ostream(&streambuf),
@@ -13,20 +15,5 @@ std::string* ServiceInterfaceStream::getPreamble() {
 	return streambuf.getPreamble();
 }
 
-void ServiceInterfaceStream::print(std::string error,
-		bool withPreamble, bool withNewline, bool flush) {
-	if(not streambuf.isBuffered() and withPreamble) {
-		*this << getPreamble() << error;
-	}
-	else {
-		*this << error;
-	}
+#endif
 
-	if(withNewline) {
-		*this << "\n";
-	}
-	// if mode is non-buffered, no need to flush.
-	if(flush and streambuf.isBuffered()) {
-		this->flush();
-	}
-}
diff --git a/serviceinterface/ServiceInterfaceStream.h b/serviceinterface/ServiceInterfaceStream.h
index 76fa1bf2..f3cb2cd0 100644
--- a/serviceinterface/ServiceInterfaceStream.h
+++ b/serviceinterface/ServiceInterfaceStream.h
@@ -2,6 +2,10 @@
 #define FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_
 
 #include "ServiceInterfaceBuffer.h"
+#include <FSFWConfig.h>
+
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+
 #include <iostream>
 #include <cstdio>
 
@@ -35,13 +39,6 @@ public:
 	 */
 	std::string* getPreamble();
 
-	/**
-	 * This prints an error with a preamble. Useful if using the unbuffered
-	 * mode. Flushes in default mode (prints immediately).
-	 */
-	void print(std::string error, bool withPreamble = true,
-			bool withNewline = true, bool flush = true);
-
 protected:
 	ServiceInterfaceBuffer streambuf;
 };
@@ -55,4 +52,6 @@ extern ServiceInterfaceStream warning;
 extern ServiceInterfaceStream error;
 }
 
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+
 #endif /* FRAMEWORK_SERVICEINTERFACE_SERVICEINTERFACESTREAM_H_ */
diff --git a/serviceinterface/serviceInterfaceDefintions.h b/serviceinterface/serviceInterfaceDefintions.h
new file mode 100644
index 00000000..684c7366
--- /dev/null
+++ b/serviceinterface/serviceInterfaceDefintions.h
@@ -0,0 +1,16 @@
+#ifndef FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_
+#define FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_
+
+namespace fsfw {
+
+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";
+static const char* const ANSI_COLOR_BLUE = "\x1b[34m";
+static const char* const ANSI_COLOR_MAGENTA = "\x1b[35m";
+static const char* const ANSI_COLOR_CYAN = "\x1b[36m";
+static const char* const ANSI_COLOR_RESET = "\x1b[0m";
+
+}
+
+#endif /* FSFW_SERVICEINTERFACE_SERVICEINTERFACEDEFINTIONS_H_ */
diff --git a/storagemanager/ConstStorageAccessor.cpp b/storagemanager/ConstStorageAccessor.cpp
index 842f1ce8..aab84862 100644
--- a/storagemanager/ConstStorageAccessor.cpp
+++ b/storagemanager/ConstStorageAccessor.cpp
@@ -4,6 +4,8 @@
 #include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../globalfunctions/arrayprinter.h"
 
+#include <algorithm>
+
 ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId):
 		storeId(storeId) {}
 
@@ -46,7 +48,9 @@ const uint8_t* ConstStorageAccessor::data() const {
 
 size_t ConstStorageAccessor::size() const {
 	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 	}
 	return size_;
 }
@@ -54,12 +58,16 @@ size_t ConstStorageAccessor::size() const {
 ReturnValue_t ConstStorageAccessor::getDataCopy(uint8_t *pointer,
 		size_t maxSize) {
 	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(size_ > maxSize) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "StorageAccessor: Supplied buffer not large enough"
 				<< std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	std::copy(constDataPointer, constDataPointer + size_, pointer);
@@ -76,7 +84,9 @@ store_address_t ConstStorageAccessor::getId() const {
 
 void ConstStorageAccessor::print() const {
 	if(internalState == AccessState::UNINIT or constDataPointer == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 		return;
 	}
 	arrayprinter::print(constDataPointer, size_);
diff --git a/storagemanager/LocalPool.cpp b/storagemanager/LocalPool.cpp
index 74e362d4..cc7c9266 100644
--- a/storagemanager/LocalPool.cpp
+++ b/storagemanager/LocalPool.cpp
@@ -8,8 +8,10 @@ LocalPool::LocalPool(object_id_t setObjectId, const LocalPoolConfig& poolConfig,
 		NUMBER_OF_POOLS(poolConfig.size()),
 		spillsToHigherPools(spillsToHigherPools) {
 	if(NUMBER_OF_POOLS == 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "LocalPool::LocalPool: Passed pool configuration is "
 				<< " invalid!" << std::endl;
+#endif
 	}
 	max_pools_t index = 0;
 	for (const auto& currentPoolConfig: poolConfig) {
@@ -118,8 +120,10 @@ ReturnValue_t LocalPool::modifyData(store_address_t storeId,
 
 ReturnValue_t LocalPool::deleteData(store_address_t storeId) {
 #if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::debug << "Delete: Pool: " << std::dec << storeId.poolIndex
               << " Index: " << storeId.packetIndex << std::endl;
+#endif
 
 #endif
     ReturnValue_t status = RETURN_OK;
@@ -134,8 +138,10 @@ ReturnValue_t LocalPool::deleteData(store_address_t storeId) {
     }
     else {
         //pool_index or packet_index is too large
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPool::deleteData: Illegal store ID, no deletion!"
         		<< std::endl;
+#endif
         status = ILLEGAL_STORAGE_ID;
     }
     return status;
@@ -158,8 +164,10 @@ ReturnValue_t LocalPool::deleteData(uint8_t *ptr, size_t size,
             result = deleteData(localId);
 #if FSFW_VERBOSE_PRINTOUT == 2
             if (deltaAddress % elementSizes[n] != 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
                 sif::error << "LocalPool::deleteData: Address not aligned!"
                         << std::endl;
+#endif
             }
 #endif
             break;
@@ -186,8 +194,10 @@ ReturnValue_t LocalPool::initialize() {
     //Check if any pool size is large than the maximum allowed.
     for (uint8_t count = 0; count < NUMBER_OF_POOLS; count++) {
         if (elementSizes[count] >= STORAGE_FREE) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::error << "LocalPool::initialize: Pool is too large! "
                     "Max. allowed size is: " << (STORAGE_FREE - 1) << std::endl;
+#endif
             return StorageManagerIF::POOL_TOO_LARGE;
         }
     }
@@ -209,8 +219,10 @@ ReturnValue_t LocalPool::reserveSpace(const size_t size,
         store_address_t *storeId, bool ignoreFault) {
     ReturnValue_t status = getPoolIndex(size, &storeId->poolIndex);
     if (status != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "LocalPool( " << std::hex << getObjectId() << std::dec
                 << " )::reserveSpace: Packet too large." << std::endl;
+#endif
         return status;
     }
     status = findEmpty(storeId->poolIndex, &storeId->packetIndex);
@@ -224,9 +236,11 @@ ReturnValue_t LocalPool::reserveSpace(const size_t size,
     }
     if (status == RETURN_OK) {
 #if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "Reserve: Pool: " << std::dec
                 << storeId->poolIndex << " Index: " << storeId->packetIndex
                 << std::endl;
+#endif
 #endif
         sizeLists[storeId->poolIndex][storeId->packetIndex] = size;
     }
@@ -266,8 +280,10 @@ ReturnValue_t LocalPool::getPoolIndex(size_t packetSize, uint16_t *poolIndex,
         uint16_t startAtIndex) {
     for (uint16_t n = startAtIndex; n < NUMBER_OF_POOLS; n++) {
 #if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "LocalPool " << getObjectId() << "::getPoolIndex: Pool: "
              << n << ", Element Size: " << elementSizes[n] << std::endl;
+#endif
 #endif
         if (elementSizes[n] >= packetSize) {
             *poolIndex = n;
diff --git a/storagemanager/PoolManager.cpp b/storagemanager/PoolManager.cpp
index 3b7b549b..4e8014fd 100644
--- a/storagemanager/PoolManager.cpp
+++ b/storagemanager/PoolManager.cpp
@@ -26,9 +26,11 @@ ReturnValue_t PoolManager::reserveSpace(const size_t size,
 ReturnValue_t PoolManager::deleteData(
         store_address_t storeId) {
 #if FSFW_VERBOSE_PRINTOUT == 2
+#if FSFW_CPP_OSTREAM_ENABLED == 1
      sif::debug << "PoolManager( " << translateObject(getObjectId()) <<
            " )::deleteData from store " << storeId.poolIndex <<
            ". id is "<< storeId.packetIndex << std::endl;
+#endif
 #endif
     MutexHelper mutexHelper(mutex, MutexIF::TimeoutType::WAITING,
             mutexTimeoutMs);
diff --git a/storagemanager/StorageAccessor.cpp b/storagemanager/StorageAccessor.cpp
index 9c2f936a..a7b4fae4 100644
--- a/storagemanager/StorageAccessor.cpp
+++ b/storagemanager/StorageAccessor.cpp
@@ -2,6 +2,8 @@
 #include "StorageManagerIF.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
 
+#include <algorithm>
+
 StorageAccessor::StorageAccessor(store_address_t storeId):
 		ConstStorageAccessor(storeId) {
 }
@@ -26,12 +28,16 @@ StorageAccessor::StorageAccessor(StorageAccessor&& other):
 
 ReturnValue_t StorageAccessor::getDataCopy(uint8_t *pointer, size_t maxSize) {
 	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(size_ > maxSize) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "StorageAccessor: Supplied buffer not large "
 				"enough" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	std::copy(dataPointer, dataPointer + size_, pointer);
@@ -40,7 +46,9 @@ ReturnValue_t StorageAccessor::getDataCopy(uint8_t *pointer, size_t maxSize) {
 
 uint8_t* StorageAccessor::data() {
 	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 	}
 	return dataPointer;
 }
@@ -48,12 +56,16 @@ uint8_t* StorageAccessor::data() {
 ReturnValue_t StorageAccessor::write(uint8_t *data, size_t size,
 		uint16_t offset) {
 	if(internalState == AccessState::UNINIT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(offset + size > size_) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "StorageAccessor: Data too large for pool "
 				"entry!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	std::copy(data, data + size, dataPointer + offset);
diff --git a/subsystem/SubsystemBase.cpp b/subsystem/SubsystemBase.cpp
index f60c8847..bcfa4b0e 100644
--- a/subsystem/SubsystemBase.cpp
+++ b/subsystem/SubsystemBase.cpp
@@ -86,8 +86,10 @@ void SubsystemBase::executeTable(HybridIterator<ModeListEntry> tableIter,
 		object_id_t object = tableIter.value->getObject();
 		if ((iter = childrenMap.find(object)) == childrenMap.end()) {
 			//illegal table entry, should only happen due to misconfigured mode table
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::debug << std::hex << getObjectId() << ": invalid mode table entry"
 					<< std::endl;
+#endif
 			continue;
 		}
 
diff --git a/tasks/FixedSlotSequence.cpp b/tasks/FixedSlotSequence.cpp
index e5db4301..54b6ae6d 100644
--- a/tasks/FixedSlotSequence.cpp
+++ b/tasks/FixedSlotSequence.cpp
@@ -91,8 +91,10 @@ void FixedSlotSequence::addSlot(object_id_t componentId, uint32_t slotTimeMs,
 
 ReturnValue_t FixedSlotSequence::checkSequence() const {
 	if(slotList.empty()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "FixedSlotSequence::checkSequence:"
 				<< " Slot list is empty!" << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 
@@ -100,8 +102,10 @@ ReturnValue_t FixedSlotSequence::checkSequence() const {
 		ReturnValue_t result = customCheckFunction(slotList);
 		if(result != HasReturnvaluesIF::RETURN_OK) {
 			// Continue for now but print error output.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "FixedSlotSequence::checkSequence:"
 					<< " Custom check failed!" << std::endl;
+#endif
 		}
 	}
 
@@ -112,21 +116,27 @@ ReturnValue_t FixedSlotSequence::checkSequence() const {
 			errorCount++;
 		}
 		else if (slot.pollingTimeMs < time) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "FixedSlotSequence::checkSequence: Time: "
 					<< slot.pollingTimeMs << " is smaller than previous with "
 					<< time << std::endl;
+#endif
 			errorCount++;
 		}
 		else {
 			// All ok, print slot.
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			//sif::info << "Current slot polling time: " << std::endl;
 			//sif::info << std::dec << slotIt->pollingTimeMs << std::endl;
+#endif
 		}
 		time = slot.pollingTimeMs;
 
 	}
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::info << "Number of elements in slot list: "
 	//	   << slotList.size() << std::endl;
+#endif
 	if (errorCount > 0) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
@@ -149,8 +159,10 @@ ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const {
         }
     }
     if (count > 0) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::error << "FixedSlotSequence::intializeSequenceAfterTaskCreation:"
                 "Counted " << count << " failed initializations!"  << std::endl;
+#endif
         return HasReturnvaluesIF::RETURN_FAILED;
     }
     return HasReturnvaluesIF::RETURN_OK;
diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp
index ddd63308..b795854f 100644
--- a/tcdistribution/CCSDSDistributor.cpp
+++ b/tcdistribution/CCSDSDistributor.cpp
@@ -11,21 +11,27 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 CCSDSDistributor::~CCSDSDistributor() {}
 
 TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //	sif::debug << "CCSDSDistributor::selectDestination received: " <<
 //			this->currentMessage.getStorageId().pool_index << ", " <<
 //			this->currentMessage.getStorageId().packet_index << std::endl;
+#endif
 	const uint8_t* packet = nullptr;
 	size_t size = 0;
 	ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
 			&packet, &size );
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CCSDSDistributor::selectDestination: Getting data from"
 				" store failed!" << std::endl;
+#endif
 	}
 	SpacePacketBase currentPacket(packet);
 
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //	sif:: info << "CCSDSDistributor::selectDestination has packet with APID "
 //			<< std::hex << currentPacket.getAPID() << std::dec << std::endl;
+#endif
 	TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
 	if ( position != this->queueMap.end() ) {
 		return position;
@@ -70,8 +76,10 @@ ReturnValue_t CCSDSDistributor::initialize() {
 	ReturnValue_t status = this->TcDistributor::initialize();
 	this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
 	if (this->tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CCSDSDistributor::initialize: Could not initialize"
 	            " TC store!" << std::endl;
+#endif
 	    status = RETURN_FAILED;
 	}
 	return status;
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 8e5b94cc..00fd9029 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -13,9 +13,11 @@ PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
 PUSDistributor::~PUSDistributor() {}
 
 PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     // sif:: debug << "PUSDistributor::handlePacket received: "
     //          << this->current_packet_id.store_index << ", "
     //          << this->current_packet_id.packet_index << std::endl;
+#endif
     TcMqMapIter queueMapIt = this->queueMap.end();
     if(this->currentPacket == nullptr) {
         return queueMapIt;
@@ -25,9 +27,11 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
         tcStatus = checker.checkPacket(currentPacket);
 #ifdef DEBUG
         if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
             sif::debug << "PUSDistributor::handlePacket: Packet format "
                     << "invalid, code "<< static_cast<int>(tcStatus)
                     << std::endl;
+#endif
         }
 #endif
         uint32_t queue_id = currentPacket->getService();
@@ -40,8 +44,10 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
     if (queueMapIt == this->queueMap.end()) {
         tcStatus = DESTINATION_NOT_FOUND;
 #ifdef DEBUG
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "PUSDistributor::handlePacket: Destination not found, "
                 << "code "<< static_cast<int>(tcStatus) << std::endl;
+#endif
 #endif
     }
 
@@ -57,12 +63,16 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
 ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
 	uint16_t serviceId = service->getIdentifier();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	// sif::info << "Service ID: " << (int)serviceId << std::endl;
+#endif
 	MessageQueueId_t queue = service->getRequestQueue();
 	auto returnPair = queueMap.emplace(serviceId, queue);
 	if (not returnPair.second) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PUSDistributor::registerService: Service ID already"
 				" exists in map." << std::endl;
+#endif
 		return SERVICE_ID_ALREADY_EXISTS;
 	}
 	return HasReturnvaluesIF::RETURN_OK;
@@ -104,9 +114,11 @@ ReturnValue_t PUSDistributor::initialize() {
 	CCSDSDistributorIF* ccsdsDistributor =
 			objectManager->get<CCSDSDistributorIF>(packetSource);
 	if (ccsdsDistributor == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PUSDistributor::initialize: Packet source invalid."
 		        << " Make sure it exists and implements CCSDSDistributorIF!"
 		        << std::endl;
+#endif
 	    return RETURN_FAILED;
 	}
 	return ccsdsDistributor->registerApplication(this);
diff --git a/tcdistribution/TcDistributor.cpp b/tcdistribution/TcDistributor.cpp
index 06e1817f..df069556 100644
--- a/tcdistribution/TcDistributor.cpp
+++ b/tcdistribution/TcDistributor.cpp
@@ -39,6 +39,7 @@ ReturnValue_t TcDistributor::handlePacket() {
 }
 
 void TcDistributor::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::debug << "Distributor content is: " << std::endl
 	        << "ID\t| Message Queue ID" << std::endl;
 	sif::debug << std::setfill('0') << std::setw(8) << std::hex;
@@ -47,7 +48,7 @@ void TcDistributor::print() {
 		         << std::endl;
 	}
 	sif::debug << std::setfill(' ') << std::dec;
-
+#endif
 }
 
 ReturnValue_t TcDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
diff --git a/timemanager/Stopwatch.cpp b/timemanager/Stopwatch.cpp
index 302e2ac0..f79d2eeb 100644
--- a/timemanager/Stopwatch.cpp
+++ b/timemanager/Stopwatch.cpp
@@ -28,13 +28,17 @@ double Stopwatch::stopSeconds() {
 
 void Stopwatch::display() {
     if(displayMode == StopwatchDisplayMode::MILLIS) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info << "Stopwatch: Operation took " << (elapsedTime.tv_sec * 1000 +
                 elapsedTime.tv_usec / 1000) << " milliseconds" << std::endl;
+#endif
     }
     else if(displayMode == StopwatchDisplayMode::SECONDS) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::info <<"Stopwatch: Operation took "  << std::setprecision(3)
              << std::fixed << timevalOperations::toDouble(elapsedTime)
              << " seconds" << std::endl;
+#endif
     }
 }
 
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index eaa8416a..ca3e2a99 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -63,7 +63,9 @@ uint8_t TcPacketBase::getPusVersionNumber() {
 }
 
 void TcPacketBase::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::debug << "TcPacketBase::print: " << std::endl;
+#endif
 	arrayprinter::print(getWholeData(), getFullSize());
 }
 
diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp
index fffc86ae..f320386c 100644
--- a/tmtcpacket/pus/TcPacketStored.cpp
+++ b/tmtcpacket/pus/TcPacketStored.cpp
@@ -23,8 +23,10 @@ TcPacketStored::TcPacketStored(uint16_t apid,  uint8_t service,
 	ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress,
 			(TC_PACKET_MIN_SIZE + size), &pData);
 	if (returnValue != this->store->RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "TcPacketStored: Could not get free element from store!"
 				<< std::endl;
+#endif
 		return;
 	}
 	this->setData(pData);
@@ -39,7 +41,9 @@ ReturnValue_t TcPacketStored::getData(const uint8_t ** dataPtr,
 		size_t* dataSize) {
 	auto result = this->store->getData(storeAddress, dataPtr, dataSize);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "TcPacketStored: Could not get data!" << std::endl;
+#endif
 	}
 	return result;
 }
@@ -61,8 +65,10 @@ bool TcPacketStored::checkAndSetStore() {
 	if (this->store == nullptr) {
 		this->store = objectManager->get<StorageManagerIF>(objects::TC_STORE);
 		if (this->store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TcPacketStored::TcPacketStored: TC Store not found!"
 					<< std::endl;
+#endif
 			return false;
 		}
 	}
diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp
index 2c8bfd63..c8e4b430 100644
--- a/tmtcpacket/pus/TmPacketBase.cpp
+++ b/tmtcpacket/pus/TmPacketBase.cpp
@@ -57,7 +57,9 @@ void TmPacketBase::setData(const uint8_t* p_Data) {
 }
 
 void TmPacketBase::print() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	 sif::debug << "TmPacketBase::print: " << std::endl;
+#endif
 	 arrayprinter::print(getWholeData(), getFullSize());
 }
 
@@ -65,8 +67,10 @@ bool TmPacketBase::checkAndSetStamper() {
 	if (timeStamper == NULL) {
 		timeStamper = objectManager->get<TimeStamperIF>(timeStamperId);
 		if (timeStamper == NULL) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TmPacketBase::checkAndSetStamper: Stamper not found!"
 					<< std::endl;
+#endif
 			return false;
 		}
 	}
diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp
index 0fb789aa..0fd2a4a0 100644
--- a/tmtcpacket/pus/TmPacketStored.cpp
+++ b/tmtcpacket/pus/TmPacketStored.cpp
@@ -105,8 +105,10 @@ bool TmPacketStored::checkAndSetStore() {
 	if (store == nullptr) {
 		store = objectManager->get<StorageManagerIF>(objects::TM_STORE);
 		if (store == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TmPacketStored::TmPacketStored: TM Store not found!"
 					<< std::endl;
+#endif
 			return false;
 		}
 	}
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index ed765ecc..259f2ccb 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -75,8 +75,10 @@ ReturnValue_t CommandingServiceBase::initialize() {
 			packetSource);
 
 	if (packetForwarding == nullptr or distributor == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CommandingServiceBase::intialize: Packet source or "
 	            "packet destination invalid!" << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -88,8 +90,10 @@ ReturnValue_t CommandingServiceBase::initialize() {
 	TCStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
 
 	if (IPCStore == nullptr or TCStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CommandingServiceBase::intialize: IPC store or TC store "
 	                    "not initialized yet!" << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
diff --git a/tmtcservices/PusServiceBase.cpp b/tmtcservices/PusServiceBase.cpp
index cb5a1247..0a5cb202 100644
--- a/tmtcservices/PusServiceBase.cpp
+++ b/tmtcservices/PusServiceBase.cpp
@@ -25,9 +25,11 @@ ReturnValue_t PusServiceBase::performOperation(uint8_t opCode) {
 	handleRequestQueue();
 	ReturnValue_t result = this->performService();
 	if (result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PusService " << (uint16_t) this->serviceId
 				<< ": performService returned with " << (int16_t) result
 				<< std::endl;
+#endif
 		return RETURN_FAILED;
 	}
 	return RETURN_OK;
@@ -43,11 +45,13 @@ void PusServiceBase::handleRequestQueue() {
 	for (uint8_t count = 0; count < PUS_SERVICE_MAX_RECEPTION; count++) {
 		ReturnValue_t status = this->requestQueue->receiveMessage(&message);
 //		if(status != MessageQueueIF::EMPTY) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //			sif::debug << "PusServiceBase::performOperation: Receiving from "
 //					<< "MQ ID: " << std::hex << "0x" << std::setw(8)
 //					<< std::setfill('0') << this->requestQueue->getId()
 //					<< std::dec << " returned: " << status << std::setfill(' ')
 //					<<  std::endl;
+#endif
 //		}
 
 		if (status == RETURN_OK) {
@@ -79,9 +83,11 @@ void PusServiceBase::handleRequestQueue() {
 			break;
 		}
 		else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "PusServiceBase::performOperation: Service "
 					<< this->serviceId << ": Error receiving packet. Code: "
 					<< std::hex << status << std::dec << std::endl;
+#endif
 		}
 	}
 }
@@ -104,10 +110,12 @@ ReturnValue_t PusServiceBase::initialize() {
 	PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
 			packetSource);
 	if (destService == nullptr or distributor == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "PusServiceBase::PusServiceBase: Service "
 				<< this->serviceId << ": Configuration error. Make sure "
 				<<	"packetSource and packetDestination are defined correctly"
 				<< std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	this->requestQueue->setDefaultDestination(
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index b4e9967b..2513071a 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -23,9 +23,11 @@ ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(
 		return RETURN_OK;
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::warning << "TmTcBridge::setNumberOfSentPacketsPerCycle: Number of "
 	             << "packets sent per cycle exceeds limits. "
 	             << "Keeping default value." << std::endl;
+#endif
 		return RETURN_FAILED;
 	}
 }
@@ -37,9 +39,11 @@ ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(
         return RETURN_OK;
     }
     else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::warning << "TmTcBridge::setMaxNumberOfPacketsStored: Number of "
                 << "packets stored exceeds limits. "
                 << "Keeping default value." << std::endl;
+#endif
         return RETURN_FAILED;
     }
 }
@@ -47,21 +51,27 @@ ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(
 ReturnValue_t TmTcBridge::initialize() {
 	tcStore = objectManager->get<StorageManagerIF>(tcStoreId);
 	if (tcStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TC store invalid. Make sure"
 				"it is created and set up properly." << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	tmStore = objectManager->get<StorageManagerIF>(tmStoreId);
 	if (tmStore == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TM store invalid. Make sure"
 				"it is created and set up properly." << std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 	AcceptsTelecommandsIF* tcDistributor =
 			objectManager->get<AcceptsTelecommandsIF>(tcDestination);
 	if (tcDistributor == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::initialize: TC Distributor invalid"
 				<< std::endl;
+#endif
 		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
@@ -75,13 +85,17 @@ ReturnValue_t TmTcBridge::performOperation(uint8_t operationCode) {
 	ReturnValue_t result;
 	result = handleTc();
 	if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::debug << "TmTcBridge::performOperation: "
 		        << "Error handling TCs" << std::endl;
+#endif
 	}
 	result = handleTm();
 	if (result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::debug << "TmTcBridge::performOperation: "
 	                    << "Error handling TMs" << std::endl;
+#endif
 	}
 	return result;
 }
@@ -94,9 +108,11 @@ ReturnValue_t TmTcBridge::handleTm() {
     ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
 	ReturnValue_t result = handleTmQueue();
 	if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "TmTcBridge::handleTm: Error handling TM queue with "
 		       << "error code 0x" << std::hex << result  << std::dec
 			   << "!" << std::endl;
+#endif
 		status = result;
 	}
 
@@ -104,8 +120,10 @@ ReturnValue_t TmTcBridge::handleTm() {
 	        (packetSentCounter < sentPacketsPerCycle)) {
 	    result = handleStoredTm();
 	    if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	        sif::error << "TmTcBridge::handleTm: Error handling stored TMs!"
 	                << std::endl;
+#endif
 	        status = result;
 	    }
 	}
@@ -122,7 +140,9 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
 		 result == HasReturnvaluesIF::RETURN_OK;
 		 result = tmTcReceptionQueue->receiveMessage(&message))
 	{
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    //sif::info << (int) packetSentCounter << std::endl;
+#endif
 		if(communicationLinkUp == false or
 		        packetSentCounter >= sentPacketsPerCycle) {
 			storeDownlinkData(&message);
@@ -151,8 +171,10 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
 	store_address_t storeId = 0;
 
 	if(tmFifo->full()) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::debug << "TmTcBridge::storeDownlinkData: TM downlink max. number "
 	                    << "of stored packet IDs reached! " << std::endl;
+#endif
 	    if(overwriteOld) {
 	        tmFifo->retrieve(&storeId);
 	        tmStore->deleteData(storeId);
@@ -171,8 +193,10 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
 ReturnValue_t TmTcBridge::handleStoredTm() {
     ReturnValue_t status = RETURN_OK;
 	while(not tmFifo->empty() and packetSentCounter < sentPacketsPerCycle) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		//sif::info << "TMTC Bridge: Sending stored TM data. There are "
 		//     << (int) tmFifo->size() << " left to send\r\n" << std::flush;
+#endif
 
 		store_address_t storeId;
 		const uint8_t* data = nullptr;
@@ -185,8 +209,10 @@ ReturnValue_t TmTcBridge::handleStoredTm() {
 
 		result = sendTm(data,size);
 		if(result != RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 			sif::error << "TMTC Bridge: Could not send stored downlink data"
 			      << std::endl;
+#endif
 			status = result;
 		}
 		packetSentCounter ++;
@@ -201,13 +227,17 @@ ReturnValue_t TmTcBridge::handleStoredTm() {
 
 void TmTcBridge::registerCommConnect() {
 	if(not communicationLinkUp) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		//sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
+#endif
 		communicationLinkUp = true;
 	}
 }
 
 void TmTcBridge::registerCommDisconnect() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	//sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
+#endif
 	if(communicationLinkUp) {
 		communicationLinkUp = false;
 	}
diff --git a/tmtcservices/VerificationReporter.cpp b/tmtcservices/VerificationReporter.cpp
index 7e40bd27..ff6f54f9 100644
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@@ -27,9 +27,11 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VerificationReporter::sendSuccessReport: Error writing "
 				<< "to queue. Code: " << std::hex << status << std::dec
 				<< std::endl;
+#endif
 	}
 }
 
@@ -44,9 +46,11 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VerificationReporter::sendSuccessReport: Error writing "
 				<< "to queue. Code: " << std::hex << status << std::dec
 				<< std::endl;
+#endif
 	}
 }
 
@@ -64,9 +68,11 @@ void VerificationReporter::sendFailureReport(uint8_t report_id,
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 	        &message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VerificationReporter::sendFailureReport: Error writing "
 				<< "to queue. Code: " << std::hex  << "0x" << status << std::dec
 				<< std::endl;
+#endif
 	}
 }
 
@@ -82,24 +88,30 @@ void VerificationReporter::sendFailureReport(uint8_t report_id,
 	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
 	        &message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VerificationReporter::sendFailureReport: Error writing "
 				<< "to queue. Code: " << std::hex << "0x" << status << std::dec
 				<< std::endl;
+#endif
 	}
 }
 
 void VerificationReporter::initialize() {
 	if(messageReceiver == objects::NO_OBJECT) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "VerificationReporter::initialize: Verification message"
 				" receiver object ID not set yet in Factory!" << std::endl;
+#endif
 		return;
 	}
 	AcceptsVerifyMessageIF* temp = objectManager->get<AcceptsVerifyMessageIF>(
 			messageReceiver);
 	if (temp == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "VerificationReporter::initialize: Message "
 				<< "receiver invalid. Make sure it is set up properly and "
 				<< "implementsAcceptsVerifyMessageIF" << std::endl;
+#endif
 		return;
 	}
 	this->acknowledgeQueue = temp->getVerificationQueue();
diff --git a/unittest/internal/InternalUnitTester.cpp b/unittest/internal/InternalUnitTester.cpp
index 503a71ec..bd41969c 100644
--- a/unittest/internal/InternalUnitTester.cpp
+++ b/unittest/internal/InternalUnitTester.cpp
@@ -13,13 +13,17 @@ InternalUnitTester::InternalUnitTester() {}
 InternalUnitTester::~InternalUnitTester() {}
 
 ReturnValue_t InternalUnitTester::performTests() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::info << "Running internal unit tests.." << std::endl;
+#endif
 	testserialize::test_serialization();
 	testmq::testMq();
 	testsemaph::testBinSemaph();
 	testsemaph::testCountingSemaph();
 	testmutex::testMutex();
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 	sif::info << "Internal unit tests finished." << std::endl;
+#endif
 	return RETURN_OK;
 }
 
diff --git a/unittest/internal/UnittDefinitions.cpp b/unittest/internal/UnittDefinitions.cpp
index 6265e9fd..d20771a4 100644
--- a/unittest/internal/UnittDefinitions.cpp
+++ b/unittest/internal/UnittDefinitions.cpp
@@ -1,7 +1,11 @@
 #include "UnittDefinitions.h"
 
  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;
+			<< 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/internal/osal/IntTestSemaphore.cpp b/unittest/internal/osal/IntTestSemaphore.cpp
index 5a82f78c..43990c2c 100644
--- a/unittest/internal/osal/IntTestSemaphore.cpp
+++ b/unittest/internal/osal/IntTestSemaphore.cpp
@@ -94,8 +94,10 @@ void testsemaph::testBinSemaphoreImplementation(SemaphoreIF* binSemaph,
 		result = binSemaph->acquire(SemaphoreIF::TimeoutType::WAITING, 10);
 		//dur_millis_t time = stopwatch.stop();
 //		if(abs(time - 10) > 2) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 //		    sif::error << "UnitTester: Semaphore timeout measured incorrect."
 //		            << std::endl;
+#endif
 //			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..569a8d5f
--- /dev/null
+++ b/unittest/tests/datapoollocal/CMakeLists.txt
@@ -0,0 +1,3 @@
+target_sources(${TARGET_NAME} PRIVATE
+	LocalPoolVariableTest.cpp
+)
diff --git a/unittest/tests/datapoollocal/DataSetTest.cpp b/unittest/tests/datapoollocal/DataSetTest.cpp
new file mode 100644
index 00000000..ccb7603d
--- /dev/null
+++ b/unittest/tests/datapoollocal/DataSetTest.cpp
@@ -0,0 +1,21 @@
+#include "LocalPoolOwnerBase.h"
+
+#include <catch2/catch_test_macros.hpp>
+#include <fsfw/datapoollocal/HasLocalDataPoolIF.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);
+
+	SECTION("BasicTest") {
+		//StaticLocalDataSet<3> localSet = StaticLocalDataSet<3>()
+	}
+}
+
+
+
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..eb5abf35
--- /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());
+
+		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..c86a2681
--- /dev/null
+++ b/unittest/tests/datapoollocal/LocalPoolVectorTest.cpp
@@ -0,0 +1,16 @@
+#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);
+}
+
+
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/testcfg/pollingsequence/PollingSequenceFactory.cpp b/unittest/user/testcfg/pollingsequence/PollingSequenceFactory.cpp
index f836a746..b7f1fb3e 100644
--- a/unittest/user/testcfg/pollingsequence/PollingSequenceFactory.cpp
+++ b/unittest/user/testcfg/pollingsequence/PollingSequenceFactory.cpp
@@ -15,8 +15,10 @@ ReturnValue_t pst::pollingSequenceInitDefault(
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	else {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "pst::pollingSequenceInitDefault: Sequence invalid!"
 		        << std::endl;
+#endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 }
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 0b66558a..02a935d2 100644
--- a/unittest/user/unittest/core/CatchDefinitions.cpp
+++ b/unittest/user/unittest/core/CatchDefinitions.cpp
@@ -1,11 +1,16 @@
 #include "CatchDefinitions.h"
+#include <fsfw/serviceinterface/ServiceInterface.h>
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 
 StorageManagerIF* tglob::getIpcStoreHandle() {
 	if(objectManager != nullptr) {
 		return objectManager->get<StorageManagerIF>(objects::IPC_STORE);
 	} 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) {