diff --git a/FSFWVersion.h b/FSFWVersion.h
index 11a608919..df2d49a51 100644
--- a/FSFWVersion.h
+++ b/FSFWVersion.h
@@ -3,9 +3,9 @@
 
 const char* const FSFW_VERSION_NAME = "ASTP";
 
-#define FSFW_VERSION 		0
-#define	FSFW_SUBVERSION 	0
-#define	FSFW_REVISION 	1
+#define FSFW_VERSION        1
+#define	FSFW_SUBVERSION     0
+#define	FSFW_REVISION       0
 
 
 
diff --git a/action/ActionHelper.cpp b/action/ActionHelper.cpp
index 4b64a40ca..b2374ed69 100644
--- a/action/ActionHelper.cpp
+++ b/action/ActionHelper.cpp
@@ -147,11 +147,6 @@ ReturnValue_t ActionHelper::reportData(MessageQueueId_t reportTo,
         return result;
     }
 
-    if (result != HasReturnvaluesIF::RETURN_OK) {
-        ipcStore->deleteData(storeAddress);
-        return result;
-    }
-
     /* We don't need to report the objectId, as we receive REQUESTED data before the completion
     success message. True aperiodic replies need to be reported with another dedicated message. */
     ActionMessage::setDataReply(&reply, replyId, storeAddress);
diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp
index 5e20bb6fe..fe36341dd 100644
--- a/container/SharedRingBuffer.cpp
+++ b/container/SharedRingBuffer.cpp
@@ -17,6 +17,9 @@ SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer,
     mutex = MutexFactory::instance()->createMutex();
 }
 
+SharedRingBuffer::~SharedRingBuffer() {
+    MutexFactory::instance()->deleteMutex(mutex);
+}
 
 void SharedRingBuffer::setToUseReceiveSizeFIFO(size_t fifoDepth) {
     this->fifoDepth = fifoDepth;
diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h
index 66a119ab0..9d6ea56c2 100644
--- a/container/SharedRingBuffer.h
+++ b/container/SharedRingBuffer.h
@@ -26,6 +26,18 @@ public:
      */
     SharedRingBuffer(object_id_t objectId, const size_t size,
             bool overwriteOld, size_t maxExcessBytes);
+    /**
+     * This constructor takes an external buffer with the specified size.
+     * @param buffer
+     * @param size
+     * @param overwriteOld
+     * If the ring buffer is overflowing at a write operartion, the oldest data
+     * will be overwritten.
+     */
+    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
+            bool overwriteOld, size_t maxExcessBytes);
+
+    virtual~ SharedRingBuffer();
 
     /**
      * @brief    This function can be used to add an optional FIFO to the class
@@ -37,16 +49,7 @@ public:
      */
     void setToUseReceiveSizeFIFO(size_t fifoDepth);
 
-    /**
-     * This constructor takes an external buffer with the specified size.
-     * @param buffer
-     * @param size
-     * @param overwriteOld
-     * If the ring buffer is overflowing at a write operartion, the oldest data
-     * will be overwritten.
-     */
-    SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size,
-            bool overwriteOld, size_t maxExcessBytes);
+
 
     /**
      * Unless a read-only constant value is read, all operations on the
diff --git a/datapool/HkSwitchHelper.cpp b/datapool/HkSwitchHelper.cpp
index 1a2a25eb9..21e37f59d 100644
--- a/datapool/HkSwitchHelper.cpp
+++ b/datapool/HkSwitchHelper.cpp
@@ -7,7 +7,7 @@ HkSwitchHelper::HkSwitchHelper(EventReportingProxyIF* eventProxy) :
 }
 
 HkSwitchHelper::~HkSwitchHelper() {
-	// TODO Auto-generated destructor stub
+    QueueFactory::instance()->deleteMessageQueue(actionQueue);
 }
 
 ReturnValue_t HkSwitchHelper::initialize() {
diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 94ee9c264..dbe68ff14 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -909,27 +909,29 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
 			errorPrint = "Unknown error";
 		}
 	}
+	object_id_t objectId = 0xffffffff;
+	if(owner != nullptr) {
+	    objectId = owner->getObjectId();
+	}
 
 	if(outputType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::warning << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 	else if(outputType == sif::OutputTypes::OUT_ERROR) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "LocalDataPoolManager::" << functionName
-				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
-				<< std::hex << owner->getObjectId() << " | " << errorPrint
-				<< std::dec << std::setfill(' ') << std::endl;
+		sif::error << "LocalDataPoolManager::" << functionName << ": Object ID 0x" <<
+		        std::setw(8) << std::setfill('0') << std::hex << objectId << " | " << errorPrint <<
+		        std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
-				functionName, owner->getObjectId(), errorPrint);
+				functionName, objectId, errorPrint);
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 	}
 #endif /* #if FSFW_VERBOSE_LEVEL >= 1 */
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index e46bd5ded..368e490b7 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1491,7 +1491,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
         if(errorCode == ObjectManagerIF::CHILD_INIT_FAILED) {
             errorPrint = "Initialization error";
         }
-        if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
+        else if(errorCode == HasReturnvaluesIF::RETURN_FAILED) {
             if(errorType == sif::OutputTypes::OUT_WARNING) {
                 errorPrint = "Generic Warning";
             }
@@ -1503,6 +1503,9 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
             errorPrint = "Unknown error";
         }
     }
