diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff060d7be..8ba6a187e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -184,6 +184,7 @@ endif()
 target_include_directories(${LIB_FSFW_NAME} INTERFACE 
 	${CMAKE_SOURCE_DIR}
 	${FSFW_CONFIG_PATH_ABSOLUTE}
+	${FSFW_ADD_INC_PATHS_ABS}
 )
 
 # Includes path required to compile FSFW itself as well
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/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp
index 359ce8ec1..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;
 }
 
@@ -132,20 +131,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 	}
 	if(targetQueue->messageQueue.size() < targetQueue->messageDepth) {
 		MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
-		// TODO: Would be nice to support other message types, but this would require
-		// create the message on the heap from an upper layer and simply storing
-		// MessageQueueMessageIF pointers in the queue.
-		MessageQueueMessage* mqmMessage = dynamic_cast<MessageQueueMessage*>(message);
-		if(mqmMessage != nullptr) {
-			targetQueue->messageQueue.push(*mqmMessage);
-		}
-		else {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-			sif::error << "MessageQueue::sendMessageFromMessageQueue: Message"
-					"is not MessageQueueMessage!" << std::endl;
-#endif
-		}
-
+		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.