+    if(functionName == nullptr) {
+        functionName = "unknown function";
+    }
 
     if(errorType == sif::OutputTypes::OUT_WARNING) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -1511,7 +1514,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
                 std::dec << std::setfill(' ') << std::endl;
 #else
         sif::printWarning("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
     else if(errorType == sif::OutputTypes::OUT_ERROR) {
@@ -1522,7 +1525,7 @@ void DeviceHandlerBase::printWarningOrError(sif::OutputTypes errorType,
                 << std::setfill(' ') << std::endl;
 #else
         sif::printError("DeviceHandlerBase::%s: Object ID 0x%08x | %s\n",
-                this->getObjectId(), errorPrint);
+                functionName, this->getObjectId(), errorPrint);
 #endif
     }
 
diff --git a/events/EventMessage.cpp b/events/EventMessage.cpp
index bbc41e100..548b4f0f3 100644
--- a/events/EventMessage.cpp
+++ b/events/EventMessage.cpp
@@ -109,6 +109,6 @@ bool EventMessage::isClearedEventMessage() {
 	return getEvent() == INVALID_EVENT;
 }
 
-size_t EventMessage::getMinimumMessageSize() {
+size_t EventMessage::getMinimumMessageSize() const {
 	return EVENT_MESSAGE_SIZE;
 }
diff --git a/events/EventMessage.h b/events/EventMessage.h
index 4d003bd7b..f2f5ffb50 100644
--- a/events/EventMessage.h
+++ b/events/EventMessage.h
@@ -45,7 +45,7 @@ public:
 
 protected:
 	static const Event INVALID_EVENT = 0;
-	virtual size_t getMinimumMessageSize();
+	virtual size_t getMinimumMessageSize() const override;
 
 };
 
diff --git a/globalfunctions/arrayprinter.cpp b/globalfunctions/arrayprinter.cpp
index c527af9c0..3c729c6bb 100644
--- a/globalfunctions/arrayprinter.cpp
+++ b/globalfunctions/arrayprinter.cpp
@@ -60,7 +60,7 @@ void arrayprinter::printHex(const uint8_t *data, size_t size,
 #else
     // General format: 0x01, 0x02, 0x03 so it is number of chars times 6
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 7 + 1];
+    char printBuffer[(size + 1) * 7 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.
@@ -103,7 +103,7 @@ void arrayprinter::printDec(const uint8_t *data, size_t size,
 #else
     // General format: 32, 243, -12 so it is number of chars times 5
     // plus line break plus small safety margin.
-    char printBuffer[(size + 1) * 5 + 1];
+    char printBuffer[(size + 1) * 5 + 1] = {};
     size_t currentPos = 0;
     for(size_t i = 0; i < size; i++) {
         // To avoid buffer overflows.
diff --git a/health/HealthTable.cpp b/health/HealthTable.cpp
index 5d720b191..8b19d8e39 100644
--- a/health/HealthTable.cpp
+++ b/health/HealthTable.cpp
@@ -68,8 +68,18 @@ void HealthTable::printAll(uint8_t* pointer, size_t maxSize) {
     MutexGuard(mutex, timeoutType, mutexTimeoutMs);
 	size_t size = 0;
 	uint16_t count = healthMap.size();
-	SerializeAdapter::serialize(&count,
+	ReturnValue_t result = SerializeAdapter::serialize(&count,
 			&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
+	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	    sif::warning << "HealthTable::printAll: Serialization of health table failed" << std::endl;
+#else
+	    sif::printWarning("HealthTable::printAll: Serialization of health table failed\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	    return;
+	}
 	for (const auto& health: healthMap) {
 		SerializeAdapter::serialize(&health.first,
 				&pointer, &size, maxSize, SerializeIF::Endianness::BIG);
diff --git a/ipc/MessageQueueIF.h b/ipc/MessageQueueIF.h
index 1c06521ca..74ccb29a2 100644
--- a/ipc/MessageQueueIF.h
+++ b/ipc/MessageQueueIF.h
@@ -27,7 +27,7 @@ public:
 	//! Returned if a reply method was called without partner
 	static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3);
 	//! Returned if the target destination is invalid.
-	static constexpr ReturnValue_t DESTINVATION_INVALID = MAKE_RETURN_CODE(4);
+	static constexpr ReturnValue_t DESTINATION_INVALID = MAKE_RETURN_CODE(4);
 
 	virtual ~MessageQueueIF() {}
 	/**
diff --git a/ipc/MessageQueueMessage.cpp b/ipc/MessageQueueMessage.cpp
index e97778c3f..1958af54a 100644
--- a/ipc/MessageQueueMessage.cpp
+++ b/ipc/MessageQueueMessage.cpp
@@ -86,3 +86,7 @@ size_t MessageQueueMessage::getMaximumMessageSize() const {
     return this->MAX_MESSAGE_SIZE;
 }
 
+size_t MessageQueueMessage::getMaximumDataSize() const {
+    return this->MAX_DATA_SIZE;
+}
+
diff --git a/ipc/MessageQueueMessage.h b/ipc/MessageQueueMessage.h
index 5234f64ff..111056caf 100644
--- a/ipc/MessageQueueMessage.h
+++ b/ipc/MessageQueueMessage.h
@@ -139,6 +139,7 @@ public:
 	virtual void setMessageSize(size_t messageSize) override;
 	virtual size_t getMinimumMessageSize() const override;
 	virtual size_t getMaximumMessageSize() const override;
+	virtual size_t getMaximumDataSize() const override;
 
 	/**
 	 * @brief	This is a debug method that prints the content.
diff --git a/ipc/MessageQueueMessageIF.h b/ipc/MessageQueueMessageIF.h
index 33e01e7d0..893c30b5e 100644
--- a/ipc/MessageQueueMessageIF.h
+++ b/ipc/MessageQueueMessageIF.h
@@ -72,6 +72,7 @@ public:
     virtual void setMessageSize(size_t messageSize) = 0;
     virtual size_t getMinimumMessageSize() const = 0;
     virtual size_t getMaximumMessageSize() const = 0;
+    virtual size_t getMaximumDataSize() const = 0;
 
 };
 
diff --git a/osal/CMakeLists.txt b/osal/CMakeLists.txt
index e4f1de7c7..76b939b1a 100644
--- a/osal/CMakeLists.txt
+++ b/osal/CMakeLists.txt
@@ -1,35 +1,34 @@
 # Check the OS_FSFW variable
 if(${OS_FSFW} STREQUAL "freertos")
-	add_subdirectory(FreeRTOS)
+    add_subdirectory(FreeRTOS)
 elseif(${OS_FSFW} STREQUAL "rtems")
-	add_subdirectory(rtems)
+    add_subdirectory(rtems)
 elseif(${OS_FSFW} STREQUAL "linux")
-	add_subdirectory(linux)
+    add_subdirectory(linux)
 elseif(${OS_FSFW} STREQUAL "host")
-	add_subdirectory(host)
-	if (WIN32)
-		add_subdirectory(windows)
-	elseif(UNIX)
-		target_sources(${LIB_FSFW_NAME}
-			PUBLIC
-				linux/TcUnixUdpPollingTask.cpp
-				linux/TmTcUnixUdpBridge.cpp
-		)
-	endif ()
+    add_subdirectory(host)
+    if (WIN32)
+        add_subdirectory(windows)
+    elseif(UNIX)
+        # We still need to pull in some Linux specific sources
+        target_sources(${LIB_FSFW_NAME} PUBLIC
+            linux/tcpipHelpers.cpp
+        )
+    endif ()
 
 else()
 
-	message(WARNING "The OS_FSFW variable was not set. Assuming host OS..")
-	# Not set. Assumuing this is a host build, try to determine host OS
-	if (WIN32)
-		add_subdirectory(host)
-		add_subdirectory(windows)
-	elseif (UNIX)
-		add_subdirectory(linux)
-	else ()
-		# MacOS or other OSes have not been tested yet / are not supported.
-		message(FATAL_ERROR "The host OS could not be determined! Aborting.")
-	endif()
+    message(WARNING "The OS_FSFW variable was not set. Assuming host OS..")
+    # Not set. Assumuing this is a host build, try to determine host OS
+    if (WIN32)
+        add_subdirectory(host)
+        add_subdirectory(windows)
+    elseif (UNIX)
+        add_subdirectory(linux)
+    else ()
+        # MacOS or other OSes have not been tested yet / are not supported.
+        message(FATAL_ERROR "The host OS could not be determined! Aborting.")
+    endif()
 
 endif()
 
diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp
index 806edcc7c..c15971fee 100644
--- a/osal/FreeRTOS/Clock.cpp
+++ b/osal/FreeRTOS/Clock.cpp
@@ -111,7 +111,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from,
         timeval* to) {
-    struct tm time_tm;
+    struct tm time_tm = {};
 
     time_tm.tm_year = from->year - 1900;
     time_tm.tm_mon = from->month - 1;
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index c0c82cf11..3a0f654ed 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -135,7 +135,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	QueueHandle_t destination = nullptr;
 
 	if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) {
-		return MessageQueueIF::DESTINVATION_INVALID;
+		return MessageQueueIF::DESTINATION_INVALID;
 	}
 	else  {
 		destination = reinterpret_cast<QueueHandle_t>(sendTo);
diff --git a/osal/common/TcpTmTcServer.cpp b/osal/common/TcpTmTcServer.cpp
index 296afad8e..08a62ffb9 100644
--- a/osal/common/TcpTmTcServer.cpp
+++ b/osal/common/TcpTmTcServer.cpp
@@ -70,6 +70,7 @@ ReturnValue_t TcpTmTcServer::initialize() {
 #endif
         freeaddrinfo(addrResult);
         handleError(Protocol::TCP, ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
 
     freeaddrinfo(addrResult);
@@ -84,8 +85,8 @@ TcpTmTcServer::~TcpTmTcServer() {
 ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
     using namespace tcpip;
     /* If a connection is accepted, the corresponding socket will be assigned to the new socket */
-    socket_t clientSocket;
-    sockaddr clientSockAddr;
+    socket_t clientSocket = 0;
+    sockaddr clientSockAddr = {};
     socklen_t connectorSockAddrLen = 0;
     int retval = 0;
 
@@ -101,6 +102,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
         if(clientSocket == INVALID_SOCKET) {
             handleError(Protocol::TCP, ErrorSources::ACCEPT_CALL, 500);
+            closeSocket(clientSocket);
             continue;
         };
 
@@ -122,6 +124,7 @@ ReturnValue_t TcpTmTcServer::performOperation(uint8_t opCode) {
 
         /* Done, shut down connection */
         retval = shutdown(clientSocket, SHUT_SEND);
+        closeSocket(clientSocket);
     }
     return HasReturnvaluesIF::RETURN_OK;
 }
diff --git a/osal/common/UdpTmTcBridge.cpp b/osal/common/UdpTmTcBridge.cpp
index 7f3dc9293..798be6f56 100644
--- a/osal/common/UdpTmTcBridge.cpp
+++ b/osal/common/UdpTmTcBridge.cpp
@@ -103,6 +103,7 @@ ReturnValue_t UdpTmTcBridge::initialize() {
 #endif
         freeaddrinfo(addrResult);
         tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::BIND_CALL);
+        return HasReturnvaluesIF::RETURN_FAILED;
     }
     freeaddrinfo(addrResult);
     return HasReturnvaluesIF::RETURN_OK;
diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp
index 3b169b5a2..016b1404f 100644
--- a/osal/host/FixedTimeslotTask.cpp
+++ b/osal/host/FixedTimeslotTask.cpp
@@ -38,7 +38,6 @@ FixedTimeslotTask::~FixedTimeslotTask(void) {
     if(mainThread.joinable()) {
         mainThread.join();
     }
-    delete this;
 }
 
 void FixedTimeslotTask::taskEntryPoint(void* argument) {
@@ -119,8 +118,11 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
     }
 
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "Component " << std::hex << componentId <<
-            " not found, not adding it to pst" << std::endl;
+    sif::error << "Component " << std::hex << "0x" << componentId << "not found, "
+            "not adding it to PST.." << std::dec << std::endl;
+#else
+    sif::printError("Component 0x%08x not found, not adding it to PST..\n",
+            static_cast<unsigned int>(componentId));
 #endif
     return HasReturnvaluesIF::RETURN_FAILED;
 }
diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 18272a68a..41c55a3df 100644
--- a/osal/host/MessageQueue.cpp
+++ b/osal/host/MessageQueue.cpp
@@ -64,9 +64,8 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 		return MessageQueueIF::EMPTY;
 	}
 	MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
-	MessageQueueMessage* currentMessage = &messageQueue.front();
-	std::copy(currentMessage->getBuffer(),
-			currentMessage->getBuffer() + messageSize, message->getBuffer());
+	std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
+	        message->getBuffer());
 	messageQueue.pop();
 	// The last partner is the first uint32_t field in the message
 	this->lastPartner = message->getSender();
@@ -80,7 +79,7 @@ MessageQueueId_t MessageQueue::getLastPartner() const {
 ReturnValue_t MessageQueue::flush(uint32_t* count) {
 	*count = messageQueue.size();
 	// Clears the queue.
-	messageQueue = std::queue<MessageQueueMessage>();
+	messageQueue = std::queue<std::vector<uint8_t>>();
 	return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -106,6 +105,9 @@ bool MessageQueue::isDefaultDestinationSet() const {
 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
         MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
         bool ignoreFault) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
 	message->setSender(sentFrom);
 	if(message->getMessageSize() > message->getMaximumMessageSize()) {
 		// Actually, this should never happen or an error will be emitted
@@ -128,21 +130,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
-		MutexGuard mutexLock(targetQueue->queueLock,
-		        MutexIF::TimeoutType::WAITING, 20);
-		// not ideal, works for now though.
-		MessageQueueMessage* mqmMessage =
-				dynamic_cast<MessageQueueMessage*>(message);
-		if(message != nullptr) {
-			targetQueue->messageQueue.push(*mqmMessage);
-		}
-		else {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
-					"is not MessageQueueMessage!" << std::endl;
-#endif
-		}
-
+		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
+		targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
+		memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
+		        message->getMaximumMessageSize());
 	}
 	else {
 		return MessageQueueIF::FULL;
diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h
index 97a9e4915..e965123dc 100644
--- a/osal/host/MessageQueue.h
+++ b/osal/host/MessageQueue.h
@@ -212,7 +212,7 @@ protected:
 	//static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault);
 
 private:
-	std::queue<MessageQueueMessage> messageQueue;
+	std::queue<std::vector<uint8_t>> messageQueue;
 	/**
 	 * @brief	The class stores the queue id it got assigned.
 	 * 			If initialization fails, the queue id is set to zero.
diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp
index d7abf9d0c..631264b74 100644
--- a/osal/host/PeriodicTask.cpp
+++ b/osal/host/PeriodicTask.cpp
@@ -38,7 +38,6 @@ PeriodicTask::~PeriodicTask(void) {
 	if(mainThread.joinable()) {
 		mainThread.join();
 	}
-	delete this;
 }
 
 void PeriodicTask::taskEntryPoint(void* argument) {
diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp
index b50d62dcf..c9100fe9a 100644
--- a/osal/host/QueueMapManager.cpp
+++ b/osal/host/QueueMapManager.cpp
@@ -10,6 +10,10 @@ QueueMapManager::QueueMapManager() {
     mapLock = MutexFactory::instance()->createMutex();
 }
 
+QueueMapManager::~QueueMapManager() {
+    MutexFactory::instance()->deleteMutex(mapLock);
+}
+
 QueueMapManager* QueueMapManager::instance() {
 	if (mqManagerInstance == nullptr){
 		mqManagerInstance = new QueueMapManager();
diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h
index 3610ca638..90c39c2f0 100644
--- a/osal/host/QueueMapManager.h
+++ b/osal/host/QueueMapManager.h
@@ -36,6 +36,8 @@ public:
 private:
 	//! External instantiation is forbidden.
 	QueueMapManager();
+	~QueueMapManager();
+
 	uint32_t queueCounter = 0;
 	MutexIF* mapLock;
 	QueueMap queueMap;
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index 60d15dee7..12774a58c 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -190,13 +190,15 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 		return HasReturnvaluesIF::RETURN_OK;
-	}else if(status==0){
+	}
+	else if (status==0) {
 		//Success but no message received
 		return MessageQueueIF::EMPTY;
-	} else {
+	}
+	else {
 		//No message was received. Keep lastPartner anyway, I might send
 		//something later. But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 		switch(errno){
 		case EAGAIN:
 			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
@@ -371,7 +373,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 					<<"mq_send to: " << sendTo << " sent from "
 					<< sentFrom << std::endl;
 #endif
-			return DESTINVATION_INVALID;
+			return DESTINATION_INVALID;
 		}
 		case EINTR:
 			//The call was interrupted by a signal.
diff --git a/osal/linux/tcpipHelpers.cpp b/osal/linux/tcpipHelpers.cpp
index 4c1b9a780..3e8f60092 100644
--- a/osal/linux/tcpipHelpers.cpp
+++ b/osal/linux/tcpipHelpers.cpp
@@ -1,5 +1,6 @@
 #include "../common/tcpipHelpers.h"
 
+#include "../../serviceinterface/ServiceInterface.h"
 #include "../../tasks/TaskFactory.h"
 
 #include <errno.h>
diff --git a/osal/rtems/MessageQueue.cpp b/osal/rtems/MessageQueue.cpp
index bfaf35690..717b80dd0 100644
--- a/osal/rtems/MessageQueue.cpp
+++ b/osal/rtems/MessageQueue.cpp
@@ -61,7 +61,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
 	} else {
 		//No message was received. Keep lastPartner anyway, I might send something later.
 		//But still, delete packet content.
-		memset(message->getData(), 0, message->getMaximumMessageSize());
+		memset(message->getData(), 0, message->getMaximumDataSize());
 	}
 	return convertReturnCode(status);
 }
diff --git a/pus/CService200ModeCommanding.cpp b/pus/CService200ModeCommanding.cpp
index c4e99359b..70caadd10 100644
--- a/pus/CService200ModeCommanding.cpp
+++ b/pus/CService200ModeCommanding.cpp
@@ -61,8 +61,7 @@ ReturnValue_t CService200ModeCommanding::prepareCommand(
 		return result;
 	}
 
-	ModeMessage::setModeMessage(dynamic_cast<CommandMessage*>(message),
-	        ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
+	ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(),
 	        modeCommandPacket.getSubmode());
 	return result;
 }
diff --git a/pus/Service1TelecommandVerification.cpp b/pus/Service1TelecommandVerification.cpp
index 9e86c752a..7ce75478b 100644
--- a/pus/Service1TelecommandVerification.cpp
+++ b/pus/Service1TelecommandVerification.cpp
@@ -15,7 +15,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(
 	tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service1TelecommandVerification::~Service1TelecommandVerification() {}
+Service1TelecommandVerification::~Service1TelecommandVerification() {
+    QueueFactory::instance()->deleteMessageQueue(tmQueue);
+}
 
 MessageQueueId_t Service1TelecommandVerification::getVerificationQueue(){
 	return tmQueue->getId();
diff --git a/pus/Service20ParameterManagement.cpp b/pus/Service20ParameterManagement.cpp
index bc3a9119b..90e966500 100644
--- a/pus/Service20ParameterManagement.cpp
+++ b/pus/Service20ParameterManagement.cpp
@@ -75,9 +75,8 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue
 #else
         sif::printError("Service20ParameterManagement::checkInterfaceAndAcquire"
                 "MessageQueue: Can't access object\n");
-        sif::printError("Object ID: 0x%08x\n", objectId);
-        sif::printError("Make sure it implements "
-                "ReceivesParameterMessagesIF!\n");
+        sif::printError("Object ID: 0x%08x\n", *objectId);
+        sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n");
 #endif
 
         return CommandingServiceBase::INVALID_OBJECT;
diff --git a/pus/Service5EventReporting.cpp b/pus/Service5EventReporting.cpp
index 29eb7f20e..965a27ad6 100644
--- a/pus/Service5EventReporting.cpp
+++ b/pus/Service5EventReporting.cpp
@@ -15,7 +15,9 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId,
 	eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
-Service5EventReporting::~Service5EventReporting(){}
+Service5EventReporting::~Service5EventReporting() {
+    QueueFactory::instance()->deleteMessageQueue(eventQueue);
+}
 
 ReturnValue_t Service5EventReporting::performService() {
 	EventMessage message;
diff --git a/pus/Service8FunctionManagement.cpp b/pus/Service8FunctionManagement.cpp
index a2202abc1..54187a829 100644
--- a/pus/Service8FunctionManagement.cpp
+++ b/pus/Service8FunctionManagement.cpp
@@ -53,12 +53,14 @@ ReturnValue_t Service8FunctionManagement::checkInterfaceAndAcquireMessageQueue(
 ReturnValue_t Service8FunctionManagement::prepareCommand(
 		CommandMessage* message, uint8_t subservice, const uint8_t* tcData,
 		size_t tcDataLen, uint32_t* state, object_id_t objectId) {
-	return prepareDirectCommand(dynamic_cast<CommandMessage*>(message),
-				tcData, tcDataLen);
+	return prepareDirectCommand(message, tcData, tcDataLen);
 }
 
 ReturnValue_t Service8FunctionManagement::prepareDirectCommand(
 		CommandMessage *message, const uint8_t *tcData, size_t tcDataLen) {
+    if(message == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
     if(tcDataLen < sizeof(object_id_t) + sizeof(ActionId_t)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         sif::debug << "Service8FunctionManagement::prepareDirectCommand:"
diff --git a/pus/servicepackets/Service8Packets.h b/pus/servicepackets/Service8Packets.h
index b026edf52..a27cf8bb6 100644
--- a/pus/servicepackets/Service8Packets.h
+++ b/pus/servicepackets/Service8Packets.h
@@ -43,8 +43,8 @@ public:
 
 private:
 	DirectCommand(const DirectCommand &command);
-	object_id_t objectId;
-	ActionId_t actionId;
+	object_id_t objectId = 0;
+	ActionId_t actionId = 0;
 	uint32_t parametersSize; //!< [EXPORT] : [IGNORE]
 	const uint8_t * parameterBuffer; //!< [EXPORT] : [MAXSIZE] 65535 Bytes
 
diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h
index 470802927..db66f9cc2 100644
--- a/serialize/SerializeElement.h
+++ b/serialize/SerializeElement.h
@@ -25,7 +25,7 @@ public:
 
 	}
 	SerializeElement() :
-			LinkedElement<SerializeIF>(this) {
+			LinkedElement<SerializeIF>(this), entry() {
 	}
 
 	ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,
diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp
index b795854f2..62cbfbf2b 100644
--- a/tcdistribution/CCSDSDistributor.cpp
+++ b/tcdistribution/CCSDSDistributor.cpp
@@ -1,8 +1,10 @@
 #include "CCSDSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/SpacePacketBase.h"
 
+#define CCSDS_DISTRIBUTOR_DEBUGGING     0
+
 CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 		object_id_t setObjectId):
 		TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
@@ -11,26 +13,36 @@ CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
 CCSDSDistributor::~CCSDSDistributor() {}
 
 TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
+#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif::debug << "CCSDSDistributor::selectDestination received: " <<
-//			this->currentMessage.getStorageId().pool_index << ", " <<
-//			this->currentMessage.getStorageId().packet_index << std::endl;
+	sif::debug << "CCSDSDistributor::selectDestination received: " <<
+			this->currentMessage.getStorageId().poolIndex << ", " <<
+			this->currentMessage.getStorageId().packetIndex << std::endl;
+#else
+	sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
+	        currentMessage.getStorageId().poolIndex, currentMessage.getStorageId().packetIndex);
+#endif
 #endif
 	const uint8_t* packet = nullptr;
 	size_t size = 0;
 	ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
 			&packet, &size );
 	if(result != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "CCSDSDistributor::selectDestination: Getting data from"
 				" store failed!" << std::endl;
+#else
+		sif::printError("CCSDSDistributor::selectDestination: Getting data from"
+                " store failed!\n");
+#endif
 #endif
 	}
 	SpacePacketBase currentPacket(packet);
 
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-//	sif:: info << "CCSDSDistributor::selectDestination has packet with APID "
-//			<< std::hex << currentPacket.getAPID() << std::dec << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
+	sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex <<
+	        currentPacket.getAPID() << std::dec << std::endl;
 #endif
 	TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
 	if ( position != this->queueMap.end() ) {
@@ -76,9 +88,14 @@ ReturnValue_t CCSDSDistributor::initialize() {
 	ReturnValue_t status = this->TcDistributor::initialize();
 	this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
 	if (this->tcStore == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 	    sif::error << "CCSDSDistributor::initialize: Could not initialize"
 	            " TC store!" << std::endl;
+#else
+	    sif::printError("CCSDSDistributor::initialize: Could not initialize"
+                " TC store!\n");
+#endif
 #endif
 	    status = RETURN_FAILED;
 	}
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 00fd9029a..abdd1f8d3 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -1,22 +1,24 @@
 #include "CCSDSDistributorIF.h"
 #include "PUSDistributor.h"
 
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../serviceinterface/ServiceInterface.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcservices/PusVerificationReport.h"
 
+#define PUS_DISTRIBUTOR_DEBUGGING     0
+
 PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
-		object_id_t setPacketSource) :
-		TcDistributor(setObjectId), checker(setApid), verifyChannel(),
-		tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
+        object_id_t setPacketSource) :
+        TcDistributor(setObjectId), checker(setApid), verifyChannel(),
+        tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
 
 PUSDistributor::~PUSDistributor() {}
 
 PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    // sif:: debug << "PUSDistributor::handlePacket received: "
-    //          << this->current_packet_id.store_index << ", "
-    //          << this->current_packet_id.packet_index << std::endl;
+#if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
+    store_address_t storeId = this->currentMessage.getStorageId());
+    sif:: debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " <<
+            storeId.packetIndex << std::endl;
 #endif
     TcMqMapIter queueMapIt = this->queueMap.end();
     if(this->currentPacket == nullptr) {
@@ -25,15 +27,17 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
     this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
     if (currentPacket->getWholeData() != nullptr) {
         tcStatus = checker.checkPacket(currentPacket);
-#ifdef DEBUG
         if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-            sif::debug << "PUSDistributor::handlePacket: Packet format "
-                    << "invalid, code "<< static_cast<int>(tcStatus)
-                    << std::endl;
+            sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
+                    static_cast<int>(tcStatus) << std::endl;
+#else
+            sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
+                    static_cast<int>(tcStatus));
+#endif
 #endif
         }
-#endif
         uint32_t queue_id = currentPacket->getService();
         queueMapIt = this->queueMap.find(queue_id);
     }
@@ -43,11 +47,12 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
     if (queueMapIt == this->queueMap.end()) {
         tcStatus = DESTINATION_NOT_FOUND;
-#ifdef DEBUG
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::debug << "PUSDistributor::handlePacket: Destination not found, "
-                << "code "<< static_cast<int>(tcStatus) << std::endl;
-#endif
+        sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
+#else
+        sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
+#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif
     }
 
@@ -62,46 +67,54 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
 
 
 ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
-	uint16_t serviceId = service->getIdentifier();
+    uint16_t serviceId = service->getIdentifier();
+#if PUS_DISTRIBUTOR_DEBUGGING == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-	// sif::info << "Service ID: " << (int)serviceId << std::endl;
+    sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
+#else
+    sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
 #endif
-	MessageQueueId_t queue = service->getRequestQueue();
-	auto returnPair = queueMap.emplace(serviceId, queue);
-	if (not returnPair.second) {
+#endif
+    MessageQueueId_t queue = service->getRequestQueue();
+    auto returnPair = queueMap.emplace(serviceId, queue);
+    if (not returnPair.second) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::registerService: Service ID already"
-				" exists in map." << std::endl;
+        sif::error << "PUSDistributor::registerService: Service ID already"
+                " exists in map" << std::endl;
+#else
+        sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
 #endif
-		return SERVICE_ID_ALREADY_EXISTS;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+#endif
+        return SERVICE_ID_ALREADY_EXISTS;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 MessageQueueId_t PUSDistributor::getRequestQueue() {
-	return tcQueue->getId();
+    return tcQueue->getId();
 }
 
 ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
-	if (queueStatus != RETURN_OK) {
-		tcStatus = queueStatus;
-	}
-	if (tcStatus != RETURN_OK) {
-		this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
-				currentPacket, tcStatus);
-		// A failed packet is deleted immediately after reporting,
-		// otherwise it will block memory.
-		currentPacket->deletePacket();
-		return RETURN_FAILED;
-	} else {
-		this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
-				currentPacket);
-		return RETURN_OK;
-	}
+    if (queueStatus != RETURN_OK) {
+        tcStatus = queueStatus;
+    }
+    if (tcStatus != RETURN_OK) {
+        this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
+                currentPacket, tcStatus);
+        // A failed packet is deleted immediately after reporting,
+        // otherwise it will block memory.
+        currentPacket->deletePacket();
+        return RETURN_FAILED;
+    } else {
+        this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
+                currentPacket);
+        return RETURN_OK;
+    }
 }
 
 uint16_t PUSDistributor::getIdentifier() {
-	return checker.getApid();
+    return checker.getApid();
 }
 
 ReturnValue_t PUSDistributor::initialize() {
@@ -111,15 +124,17 @@ ReturnValue_t PUSDistributor::initialize() {
         return ObjectManagerIF::CHILD_INIT_FAILED;
     }
 
-	CCSDSDistributorIF* ccsdsDistributor =
-			objectManager->get<CCSDSDistributorIF>(packetSource);
-	if (ccsdsDistributor == nullptr) {
+    CCSDSDistributorIF* ccsdsDistributor =
+            objectManager->get<CCSDSDistributorIF>(packetSource);
+    if (ccsdsDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "PUSDistributor::initialize: Packet source invalid."
-		        << " Make sure it exists and implements CCSDSDistributorIF!"
-		        << std::endl;
+        sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
+        sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
+#else
+        sif::printError("PUSDistributor::initialize: Packet source invalid\n");
+        sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
 #endif
-	    return RETURN_FAILED;
-	}
-	return ccsdsDistributor->registerApplication(this);
+        return RETURN_FAILED;
+    }
+    return ccsdsDistributor->registerApplication(this);
 }
diff --git a/thermal/TemperatureSensor.h b/thermal/TemperatureSensor.h
index 2b1fb1f07..ceb8a8617 100644
--- a/thermal/TemperatureSensor.h
+++ b/thermal/TemperatureSensor.h
@@ -1,11 +1,14 @@
 #ifndef TEMPERATURESENSOR_H_
 #define TEMPERATURESENSOR_H_
 
-#include "../thermal/AbstractTemperatureSensor.h"
-#include "../datapoolglob/GlobalDataSet.h"
-#include "../datapoolglob/GlobalPoolVariable.h"
+#include "tcsDefinitions.h"
+#include "AbstractTemperatureSensor.h"
+
+#include "../datapoollocal/LocalPoolDataSetBase.h"
+#include "../datapoollocal/LocalPoolVariable.h"
 #include "../monitoring/LimitMonitor.h"
 
+
 /**
  * @brief 	This building block handles non-linear value conversion and
  * 			range checks for analog temperature sensors.
@@ -57,27 +60,26 @@ public:
 
 	/**
 	 * Instantiate Temperature Sensor Object.
-	 * @param setObjectid objectId of the sensor object
-	 * @param inputValue Input value which is converted to a temperature
-	 * @param poolVariable Pool Variable to store the temperature value
-	 * @param vectorIndex Vector Index for the sensor monitor
-	 * @param parameters Calculation parameters, temperature limits, gradient limit
-	 * @param datapoolId Datapool ID of the output temperature
-	 * @param outputSet Output dataset for the output temperature to fetch it with read()
+	 * @param setObjectid   objectId of the sensor object
+	 * @param inputValue    Pointer to input value which is converted to a temperature
+	 * @param variableGpid  Global Pool ID of the output value
+	 * @param inputVariable Input variable handle
+	 * @param vectorIndex   Vector Index for the sensor monitor
+	 * @param parameters    Calculation parameters, temperature limits, gradient limit
+	 * @param outputSet     Output dataset for the output temperature to fetch it with read()
 	 * @param thermalModule respective thermal module, if it has one
 	 */
 	TemperatureSensor(object_id_t setObjectid,
-			inputType *inputValue, PoolVariableIF *poolVariable,
-			uint8_t vectorIndex, uint32_t datapoolId, Parameters parameters = {0, 0, 0, 0, 0, 0},
-			GlobDataSet *outputSet = NULL, ThermalModuleIF *thermalModule = NULL) :
+			inputType *inputValue, gp_id_t variableGpid, PoolVariableIF* inputVariable,
+			uint8_t vectorIndex, Parameters parameters = {0, 0, 0, 0, 0, 0},
+			LocalPoolDataSetBase *outputSet = NULL, ThermalModuleIF *thermalModule = NULL) :
 			AbstractTemperatureSensor(setObjectid, thermalModule), parameters(parameters),
-			inputValue(inputValue), poolVariable(poolVariable),
-			outputTemperature(datapoolId, outputSet, PoolVariableIF::VAR_WRITE),
-			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR,
-				GlobalDataPool::poolIdAndPositionToPid(poolVariable->getDataPoolId(), vectorIndex),
+			inputValue(inputValue), poolVariable(inputVariable),
+			outputTemperature(variableGpid, outputSet, PoolVariableIF::VAR_WRITE),
+			sensorMonitor(setObjectid, DOMAIN_ID_SENSOR, poolVariable,
 				DEFAULT_CONFIRMATION_COUNT, parameters.lowerLimit, parameters.upperLimit,
 				TEMP_SENSOR_LOW, TEMP_SENSOR_HIGH),
-			oldTemperature(20), uptimeOfOldTemperature( { INVALID_TEMPERATURE, 0 }) {
+			oldTemperature(20), uptimeOfOldTemperature({ thermal::INVALID_TEMPERATURE, 0 }) {
 	}
 
 
@@ -98,7 +100,7 @@ protected:
 
 private:
 	void setInvalid() {
-		outputTemperature = INVALID_TEMPERATURE;
+		outputTemperature = thermal::INVALID_TEMPERATURE;
 		outputTemperature.setValid(false);
 		uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 		sensorMonitor.setToInvalid();
@@ -108,11 +110,11 @@ protected:
 
 	UsedParameters parameters;
 
-	inputType * inputValue;
+	inputType* inputValue;
 
-	PoolVariableIF *poolVariable;
+	PoolVariableIF* poolVariable;
 
-	gp_float_t outputTemperature;
+	lp_var_t<float> outputTemperature;
 
 	LimitMonitor<limitType> sensorMonitor;
 
@@ -120,8 +122,8 @@ protected:
 	timeval uptimeOfOldTemperature;
 
 	void doChildOperation() {
-		if (!poolVariable->isValid()
-				|| !healthHelper.healthTable->isHealthy(getObjectId())) {
+		if ((not poolVariable->isValid()) or
+		        (not healthHelper.healthTable->isHealthy(getObjectId()))) {
 			setInvalid();
 			return;
 		}
@@ -152,13 +154,13 @@ protected:
 			}
 		}
 
-		//Check is done against raw limits. SHOULDDO: Why? Using �C would be more easy to handle.
+		//Check is done against raw limits. SHOULDDO: Why? Using C would be more easy to handle.
 		sensorMonitor.doCheck(outputTemperature.value);
 
 		if (sensorMonitor.isOutOfLimits()) {
 			uptimeOfOldTemperature.tv_sec = INVALID_UPTIME;
 			outputTemperature.setValid(PoolVariableIF::INVALID);
-			outputTemperature = INVALID_TEMPERATURE;
+			outputTemperature = thermal::INVALID_TEMPERATURE;
 		} else {
 			oldTemperature = outputTemperature;
 			uptimeOfOldTemperature = uptime;
diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp
index f99b90516..dcffac41e 100644
--- a/tmtcservices/TmTcBridge.cpp
+++ b/tmtcservices/TmTcBridge.cpp
@@ -16,7 +16,9 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination,
             createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
 }
 
-TmTcBridge::~TmTcBridge() {}
+TmTcBridge::~TmTcBridge() {
+    QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue);
+}
 
 ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(
 		uint8_t sentPacketsPerCycle) {