Merge branch 'mueller/MQM_IF_UpdateLinux' into mueller/MQMQ_IF_UpdateFreeRTOS

2020-09-29 15:24:12 +02:00 · 2020-09-29 15:24:12 +02:00 · 046eec6c18
parent bf5688c8d8 47c21cfc6c
commit 046eec6c18
42 changed files with 1100 additions and 533 deletions
--- a/container/ArrayList.h
+++ b/container/ArrayList.h
@ -97,8 +97,7 @@ public:
 		bool operator!=(const typename ArrayList<T, count_t>::Iterator& other) const {
 			return !(*this == other);
 		}
-	}
-	;
+	};

 	/**
 	 * Number of Elements stored in this List
--- a/container/FixedOrderedMultimap.h
+++ b/container/FixedOrderedMultimap.h
@ -10,10 +10,9 @@
 template<typename key_t, typename T, typename KEY_COMPARE = std::less<key_t>>
 class FixedOrderedMultimap {
 public:
-	static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
-	static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
-	static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02);
-	static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03);
+	static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MULTIMAP;
+	static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x01);
+	static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x02);

 private:
 	typedef KEY_COMPARE compare;
--- a/devicehandlers/FixedSequenceSlot.cpp
+++ b/devicehandlers/FixedSequenceSlot.cpp
@ -1,20 +0,0 @@
-/**
- * @file	PollingSlot.cpp
- * @brief	This file defines the PollingSlot class.
- * @date	19.12.2012
- * @author	baetz
- */
-
-#include "FixedSequenceSlot.h"
-#include "../objectmanager/SystemObjectIF.h"
-#include <cstddef>
-
-FixedSequenceSlot::FixedSequenceSlot(object_id_t handlerId, uint32_t setTime,
-		int8_t setSequenceId, PeriodicTaskIF* executingTask) :
-		handler(NULL), pollingTimeMs(setTime), opcode(setSequenceId) {
-	handler = objectManager->get<ExecutableObjectIF>(handlerId);
-	handler->setTaskIF(executingTask);
-}
-
-FixedSequenceSlot::~FixedSequenceSlot() {}
-
--- a/ipc/CommandMessage.h
+++ b/ipc/CommandMessage.h
@ -2,6 +2,7 @@
 #define FSFW_IPC_COMMANDMESSAGE_H_

 #include "CommandMessageIF.h"
+
 #include "MessageQueueMessage.h"
 #include "FwMessageTypes.h"

--- a/ipc/messageQueueDefinitions.h
+++ b/ipc/messageQueueDefinitions.h
@ -1,19 +1,18 @@
-#ifndef FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_
-#define FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_
-
-#include <cstdint>
-
-/*
- * TODO: Actually, the definition of this ID to be a uint32_t is not  ideal and
- * breaks layering. However, it is difficult to keep layering, as the ID is
- * stored in many places and sent around in MessageQueueMessage.
- * Ideally, one would use the (current) object_id_t only, however, doing a
- * lookup of queueIDs for every call does not sound ideal.
- * In a first step, I'll circumvent the issue by not touching it,
- * maybe in a second step. This also influences Interface design
- * (getCommandQueue) and some other issues..
- */
-using MessageQueueId_t = uint32_t;
-
-
-#endif /* FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_ */
+#ifndef FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_
+#define FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_
+
+#include <cstdint>
+
+/*
+ * TODO: Actually, the definition of this ID to be a uint32_t is not  ideal and
+ * breaks layering. However, it is difficult to keep layering, as the ID is
+ * stored in many places and sent around in MessageQueueMessage.
+ * Ideally, one would use the (current) object_id_t only, however, doing a
+ * lookup of queueIDs for every call does not sound ideal.
+ * In a first step, I'll circumvent the issue by not touching it,
+ * maybe in a second step. This also influences Interface design
+ * (getCommandQueue) and some other issues..
+ */
+using MessageQueueId_t = uint32_t;
+
+#endif /* FSFW_IPC_MESSAGEQUEUEDEFINITIONS_H_ */
--- a/memory/AcceptsMemoryMessagesIF.h
+++ b/memory/AcceptsMemoryMessagesIF.h
@ -1,12 +1,5 @@
-/**
- * @file	AcceptsMemoryMessagesIF.h
- * @brief	This file defines the AcceptsMemoryMessagesIF class.
- * @date	11.07.2013
- * @author	baetz
- */
-
-#ifndef ACCEPTSMEMORYMESSAGESIF_H_
-#define ACCEPTSMEMORYMESSAGESIF_H_
+#ifndef FSFW_MEMORY_ACCEPTSMEMORYMESSAGESIF_H_
+#define FSFW_MEMORY_ACCEPTSMEMORYMESSAGESIF_H_

 #include "HasMemoryIF.h"
 #include "MemoryMessage.h"
@ -18,4 +11,4 @@ public:
 };


-#endif /* ACCEPTSMEMORYMESSAGESIF_H_ */
+#endif /* FSFW_MEMORY_ACCEPTSMEMORYMESSAGESIF_H_ */
--- a/memory/MemoryHelper.cpp
+++ b/memory/MemoryHelper.cpp
@ -1,14 +1,15 @@
-#include "../globalfunctions/CRC.h"
 #include "MemoryHelper.h"
 #include "MemoryMessage.h"
+
+#include "../globalfunctions/CRC.h"
 #include "../objectmanager/ObjectManagerIF.h"
 #include "../serialize/EndianConverter.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"

-MemoryHelper::MemoryHelper(HasMemoryIF* workOnThis, MessageQueueIF* useThisQueue) :
-		workOnThis(workOnThis), queueToUse(useThisQueue), ipcStore(NULL), ipcAddress(), lastCommand(
-				CommandMessage::CMD_NONE), lastSender(0), reservedSpaceInIPC(
-				NULL), busy(false) {
+MemoryHelper::MemoryHelper(HasMemoryIF* workOnThis,
+		MessageQueueIF* useThisQueue):
+		workOnThis(workOnThis), queueToUse(useThisQueue), ipcAddress(),
+		lastCommand(CommandMessage::CMD_NONE), busy(false) {
 }

 ReturnValue_t MemoryHelper::handleMemoryCommand(CommandMessage* message) {
@ -33,17 +34,8 @@ ReturnValue_t MemoryHelper::handleMemoryCommand(CommandMessage* message) {
 	}
 }

-ReturnValue_t MemoryHelper::initialize() {
-	ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
-	if (ipcStore != NULL) {
-		return RETURN_OK;
-	} else {
-		return RETURN_FAILED;
-	}
-}
-
 void MemoryHelper::completeLoad(ReturnValue_t errorCode,
-		const uint8_t* dataToCopy, const uint32_t size, uint8_t* copyHere) {
+		const uint8_t* dataToCopy, const size_t size, uint8_t* copyHere) {
 	busy = false;
 	switch (errorCode) {
 	case HasMemoryIF::DO_IT_MYSELF:
@ -67,13 +59,13 @@ void MemoryHelper::completeLoad(ReturnValue_t errorCode,
 		return;
 	}
 	//Only reached on success
-	CommandMessage reply(CommandMessage::REPLY_COMMAND_OK, 0, 0);
+	CommandMessage reply( CommandMessage::REPLY_COMMAND_OK, 0, 0);
 	queueToUse->sendMessage(lastSender, &reply);
 	ipcStore->deleteData(ipcAddress);
 }

 void MemoryHelper::completeDump(ReturnValue_t errorCode,
-		const uint8_t* dataToCopy, const uint32_t size) {
+		const uint8_t* dataToCopy, const size_t size) {
 	busy = false;
 	CommandMessage reply;
 	MemoryMessage::setMemoryReplyFailed(&reply, errorCode, lastCommand);
@ -125,12 +117,12 @@ void MemoryHelper::completeDump(ReturnValue_t errorCode,
 		break;
 	}
 	if (queueToUse->sendMessage(lastSender, &reply) != RETURN_OK) {
-		reply.clearCommandMessage();
+		reply.clear();
 	}
 }

 void MemoryHelper::swapMatrixCopy(uint8_t* out, const uint8_t *in,
-		uint32_t totalSize, uint8_t datatypeSize) {
+		size_t totalSize, uint8_t datatypeSize) {
 	if (totalSize % datatypeSize != 0){
 		return;
 	}
@ -185,11 +177,18 @@ void MemoryHelper::handleMemoryCheckOrDump(CommandMessage* message) {
 }

 ReturnValue_t MemoryHelper::initialize(MessageQueueIF* queueToUse_) {
-	if(queueToUse_!=NULL){
-		this->queueToUse = queueToUse_;
-	}else{
-		return MessageQueueIF::NO_QUEUE;
+	if(queueToUse_ == nullptr) {
+		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-
+	this->queueToUse = queueToUse_;
 	return initialize();
 }
+
+ReturnValue_t MemoryHelper::initialize() {
+	ipcStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	if (ipcStore != nullptr) {
+		return RETURN_OK;
+	} else {
+		return RETURN_FAILED;
+	}
+}
--- a/memory/MemoryHelper.h
+++ b/memory/MemoryHelper.h
@ -1,11 +1,16 @@
-#ifndef MEMORYHELPER_H_
-#define MEMORYHELPER_H_
-#include "../ipc/CommandMessage.h"
+#ifndef FSFW_MEMORY_MEMORYHELPER_H_
+#define FSFW_MEMORY_MEMORYHELPER_H_
+
 #include "AcceptsMemoryMessagesIF.h"
+
+#include "../ipc/CommandMessage.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../storagemanager/StorageManagerIF.h"
 #include "../ipc/MessageQueueIF.h"

+/**
+ * @brief   TODO: documentation.
+ */
 class MemoryHelper : public HasReturnvaluesIF {
 public:
 	static const uint8_t INTERFACE_ID = CLASS_ID::MEMORY_HELPER;
@ -13,25 +18,32 @@ public:
 	static const ReturnValue_t INVALID_ADDRESS = MAKE_RETURN_CODE(0xE1);
 	static const ReturnValue_t INVALID_SIZE = MAKE_RETURN_CODE(0xE2);
 	static const ReturnValue_t STATE_MISMATCH = MAKE_RETURN_CODE(0xE3);
+
+    MemoryHelper(HasMemoryIF* workOnThis, MessageQueueIF* useThisQueue);
+    ~MemoryHelper();
+
+    ReturnValue_t handleMemoryCommand(CommandMessage* message);
+    void completeLoad(ReturnValue_t errorCode,
+            const uint8_t* dataToCopy = nullptr, const size_t size = 0,
+            uint8_t* copyHere = nullptr);
+    void completeDump(ReturnValue_t errorCode,
+            const uint8_t* dataToCopy = nullptr, const size_t size = 0);
+    void swapMatrixCopy(uint8_t *out, const uint8_t *in, size_t totalSize,
+            uint8_t datatypeSize);
+    ReturnValue_t initialize(MessageQueueIF* queueToUse_);
+
 private:
 	HasMemoryIF* workOnThis;
 	MessageQueueIF* queueToUse;
-	StorageManagerIF* ipcStore;
+	StorageManagerIF* ipcStore = nullptr;
 	store_address_t ipcAddress;
 	Command_t lastCommand;
-	MessageQueueId_t lastSender;
-	uint8_t* reservedSpaceInIPC;
+	MessageQueueId_t lastSender = MessageQueueIF::NO_QUEUE;
+	uint8_t* reservedSpaceInIPC = nullptr;
 	bool busy;
 	void handleMemoryLoad(CommandMessage* message);
 	void handleMemoryCheckOrDump(CommandMessage* message);
 	ReturnValue_t initialize();
-public:
-	ReturnValue_t handleMemoryCommand(CommandMessage* message);
-	void completeLoad( ReturnValue_t errorCode, const uint8_t* dataToCopy = NULL, const uint32_t size = 0, uint8_t* copyHere = NULL );
-	void completeDump(  ReturnValue_t errorCode, const uint8_t* dataToCopy = NULL, const uint32_t size = 0);
-	void swapMatrixCopy( uint8_t *out, const uint8_t *in, uint32_t totalSize, uint8_t datatypeSize);
-	ReturnValue_t initialize(MessageQueueIF* queueToUse_);
-	MemoryHelper( HasMemoryIF* workOnThis, MessageQueueIF* useThisQueue );
-	~MemoryHelper();
+
 };
-#endif /* MEMORYHELPER_H_ */
+#endif /* FSFW_MEMORY_MEMORYHELPER_H_ */
--- a/memory/MemoryMessage.cpp
+++ b/memory/MemoryMessage.cpp
@ -1,7 +1,6 @@
 #include "MemoryMessage.h"
+
 #include "../objectmanager/ObjectManagerIF.h"
-MemoryMessage::MemoryMessage() {
-}

 uint32_t MemoryMessage::getAddress(const CommandMessage* message) {
 	return message->getParameter();
@ -17,26 +16,24 @@ uint32_t MemoryMessage::getLength(const CommandMessage* message) {
 	return message->getParameter2();
 }

-ReturnValue_t MemoryMessage::setMemoryDumpCommand(CommandMessage* message,
+void MemoryMessage::setMemoryDumpCommand(CommandMessage* message,
 		uint32_t address, uint32_t length) {
 	message->setCommand(CMD_MEMORY_DUMP);
 	message->setParameter( address );
 	message->setParameter2( length );
-	return HasReturnvaluesIF::RETURN_OK;
 }

-ReturnValue_t MemoryMessage::setMemoryDumpReply(CommandMessage* message, store_address_t storageID) {
+void MemoryMessage::setMemoryDumpReply(CommandMessage* message,
+        store_address_t storageID) {
 	message->setCommand(REPLY_MEMORY_DUMP);
 	message->setParameter2( storageID.raw );
-	return HasReturnvaluesIF::RETURN_OK;
 }

-ReturnValue_t MemoryMessage::setMemoryLoadCommand(CommandMessage* message,
+void MemoryMessage::setMemoryLoadCommand(CommandMessage* message,
 		uint32_t address, store_address_t storageID) {
 	message->setCommand(CMD_MEMORY_LOAD);
 	message->setParameter( address );
 	message->setParameter2( storageID.raw );
-	return HasReturnvaluesIF::RETURN_OK;
 }

 ReturnValue_t MemoryMessage::getErrorCode(const CommandMessage* message) {
@ -65,22 +62,21 @@ void MemoryMessage::clear(CommandMessage* message) {
 		}
 }

-ReturnValue_t MemoryMessage::setMemoryCheckCommand(CommandMessage* message,
+void MemoryMessage::setMemoryCheckCommand(CommandMessage* message,
 		uint32_t address, uint32_t length) {
 	message->setCommand(CMD_MEMORY_CHECK);
 	message->setParameter( address );
 	message->setParameter2( length );
-	return HasReturnvaluesIF::RETURN_OK;
 }

-ReturnValue_t MemoryMessage::setMemoryCheckReply(CommandMessage* message,
+void MemoryMessage::setMemoryCheckReply(CommandMessage* message,
 		uint16_t crc) {
 	message->setCommand(REPLY_MEMORY_CHECK);
 	message->setParameter( crc );
-	return HasReturnvaluesIF::RETURN_OK;
 }

-void MemoryMessage::setCrcReturnValue(CommandMessage* message, ReturnValue_t returnValue){
+void MemoryMessage::setCrcReturnValue(CommandMessage* message,
+        ReturnValue_t returnValue){
 	message->setParameter(returnValue<<16);
 };

@ -96,18 +92,16 @@ Command_t MemoryMessage::getInitialCommand(const CommandMessage* message) {
 	return message->getParameter2();
 }

-ReturnValue_t MemoryMessage::setMemoryReplyFailed(CommandMessage* message,
+void MemoryMessage::setMemoryReplyFailed(CommandMessage* message,
 		ReturnValue_t errorCode, Command_t initialCommand) {
 	message->setCommand(REPLY_MEMORY_FAILED);
 	message->setParameter(errorCode);
 	message->setParameter2(initialCommand);
-	return HasReturnvaluesIF::RETURN_OK;
 }

-ReturnValue_t MemoryMessage::setMemoryCopyEnd(CommandMessage* message) {
+void MemoryMessage::setMemoryCopyEnd(CommandMessage* message) {
 	message->setCommand(END_OF_MEMORY_COPY);
 	message->setParameter(0);
 	message->setParameter2(0);
-	return HasReturnvaluesIF::RETURN_OK;
 }

--- a/memory/MemoryMessage.h
+++ b/memory/MemoryMessage.h
@ -1,14 +1,15 @@
-#ifndef MEMORYMESSAGE_H_
-#define MEMORYMESSAGE_H_
+#ifndef FSFW_MEMORY_MEMORYMESSAGE_H_
+#define FSFW_MEMORY_MEMORYMESSAGE_H_

 #include "../ipc/CommandMessage.h"
 #include "../storagemanager/StorageManagerIF.h"


 class MemoryMessage {
-private:
-	MemoryMessage(); //A private ctor inhibits instantiation
 public:
+    // Instantiation forbidden.
+	MemoryMessage() = delete;
+
 	static const uint8_t MESSAGE_ID = messagetypes::MEMORY;
 	static const Command_t CMD_MEMORY_LOAD = MAKE_COMMAND_ID( 0x01 );
 	static const Command_t CMD_MEMORY_DUMP = MAKE_COMMAND_ID( 0x02 );
@ -19,21 +20,29 @@ public:
 	static const Command_t END_OF_MEMORY_COPY = MAKE_COMMAND_ID(0xF0);

 	static uint32_t getAddress( const CommandMessage* message );
-	static store_address_t getStoreID( const CommandMessage* message );
+	static store_address_t getStoreID(const CommandMessage* message);
 	static uint32_t getLength( const CommandMessage* message );
-	static ReturnValue_t getErrorCode( const CommandMessage* message );
-	static ReturnValue_t setMemoryDumpCommand( CommandMessage* message, uint32_t address, uint32_t length );
-	static ReturnValue_t setMemoryDumpReply( CommandMessage* message, store_address_t storageID );
-	static ReturnValue_t setMemoryLoadCommand( CommandMessage* message, uint32_t address, store_address_t storageID );
-	static ReturnValue_t setMemoryCheckCommand( CommandMessage* message, uint32_t address, uint32_t length );
-	static ReturnValue_t setMemoryCheckReply(  CommandMessage* message, uint16_t crc );
-	static ReturnValue_t setMemoryReplyFailed(  CommandMessage* message, ReturnValue_t errorCode, Command_t initialCommand );
-	static ReturnValue_t setMemoryCopyEnd( CommandMessage* message);
+	static ReturnValue_t getErrorCode(const CommandMessage* message);
+    static uint16_t getCrc(const CommandMessage* message );
+    static ReturnValue_t getCrcReturnValue(const CommandMessage* message);
+    static Command_t getInitialCommand(const CommandMessage* message);
+
+	static void setMemoryDumpCommand(CommandMessage* message,
+	        uint32_t address, uint32_t length );
+	static void setMemoryDumpReply(CommandMessage* message,
+	        store_address_t storageID);
+	static void setMemoryLoadCommand(CommandMessage* message,
+	        uint32_t address, store_address_t storageID );
+	static void setMemoryCheckCommand(CommandMessage* message,
+	        uint32_t address, uint32_t length);
+	static void setMemoryCheckReply(CommandMessage* message,
+	        uint16_t crc);
+	static void setMemoryReplyFailed(CommandMessage* message,
+	        ReturnValue_t errorCode, Command_t initialCommand);
+	static void setMemoryCopyEnd(CommandMessage* message);
 	static void setCrcReturnValue(CommandMessage*, ReturnValue_t returnValue);
-	static uint16_t getCrc( const CommandMessage* message );
-	static ReturnValue_t getCrcReturnValue(const CommandMessage* message);
-	static Command_t getInitialCommand( const CommandMessage* message );
+
 	static void clear(CommandMessage* message);
 };

-#endif /* MEMORYMESSAGE_H_ */
+#endif /* FSFW_MEMORY_MEMORYMESSAGE_H_ */
--- a/memory/MemoryProxyIF.h
+++ b/memory/MemoryProxyIF.h
@ -1,22 +0,0 @@
-#ifndef FRAMEWORK_MEMORY_MEMORYPROXYIF_H_
-#define FRAMEWORK_MEMORY_MEMORYPROXYIF_H_
-
-#include "AcceptsMemoryMessagesIF.h"
-
-/**
- * This was a nice idea to transparently forward incoming messages to another object.
- * But it doesn't work like that.
- */
-class MemoryProxyIF : public AcceptsMemoryMessagesIF {
-public:
-	virtual MessageQueueId_t getProxyQueue() const = 0;
-	MessageQueueId_t getCommandQueue() const {
-		return getProxyQueue();
-	}
-	virtual ~MemoryProxyIF() {}
-
-};
-
-
-
-#endif /* FRAMEWORK_MEMORY_MEMORYPROXYIF_H_ */
--- a/modes/HasModesIF.h
+++ b/modes/HasModesIF.h
@ -1,18 +1,11 @@
-/**
- * @file	HasModesIF.h
- * @brief	This file defines the HasModesIF class.
- * @date	20.06.2013
- * @author	baetz
- */
+#ifndef FSFW_MODES_HASMODESIF_H_
+#define FSFW_MODES_HASMODESIF_H_

-#ifndef HASMODESIF_H_
-#define HASMODESIF_H_
-
-#include "../events/Event.h"
 #include "ModeHelper.h"
 #include "ModeMessage.h"
+#include "../events/Event.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
-#include <stdint.h>
+#include <cstdint>


 class HasModesIF {
@ -37,21 +30,22 @@ public:
 	static const Mode_t MODE_ON = 1; //!< The device is powered and ready to perform operations. In this mode, no commands are sent by the device handler itself, but direct commands van be commanded and will be interpreted
 	static const Mode_t MODE_OFF = 0; //!< The device is powered off. The only command accepted in this mode is a mode change to on.
 	static const Submode_t SUBMODE_NONE = 0; //!< To avoid checks against magic number "0".
-	virtual ~HasModesIF() {
-	}
+
+	virtual ~HasModesIF() {}
 	virtual MessageQueueId_t getCommandQueue() const = 0;
 	virtual void getMode(Mode_t *mode, Submode_t *submode) = 0;
+
 protected:
 	virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
 			uint32_t *msToReachTheMode) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
-	virtual void startTransition(Mode_t mode, Submode_t submode) {
-	}
-	virtual void setToExternalControl() {
-	}
-	virtual void announceMode(bool recursive) {
-	}
+
+	virtual void startTransition(Mode_t mode, Submode_t submode) {}
+
+	virtual void setToExternalControl() {}
+
+	virtual void announceMode(bool recursive) {}
 };

-#endif /* HASMODESIF_H_ */
+#endif /*FSFW_MODES_HASMODESIF_H_ */
--- a/modes/ModeHelper.cpp
+++ b/modes/ModeHelper.cpp
@ -1,38 +1,39 @@
 #include "HasModesIF.h"
 #include "ModeHelper.h"
+
 #include "../ipc/MessageQueueSenderIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"

 ModeHelper::ModeHelper(HasModesIF *owner) :
-		theOneWhoCommandedAMode(0), commandedMode(HasModesIF::MODE_OFF), commandedSubmode(
-				HasModesIF::SUBMODE_NONE), owner(owner), parentQueueId(0), forced(
-				false) {
-}
+		commandedMode(HasModesIF::MODE_OFF),
+		commandedSubmode(HasModesIF::SUBMODE_NONE),
+		owner(owner), forced(false) {}

 ModeHelper::~ModeHelper() {

 }

-ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* message) {
+ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* command) {
 	CommandMessage reply;
 	Mode_t mode;
 	Submode_t submode;
-	switch (message->getCommand()) {
+	switch (command->getCommand()) {
 	case ModeMessage::CMD_MODE_COMMAND_FORCED:
 		forced = true;
 		/* NO BREAK falls through*/
 	case ModeMessage::CMD_MODE_COMMAND: {
-		mode = ModeMessage::getMode(message);
-		submode = ModeMessage::getSubmode(message);
+		mode = ModeMessage::getMode(command);
+		submode = ModeMessage::getSubmode(command);
 		uint32_t timeout;
 		ReturnValue_t result = owner->checkModeCommand(mode, submode, &timeout);
 		if (result != HasReturnvaluesIF::RETURN_OK) {
-			ModeMessage::cantReachMode(&reply, result);
-			MessageQueueSenderIF::sendMessage(message->getSender(), &reply,
-					owner->getCommandQueue());
+			ModeMessage::setCantReachMode(&reply, result);
+			MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
+			        owner->getCommandQueue());
 			break;
 		}
 		//Free to start transition
-		theOneWhoCommandedAMode = message->getSender();
+		theOneWhoCommandedAMode = command->getSender();
 		commandedMode = mode;
 		commandedSubmode = submode;

@ -42,7 +43,6 @@ ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* message) {
 		}

 		countdown.setTimeout(timeout);
-
 		owner->startTransition(mode, submode);
 	}
 		break;
@ -50,8 +50,8 @@ ReturnValue_t ModeHelper::handleModeCommand(CommandMessage* message) {
 		owner->getMode(&mode, &submode);
 		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY, mode,
 				submode);
-		MessageQueueSenderIF::sendMessage(message->getSender(), &reply,
-				owner->getCommandQueue());
+		MessageQueueSenderIF::sendMessage(command->getSender(), &reply,
+		       owner->getCommandQueue());
 	}
 		break;
 	case ModeMessage::CMD_MODE_ANNOUNCE:
@ -71,27 +71,45 @@ ReturnValue_t ModeHelper::initialize(MessageQueueId_t parentQueueId) {
 	return initialize();
 }

-void ModeHelper::modeChanged(Mode_t mode, Submode_t submode) {
+void ModeHelper::modeChanged(Mode_t ownerMode, Submode_t ownerSubmode) {
 	forced = false;
+	sendModeReplyMessage(ownerMode, ownerSubmode);
+	sendModeInfoMessage(ownerMode, ownerSubmode);
+	theOneWhoCommandedAMode = MessageQueueIF::NO_QUEUE;
+}
+
+void ModeHelper::sendModeReplyMessage(Mode_t ownerMode,
+		Submode_t ownerSubmode) {
 	CommandMessage reply;
-	if (theOneWhoCommandedAMode != MessageQueueIF::NO_QUEUE) {
-		if ((mode != commandedMode) || (submode != commandedSubmode)) {
+	if (theOneWhoCommandedAMode != MessageQueueIF::NO_QUEUE)
+	{
+		if (ownerMode != commandedMode or ownerSubmode != commandedSubmode)
+		{
 			ModeMessage::setModeMessage(&reply,
-					ModeMessage::REPLY_WRONG_MODE_REPLY, mode, submode);
-		} else {
+					ModeMessage::REPLY_WRONG_MODE_REPLY, ownerMode,
+					ownerSubmode);
+		}
+		else
+		{
 			ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_REPLY,
-					mode, submode);
+					ownerMode, ownerSubmode);
 		}
 		MessageQueueSenderIF::sendMessage(theOneWhoCommandedAMode, &reply,
-				owner->getCommandQueue());
+		        owner->getCommandQueue());
 	}
+}
+
+void ModeHelper::sendModeInfoMessage(Mode_t ownerMode,
+		Submode_t ownerSubmode) {
+	CommandMessage reply;
 	if (theOneWhoCommandedAMode != parentQueueId
-			&& parentQueueId != MessageQueueIF::NO_QUEUE) {
-		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO, mode,
-				submode);
-		MessageQueueSenderIF::sendMessage(parentQueueId, &reply, owner->getCommandQueue());
+			and parentQueueId != MessageQueueIF::NO_QUEUE)
+	{
+		ModeMessage::setModeMessage(&reply, ModeMessage::REPLY_MODE_INFO,
+				ownerMode, ownerSubmode);
+		MessageQueueSenderIF::sendMessage(parentQueueId, &reply,
+		        owner->getCommandQueue());
 	}
-	theOneWhoCommandedAMode = MessageQueueIF::NO_QUEUE;
 }

 void ModeHelper::startTimer(uint32_t timeoutMs) {
--- a/modes/ModeHelper.h
+++ b/modes/ModeHelper.h
@ -1,7 +1,8 @@
-#ifndef MODEHELPER_H_
-#define MODEHELPER_H_
+#ifndef FSFW_MODES_MODEHELPER_H_
+#define FSFW_MODES_MODEHELPER_H_

 #include "ModeMessage.h"
+#include "../ipc/MessageQueueIF.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "../timemanager/Countdown.h"

@ -9,7 +10,7 @@ class HasModesIF;

 class ModeHelper {
 public:
-	MessageQueueId_t theOneWhoCommandedAMode;
+	MessageQueueId_t theOneWhoCommandedAMode = MessageQueueIF::NO_QUEUE;
 	Mode_t commandedMode;
 	Submode_t commandedSubmode;

@ -19,14 +20,14 @@ public:
 	ReturnValue_t handleModeCommand(CommandMessage *message);

 	/**
-	 *
-	 * @param parentQueue the Queue id of the parent object. Set to 0 if no parent present
+	 * @param parentQueue the Queue id of the parent object.
+	 * Set to 0 if no parent present
 	 */
 	void setParentQueue(MessageQueueId_t parentQueueId);

 	ReturnValue_t initialize(MessageQueueId_t parentQueueId);

-	ReturnValue_t initialize(void); //void is there to stop eclipse CODAN from falsely reporting an error
+	ReturnValue_t initialize(void);

 	void modeChanged(Mode_t mode, Submode_t submode);

@ -39,11 +40,14 @@ public:
 	void setForced(bool forced);
 protected:
 	HasModesIF *owner;
-	MessageQueueId_t parentQueueId;
+	MessageQueueId_t parentQueueId = MessageQueueIF::NO_QUEUE;

 	Countdown countdown;

 	bool forced;
+private:
+	void sendModeReplyMessage(Mode_t ownerMode, Submode_t ownerSubmode);
+	void sendModeInfoMessage(Mode_t ownerMode, Submode_t ownerSubmode);
 };

-#endif /* MODEHELPER_H_ */
+#endif /* FSFW_MODES_MODEHELPER_H_ */
--- a/modes/ModeMessage.cpp
+++ b/modes/ModeMessage.cpp
@ -8,12 +8,11 @@ Submode_t ModeMessage::getSubmode(const CommandMessage* message) {
 	return message->getParameter2();
 }

-ReturnValue_t ModeMessage::setModeMessage(CommandMessage* message, Command_t command,
-		Mode_t mode, Submode_t submode) {
+void ModeMessage::setModeMessage(CommandMessage* message,
+        Command_t command, Mode_t mode, Submode_t submode) {
 	message->setCommand( command );
 	message->setParameter( mode );
 	message->setParameter2( submode );
-	return HasReturnvaluesIF::RETURN_OK;
 }

 ReturnValue_t ModeMessage::getCantReachModeReason(const CommandMessage* message) {
@ -24,7 +23,8 @@ void ModeMessage::clear(CommandMessage* message) {
 	message->setCommand(CommandMessage::CMD_NONE);
 }

-void ModeMessage::cantReachMode(CommandMessage* message, ReturnValue_t reason) {
+void ModeMessage::setCantReachMode(CommandMessage* message,
+        ReturnValue_t reason) {
 	message->setCommand(REPLY_CANT_REACH_MODE);
 	message->setParameter(reason);
 	message->setParameter2(0);
--- a/modes/ModeMessage.h
+++ b/modes/ModeMessage.h
@ -1,12 +1,5 @@
-/**
- * @file	ModeMessage.h
- * @brief	This file defines the ModeMessage class.
- * @date	17.07.2013
- * @author	baetz
- */
-
-#ifndef MODEMESSAGE_H_
-#define MODEMESSAGE_H_
+#ifndef FSFW_MODES_MODEMESSAGE_H_
+#define FSFW_MODES_MODEMESSAGE_H_

 #include "../ipc/CommandMessage.h"

@ -30,11 +23,12 @@ public:

 	static Mode_t getMode(const CommandMessage* message);
 	static Submode_t getSubmode(const CommandMessage* message);
-	static ReturnValue_t setModeMessage(CommandMessage* message,
+    static ReturnValue_t getCantReachModeReason(const CommandMessage* message);
+
+	static void setModeMessage(CommandMessage* message,
 			Command_t command, Mode_t mode, Submode_t submode);
-	static void cantReachMode(CommandMessage* message, ReturnValue_t reason);
-	static ReturnValue_t getCantReachModeReason(const CommandMessage* message);
+	static void setCantReachMode(CommandMessage* message, ReturnValue_t reason);
 	static void clear(CommandMessage* message);
 };

-#endif /* MODEMESSAGE_H_ */
+#endif /* FSFW_MODES_MODEMESSAGE_H_ */
--- a/objectmanager/frameworkObjects.h
+++ b/objectmanager/frameworkObjects.h
@ -4,13 +4,13 @@
 namespace objects {
 enum framework_objects {
 	// Default verification reporter.
-	PUS_SERVICE_1 = 0x53000001,
-	PUS_SERVICE_2 = 0x53000002,
-	PUS_SERVICE_5 = 0x53000005,
-	PUS_SERVICE_8 = 0x53000008,
-	PUS_SERVICE_9 = 0x53000009,
-	PUS_SERVICE_17 = 0x53000017,
-	PUS_SERVICE_200 = 0x53000200,
+	PUS_SERVICE_1_VERIFICATION = 0x53000001,
+	PUS_SERVICE_2_DEVICE_ACCESS = 0x53000002,
+	PUS_SERVICE_5_EVENT_REPORTING = 0x53000005,
+	PUS_SERVICE_8_FUNCTION_MGMT = 0x53000008,
+	PUS_SERVICE_9_TIME_MGMT = 0x53000009,
+	PUS_SERVICE_17_TEST = 0x53000017,
+	PUS_SERVICE_200_MODE_MGMT = 0x53000200,

 	//Generic IDs for IPC, modes, health, events
 	HEALTH_TABLE = 0x53010000,
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@ -8,7 +8,7 @@ const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = configMINIMAL_STACK_SIZE;
 FixedTimeslotTask::FixedTimeslotTask(TaskName name, TaskPriority setPriority,
 		TaskStackSize setStack, TaskPeriod overallPeriod,
 		void (*setDeadlineMissedFunc)()) :
-		started(false), handle(NULL), pst(overallPeriod * 1000) {
+		started(false), handle(nullptr), pst(overallPeriod * 1000) {
 	configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE);
 	xTaskCreate(taskEntryPoint, name, stackSize, this, setPriority, &handle);
 	// All additional attributes are applied to the object.
@ -62,8 +62,10 @@ ReturnValue_t FixedTimeslotTask::startTask() {

 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		uint32_t slotTimeMs, int8_t executionStep) {
-	if (objectManager->get<ExecutableObjectIF>(componentId) != nullptr) {
-		pst.addSlot(componentId, slotTimeMs, executionStep, this);
+    ExecutableObjectIF* handler =
+            objectManager->get<ExecutableObjectIF>(componentId);
+	if (handler != nullptr) {
+		pst.addSlot(componentId, slotTimeMs, executionStep, handler, this);
 		return HasReturnvaluesIF::RETURN_OK;
 	}

@ -85,6 +87,8 @@ void FixedTimeslotTask::taskFunctionality() {
    // start time for the first entry.
 	auto slotListIter = pst.current;

+	pst.intializeSequenceAfterTaskCreation();
+
 	//The start time for the first entry is read.
 	uint32_t intervalMs = slotListIter->pollingTimeMs;
 	TickType_t interval = pdMS_TO_TICKS(intervalMs);
@ -143,10 +147,6 @@ void FixedTimeslotTask::checkMissedDeadline(const TickType_t xLastWakeTime,
 }

 void FixedTimeslotTask::handleMissedDeadline() {
-#ifdef DEBUG
-    sif::warning << "FixedTimeslotTask: " << pcTaskGetName(NULL) <<
-            " missed deadline!\n" << std::flush;
-#endif
    if(deadlineMissedFunc != nullptr) {
        this->deadlineMissedFunc();
    }
--- a/osal/FreeRTOS/FixedTimeslotTask.h
+++ b/osal/FreeRTOS/FixedTimeslotTask.h
@ -1,12 +1,11 @@
-#ifndef FRAMEWORK_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_
-#define FRAMEWORK_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_
+#ifndef FSFW_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_
+#define FSFW_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_

 #include "FreeRTOSTaskIF.h"
-#include "../../devicehandlers/FixedSlotSequence.h"
+#include "../../tasks/FixedSlotSequence.h"
 #include "../../tasks/FixedTimeslotTaskIF.h"
 #include "../../tasks/Typedef.h"

-
 #include <freertos/FreeRTOS.h>
 #include <freertos/task.h>

@ -99,4 +98,4 @@ protected:
 	void handleMissedDeadline();
 };

-#endif /* FRAMEWORK_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_ */
+#endif /* FSFW_OSAL_FREERTOS_FIXEDTIMESLOTTASK_H_ */
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@ -133,10 +133,6 @@ TaskHandle_t PeriodicTask::getTaskHandle() {
 }

 void PeriodicTask::handleMissedDeadline() {
-#ifdef DEBUG
-    sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
-            " missed deadline!\n" << std::flush;
-#endif
    if(deadlineMissedFunc != nullptr) {
        this->deadlineMissedFunc();
    }
--- a/osal/linux/FixedTimeslotTask.cpp
+++ b/osal/linux/FixedTimeslotTask.cpp
@ -1,5 +1,5 @@
-#include "../../serviceinterface/ServiceInterfaceStream.h"
 #include "FixedTimeslotTask.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"

 #include <limits.h>

@ -39,13 +39,16 @@ uint32_t FixedTimeslotTask::getPeriodMs() const {

 ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		uint32_t slotTimeMs, int8_t executionStep) {
-	if (objectManager->get<ExecutableObjectIF>(componentId) != nullptr) {
-		pst.addSlot(componentId, slotTimeMs, executionStep, this);
+	ExecutableObjectIF* executableObject =
+			objectManager->get<ExecutableObjectIF>(componentId);
+	if (executableObject != nullptr) {
+		pst.addSlot(componentId, slotTimeMs, executionStep,
+				executableObject,this);
 		return HasReturnvaluesIF::RETURN_OK;
 	}

 	sif::error << "Component " << std::hex << componentId <<
-			" not found, not adding it to pst" << std::endl;
+			" not found, not adding it to pst" << std::dec << std::endl;
 	return HasReturnvaluesIF::RETURN_FAILED;
 }

@ -58,6 +61,9 @@ void FixedTimeslotTask::taskFunctionality() {
 	if (!started) {
 		suspend();
 	}
+
+	pst.intializeSequenceAfterTaskCreation();
+
 	//The start time for the first entry is read.
 	uint64_t lastWakeTime = getCurrentMonotonicTimeMs();
 	uint64_t interval = pst.getIntervalToNextSlotMs();
--- a/osal/linux/FixedTimeslotTask.h
+++ b/osal/linux/FixedTimeslotTask.h
@ -1,9 +1,9 @@
-#ifndef FRAMEWORK_OSAL_LINUX_FIXEDTIMESLOTTASK_H_
-#define FRAMEWORK_OSAL_LINUX_FIXEDTIMESLOTTASK_H_
+#ifndef FSFW_OSAL_LINUX_FIXEDTIMESLOTTASK_H_
+#define FSFW_OSAL_LINUX_FIXEDTIMESLOTTASK_H_

-#include "../../tasks/FixedTimeslotTaskIF.h"
-#include "../../devicehandlers/FixedSlotSequence.h"
 #include "PosixThread.h"
+#include "../../tasks/FixedTimeslotTaskIF.h"
+#include "../../tasks/FixedSlotSequence.h"
 #include <pthread.h>

 class FixedTimeslotTask: public FixedTimeslotTaskIF, public PosixThread {
@ -74,4 +74,4 @@ private:
 	bool started;
 };

-#endif /* FRAMEWORK_OSAL_LINUX_FIXEDTIMESLOTTASK_H_ */
+#endif /* FSFW_OSAL_LINUX_FIXEDTIMESLOTTASK_H_ */
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@ -1,5 +1,6 @@
-#include "../../serviceinterface/ServiceInterfaceStream.h"
 #include "MessageQueue.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManagerIF.h"

 #include <fstream>

@ -9,9 +10,11 @@
 #include <errno.h>


+
 MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
 		id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
-		defaultDestination(MessageQueueIF::NO_QUEUE) {
+		defaultDestination(MessageQueueIF::NO_QUEUE),
+		maxMessageSize(maxMessageSize) {
 	//debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
 	mq_attr attributes;
 	this->id = 0;
@ -46,7 +49,7 @@ MessageQueue::~MessageQueue() {
 	status = mq_unlink(name);
 	if(status != 0){
 		sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: "
-				   << strerror(errno) <<std::endl;
+				   << strerror(errno) << std::endl;
 	}
 }

@ -61,22 +64,27 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 		// Just an additional helpful printout :-)
 		if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
 				defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
-			// See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
-			// This happens if the msg_max value is not large enough
-			// It is ignored if the executable is run in privileged mode.
-			// Run the unlockRealtime script or grant the mode manually by using:
-			// sudo setcap 'CAP_SYS_RESOURCE=+ep' <pathToBinary>
+			/*
+			See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
+			This happens if the msg_max value is not large enough
+			It is ignored if the executable is run in privileged mode.
+		    Run the unlockRealtime script or grant the mode manually by using:
+			sudo setcap 'CAP_SYS_RESOURCE=+ep' <pathToBinary>

-			// Persistent solution for session:
-			// echo <newMsgMax> | sudo tee /proc/sys/fs/mqueue/msg_max
+			Persistent solution for session:
+			echo <newMsgMax> | sudo tee /proc/sys/fs/mqueue/msg_max

-			// Permanent solution:
-			// sudo nano /etc/sysctl.conf
-			// Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
-			// Apply changes with: sudo sysctl -p
+			Permanent solution:
+			sudo nano /etc/sysctl.conf
+			Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
+			Apply changes with: sudo sysctl -p
+			*/
 			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;
+
 		}
 		break;
 	}
@ -118,15 +126,15 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes,
 }

 ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
-		MessageQueueMessage* message, bool ignoreFault) {
+		MessageQueueMessageIF* message, bool ignoreFault) {
 	return sendMessageFrom(sendTo, message, this->getId(), false);
 }

-ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) {
+ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
 	return sendToDefaultFrom(message, this->getId());
 }

-ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
+ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
 	if (this->lastPartner != 0) {
 		return sendMessageFrom(this->lastPartner, message, this->getId());
 	} else {
@ -134,21 +142,34 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
 	}
 }

-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
+ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
 		MessageQueueId_t* receivedFrom) {
 	ReturnValue_t status = this->receiveMessage(message);
 	*receivedFrom = this->lastPartner;
 	return status;
 }

-ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
+ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
+	if(message == nullptr) {
+		sif::error << "MessageQueue::receiveMessage: Message is "
+				"nullptr!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
+	if(message->getMaximumMessageSize() < maxMessageSize) {
+		sif::error << "MessageQueue::receiveMessage: Message size "
+				<< message->getMaximumMessageSize()
+				<< " too small to receive data!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
 	unsigned int messagePriority = 0;
 	int status = mq_receive(id,reinterpret_cast<char*>(message->getBuffer()),
-			message->MAX_MESSAGE_SIZE,&messagePriority);
+			message->getMaximumMessageSize(),&messagePriority);
 	if (status > 0) {
 		this->lastPartner = message->getSender();
 		//Check size of incoming message.
-		if (message->messageSize < message->getMinimumMessageSize()) {
+		if (message->getMessageSize() < message->getMinimumMessageSize()) {
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
 		return HasReturnvaluesIF::RETURN_OK;
@ -158,7 +179,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) {
 	} else {
 		//No message was received. Keep lastPartner anyway, I might send
 		//something later. But still, delete packet content.
-		memset(message->getData(), 0, message->MAX_DATA_SIZE);
+		memset(message->getData(), 0, message->getMaximumMessageSize());
 		switch(errno){
 		case EAGAIN:
 			//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
@ -258,18 +279,19 @@ void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
 	this->defaultDestination = defaultDestination;
 }

-ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
-		MessageQueueMessage* message, MessageQueueId_t sentFrom,
-		bool ignoreFault) {
-	return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
-
-}
-
-ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message,
+ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
 		MessageQueueId_t sentFrom, bool ignoreFault) {
 	return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
 }

+
+ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
+		MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+		bool ignoreFault) {
+	return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault);
+
+}
+
 MessageQueueId_t MessageQueue::getDefaultDestination() const {
 	return this->defaultDestination;
 }
@ -281,11 +303,18 @@ bool MessageQueue::isDefaultDestinationSet() const {
 uint16_t MessageQueue::queueCounter = 0;

 ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
-		MessageQueueMessage *message, MessageQueueId_t sentFrom,
+		MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
 		bool ignoreFault) {
+	if(message == nullptr) {
+		sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is "
+				"nullptr!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+
 	message->setSender(sentFrom);
 	int result = mq_send(sendTo,
-			reinterpret_cast<const char*>(message->getBuffer()), message->messageSize,0);
+			reinterpret_cast<const char*>(message->getBuffer()),
+			message->getMessageSize(),0);

 	//TODO: Check if we're in ISR.
 	if (result != 0) {
@ -303,13 +332,16 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 			//MQ_NONBLOCK flag was set in its attributes, and the
 			//specified queue is full.
 			return MessageQueueIF::FULL;
-		case EBADF:
+		case EBADF: {
 			//mq_des doesn't represent a valid message queue descriptor,
 			//or mq_des wasn't opened for writing.
-			sif::error << "MessageQueue::sendMessage: Configuration error "
-			           << strerror(errno) << " in mq_send mqSendTo: " << sendTo
-					   << " sent from " << sentFrom << std::endl;
-			/*NO BREAK*/
+			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;
+			return DESTINVATION_INVALID;
+		}
 		case EINTR:
 			//The call was interrupted by a signal.
 		case EINVAL:
--- a/osal/linux/MessageQueue.h
+++ b/osal/linux/MessageQueue.h
@ -1,5 +1,5 @@
-#ifndef MESSAGEQUEUE_H_
-#define MESSAGEQUEUE_H_
+#ifndef FSFW_OSAL_LINUX_MESSAGEQUEUE_H_
+#define FSFW_OSAL_LINUX_MESSAGEQUEUE_H_

 #include "../../internalError/InternalErrorReporterIF.h"
 #include "../../ipc/MessageQueueIF.h"
@ -56,14 +56,14 @@ public:
 	 * @param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
 	 */
 	virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo,
-			MessageQueueMessage* message, bool ignoreFault = false );
+			MessageQueueMessageIF* message, bool ignoreFault = false );
 	/**
 	 * @brief	This operation sends a message to the default destination.
 	 * @details	As in the sendMessage method, this function uses the sendToDefault call of the
 	 * 			MessageQueueSender parent class and adds its queue id as "sentFrom" information.
 	 * @param message	A pointer to a previously created message, which is sent.
 	 */
-	virtual ReturnValue_t sendToDefault( MessageQueueMessage* message );
+	virtual ReturnValue_t sendToDefault( MessageQueueMessageIF* message );
 	/**
 	 * @brief	This operation sends a message to the last communication partner.
 	 * @details	This operation simplifies answering an incoming message by using the stored
@ -71,7 +71,7 @@ public:
 	 * 			(i.e. lastPartner is zero), an error code is returned.
 	 * @param message	A pointer to a previously created message, which is sent.
 	 */
-	ReturnValue_t reply( MessageQueueMessage* message );
+	ReturnValue_t reply( MessageQueueMessageIF* message );

 	/**
 	 * @brief	This function reads available messages from the message queue and returns the sender.
@ -80,7 +80,7 @@ public:
 	 * @param message	A pointer to a message in which the received data is stored.
 	 * @param receivedFrom	A pointer to a queue id in which the sender's id is stored.
 	 */
-	ReturnValue_t receiveMessage(MessageQueueMessage* message,
+	ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
 			MessageQueueId_t *receivedFrom);

 	/**
@ -91,7 +91,7 @@ public:
 	 * 			message's content is cleared and the function returns immediately.
 	 * @param message	A pointer to a message in which the received data is stored.
 	 */
-	ReturnValue_t receiveMessage(MessageQueueMessage* message);
+	ReturnValue_t receiveMessage(MessageQueueMessageIF* message);
 	/**
 	 * Deletes all pending messages in the queue.
 	 * @param count The number of flushed messages.
@ -114,7 +114,9 @@ public:
 	 * 					This variable is set to zero by default.
 	 * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
 	 */
-	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false );
+	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo,
+			MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+			bool ignoreFault = false );
 	/**
 	 * \brief	The sendToDefault method sends a queue message to the default destination.
 	 * \details	In all other aspects, it works identical to the sendMessage method.
@ -122,7 +124,8 @@ public:
 	 * \param sentFrom	The sentFrom information can be set to inject the sender's queue id into the message.
 	 * 					This variable is set to zero by default.
 	 */
-	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
+	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message,
+			MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false );
 	/**
 	 * \brief	This method is a simple setter for the default destination.
 	 */
@ -145,7 +148,9 @@ protected:
 	 * 					This variable is set to zero by default.
 	 * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full.
 	 */
-	static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false);
+	static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,
+			MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE,
+			bool ignoreFault=false);
 private:
 	/**
 	 * @brief	The class stores the queue id it got assigned from the operating system in this attribute.
@ -171,11 +176,12 @@ private:
 	/**
 	 * The name of the message queue, stored for unlinking
 	 */
-	char name[5];
+	char name[16];

 	static uint16_t queueCounter;
+	const size_t maxMessageSize;

 	ReturnValue_t handleError(mq_attr* attributes, uint32_t messageDepth);
 };

-#endif /* MESSAGEQUEUE_H_ */
+#endif /* FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ */
--- a/osal/linux/QueueFactory.cpp
+++ b/osal/linux/QueueFactory.cpp
@ -15,7 +15,7 @@ QueueFactory* QueueFactory::factoryInstance = nullptr;


 ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
-			MessageQueueMessage* message, MessageQueueId_t sentFrom,
+			MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
 			bool ignoreFault) {
 	return MessageQueue::sendMessageFromMessageQueue(sendTo,message,
 			sentFrom,ignoreFault);
--- a/storagemanager/ConstStorageAccessor.cpp
+++ b/storagemanager/ConstStorageAccessor.cpp
@ -0,0 +1,88 @@
+#include "ConstStorageAccessor.h"
+#include "StorageManagerIF.h"
+
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../globalfunctions/arrayprinter.h"
+
+ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId):
+		storeId(storeId) {}
+
+ConstStorageAccessor::ConstStorageAccessor(store_address_t storeId,
+		StorageManagerIF* store):
+		storeId(storeId), store(store) {
+	internalState = AccessState::ASSIGNED;
+}
+
+ConstStorageAccessor::~ConstStorageAccessor() {
+	if(deleteData and store != nullptr) {
+		store->deleteData(storeId);
+	}
+}
+
+ConstStorageAccessor::ConstStorageAccessor(ConstStorageAccessor&& other):
+		constDataPointer(other.constDataPointer), storeId(other.storeId),
+		size_(other.size_), store(other.store), deleteData(other.deleteData),
+		internalState(other.internalState) {
+	// This prevent premature deletion
+	other.store = nullptr;
+}
+
+ConstStorageAccessor& ConstStorageAccessor::operator=(
+		ConstStorageAccessor&& other) {
+	constDataPointer = other.constDataPointer;
+	storeId = other.storeId;
+	store = other.store;
+	size_ = other.size_;
+	deleteData = other.deleteData;
+	this->store = other.store;
+	// This prevents premature deletion
+	other.store = nullptr;
+	return *this;
+}
+
+const uint8_t* ConstStorageAccessor::data() const {
+	return constDataPointer;
+}
+
+size_t ConstStorageAccessor::size() const {
+	if(internalState == AccessState::UNINIT) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+	}
+	return size_;
+}
+
+ReturnValue_t ConstStorageAccessor::getDataCopy(uint8_t *pointer,
+		size_t maxSize) {
+	if(internalState == AccessState::UNINIT) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(size_ > maxSize) {
+		sif::error << "StorageAccessor: Supplied buffer not large enough"
+				<< std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(constDataPointer, constDataPointer + size_, pointer);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void ConstStorageAccessor::release() {
+	deleteData = false;
+}
+
+store_address_t ConstStorageAccessor::getId() const {
+	return storeId;
+}
+
+void ConstStorageAccessor::print() const {
+	if(internalState == AccessState::UNINIT or constDataPointer == nullptr) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+		return;
+	}
+	arrayprinter::print(constDataPointer, size_);
+}
+
+void ConstStorageAccessor::assignStore(StorageManagerIF* store) {
+	internalState = AccessState::ASSIGNED;
+	this->store = store;
+}
--- a/storagemanager/ConstStorageAccessor.h
+++ b/storagemanager/ConstStorageAccessor.h
@ -0,0 +1,116 @@
+#ifndef FSFW_STORAGEMANAGER_CONSTSTORAGEACCESSOR_H_
+#define FSFW_STORAGEMANAGER_CONSTSTORAGEACCESSOR_H_
+
+#include "storeAddress.h"
+#include "../returnvalues/HasReturnvaluesIF.h"
+#include <cstddef>
+
+class StorageManagerIF;
+
+/**
+ * @brief 	Helper classes to facilitate safe access to storages which is also
+ * 			conforming to RAII principles
+ * @details
+ * Accessor class which can be returned by pool manager or passed and set by
+ * pool managers to have safe access to the pool resources.
+ *
+ * These helper can be used together with the StorageManager classes to manage
+ * access to a storage. It can take care of thread-safety while also providing
+ * mechanisms to automatically clear storage data.
+ */
+class ConstStorageAccessor {
+	//! StorageManager classes have exclusive access to private variables.
+	template<uint8_t NUMBER_OF_POOLS>
+	friend class PoolManager;
+	template<uint8_t NUMBER_OF_POOLS>
+	friend class LocalPool;
+public:
+	/**
+	 * @brief	Simple constructor which takes the store ID of the storage
+	 * 			entry to access.
+	 * @param storeId
+	 */
+	ConstStorageAccessor(store_address_t storeId);
+	ConstStorageAccessor(store_address_t storeId, StorageManagerIF* store);
+
+	/**
+	 * @brief 	The destructor in default configuration takes care of
+	 * 			deleting the accessed pool entry and unlocking the mutex
+	 */
+	virtual ~ConstStorageAccessor();
+
+	/**
+	 * @brief	Returns a pointer to the read-only data
+	 * @return
+	 */
+	const uint8_t* data() const;
+
+	/**
+	 * @brief	Copies the read-only data to the supplied pointer
+	 * @param pointer
+	 */
+	virtual ReturnValue_t getDataCopy(uint8_t *pointer, size_t maxSize);
+
+	/**
+	 * @brief   Calling this will prevent the Accessor from deleting the data
+	 *          when the destructor is called.
+	 */
+	void release();
+
+	/**
+	 * Get the size of the data
+	 * @return
+	 */
+	size_t size() const;
+
+	/**
+	 * Get the storage ID.
+	 * @return
+	 */
+	store_address_t getId() const;
+
+	void print() const;
+
+	/**
+	 * @brief	Move ctor and move assignment allow returning accessors as
+	 * 			a returnvalue. They prevent resource being free prematurely.
+	 * Refer to: https://github.com/MicrosoftDocs/cpp-docs/blob/master/docs/cpp/
+	 * move-constructors-and-move-assignment-operators-cpp.md
+	 * @param
+	 * @return
+	 */
+	ConstStorageAccessor& operator= (ConstStorageAccessor&&);
+	ConstStorageAccessor(ConstStorageAccessor&&);
+
+	//! The copy ctor and copy assignemnt should be deleted implicitely
+	//! according to https://foonathan.net/2019/02/special-member-functions/
+	//! but I still deleted them to make it more explicit. (remember rule of 5).
+	ConstStorageAccessor& operator=(const ConstStorageAccessor&) = delete;
+	ConstStorageAccessor(const ConstStorageAccessor&) = delete;
+protected:
+	const uint8_t* constDataPointer = nullptr;
+	store_address_t storeId;
+	size_t size_ = 0;
+	//! Managing pool, has to assign itself.
+	StorageManagerIF* store = nullptr;
+	bool deleteData = true;
+
+	enum class AccessState {
+		UNINIT,
+		ASSIGNED
+	};
+	//! Internal state for safety reasons.
+	AccessState internalState = AccessState::UNINIT;
+	/**
+	 * Used by the pool manager instances to assign themselves to the
+	 * accessor. This is necessary to delete the data when the acessor
+	 * exits the scope ! The internal state will be considered read
+	 * when this function is called, so take care all data is set properly as
+	 * well.
+	 * @param
+	 */
+	void assignStore(StorageManagerIF*);
+};
+
+
+#endif /* FSFW_STORAGEMANAGER_CONSTSTORAGEACCESSOR_H_ */
--- a/storagemanager/LocalPool.h
+++ b/storagemanager/LocalPool.h
@ -1,18 +1,14 @@
-/**
- *	@file	LocalPool
- *  @date	02.02.2012
- *	@author	Bastian Baetz
- *	@brief	This file contains the definition of the LocalPool class.
- */
-#ifndef FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_
-#define FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_
+#ifndef FSFW_STORAGEMANAGER_LOCALPOOL_H_
+#define FSFW_STORAGEMANAGER_LOCALPOOL_H_

-#include "../objectmanager/SystemObject.h"
-#include "../serviceinterface/ServiceInterfaceStream.h"
 #include "StorageManagerIF.h"
+#include "../objectmanager/SystemObject.h"
 #include "../objectmanager/ObjectManagerIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
 #include "../internalError/InternalErrorReporterIF.h"
-#include <string.h>
+#include "../storagemanager/StorageAccessor.h"
+#include <cstring>
+

 /**
 * @brief	The LocalPool class provides an intermediate data storage with
@ -27,6 +23,7 @@
 * 			0xFFFF-1 bytes.
 * 			It is possible to store empty packets in the pool.
 * 			The local pool is NOT thread-safe.
+ * @author 	Bastian Baetz
 */
 template<uint8_t NUMBER_OF_POOLS = 5>
 class LocalPool: public SystemObject, public StorageManagerIF {
@ -39,7 +36,7 @@ public:
 	/**
 	 * @brief	This is the default constructor for a pool manager instance.
 	 * @details	By passing two arrays of size NUMBER_OF_POOLS, the constructor
-	 * 			allocates memory (with \c new) for store and size_list. These
+	 * 			allocates memory (with @c new) for store and size_list. These
 	 * 			regions are all set to zero on start up.
 	 * @param setObjectId	The object identifier to be set. This allows for
 	 * 						multiple instances of LocalPool in the system.
@ -73,10 +70,17 @@ public:
 			size_t size, bool ignoreFault = false) override;
 	ReturnValue_t getFreeElement(store_address_t* storageId,const size_t size,
 			uint8_t** p_data, bool ignoreFault = false) override;
+
+	ConstAccessorPair getData(store_address_t packet_id) override;
+	ReturnValue_t getData(store_address_t packet_id, ConstStorageAccessor&) override;
 	ReturnValue_t getData(store_address_t packet_id, const uint8_t** packet_ptr,
 			size_t * size) override;
+
+	AccessorPair modifyData(store_address_t packet_id) override;
+	ReturnValue_t modifyData(store_address_t packet_id, StorageAccessor&) override;
 	ReturnValue_t modifyData(store_address_t packet_id, uint8_t** packet_ptr,
 			size_t * size) override;
+
 	virtual ReturnValue_t deleteData(store_address_t) override;
 	virtual ReturnValue_t deleteData(uint8_t* ptr, size_t size,
 			store_address_t* storeId = NULL) override;
@ -84,7 +88,7 @@ public:
 	ReturnValue_t initialize() override;
 protected:
 	/**
-	 * With this helper method, a free element of \c size is reserved.
+	 * With this helper method, a free element of @c size is reserved.
 	 * @param size	The minimum packet size that shall be reserved.
 	 * @param[out] address Storage ID of the reserved data.
 	 * @return	- #RETURN_OK on success,
@ -97,7 +101,8 @@ protected:
 private:
 	/**
 	 * Indicates that this element is free.
-	 * This value limits the maximum size of a pool. Change to larger data type if increase is required.
+	 * This value limits the maximum size of a pool. Change to larger data type
+	 * if increase is required.
 	 */
 	static const uint32_t STORAGE_FREE = 0xFFFFFFFF;
 	/**
@ -123,7 +128,9 @@ private:
 	 * 			is also dynamically allocated there.
 	 */
 	uint32_t* size_list[NUMBER_OF_POOLS];
-	bool spillsToHigherPools; //!< A variable to determine whether higher n pools are used if the store is full.
+	//! A variable to determine whether higher n pools are used if
+	//! the store is full.
+	bool spillsToHigherPools;
 	/**
 	 * @brief	This method safely stores the given data in the given packet_id.
 	 * @details	It also sets the size in size_list. The method does not perform
@ -180,4 +187,4 @@ private:

 #include "LocalPool.tpp"

-#endif /* FRAMEWORK_STORAGEMANAGER_LOCALPOOL_H_ */
+#endif /* FSFW_STORAGEMANAGER_LOCALPOOL_H_ */
--- a/storagemanager/LocalPool.tpp
+++ b/storagemanager/LocalPool.tpp
@ -1,5 +1,9 @@
-#ifndef FRAMEWORK_STORAGEMANAGER_LOCALPOOL_TPP_
-#define FRAMEWORK_STORAGEMANAGER_LOCALPOOL_TPP_
+#ifndef FSFW_STORAGEMANAGER_LOCALPOOL_TPP_
+#define FSFW_STORAGEMANAGER_LOCALPOOL_TPP_
+
+#ifndef FSFW_STORAGEMANAGER_LOCALPOOL_H_
+#error Include LocalPool.h before LocalPool.tpp!
+#endif

 template<uint8_t NUMBER_OF_POOLS>
 inline LocalPool<NUMBER_OF_POOLS>::LocalPool(object_id_t setObjectId,
@ -122,8 +126,9 @@ inline LocalPool<NUMBER_OF_POOLS>::~LocalPool(void) {
 }

 template<uint8_t NUMBER_OF_POOLS>
-inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::addData(store_address_t* storageId,
-		const uint8_t* data, size_t size, bool ignoreFault) {
+inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::addData(
+		store_address_t* storageId, const uint8_t* data, size_t size,
+		bool ignoreFault) {
 	ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
 	if (status == RETURN_OK) {
 		write(*storageId, data, size);
@ -144,15 +149,55 @@ inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::getFreeElement(
 	return status;
 }

+template<uint8_t NUMBER_OF_POOLS>
+inline ConstAccessorPair LocalPool<NUMBER_OF_POOLS>::getData(
+		store_address_t storeId) {
+	uint8_t* tempData = nullptr;
+	ConstStorageAccessor constAccessor(storeId, this);
+	ReturnValue_t status = modifyData(storeId, &tempData, &constAccessor.size_);
+	constAccessor.constDataPointer = tempData;
+	return ConstAccessorPair(status, std::move(constAccessor));
+}
+
+template<uint8_t NUMBER_OF_POOLS>
+inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::getData(store_address_t storeId,
+		ConstStorageAccessor& storeAccessor) {
+	uint8_t* tempData = nullptr;
+	ReturnValue_t status = modifyData(storeId, &tempData, &storeAccessor.size_);
+	storeAccessor.assignStore(this);
+	storeAccessor.constDataPointer = tempData;
+	return status;
+}
+
 template<uint8_t NUMBER_OF_POOLS>
 inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::getData(
 		store_address_t packet_id, const uint8_t** packet_ptr, size_t* size) {
-	uint8_t* tempData = NULL;
+	uint8_t* tempData = nullptr;
 	ReturnValue_t status = modifyData(packet_id, &tempData, size);
 	*packet_ptr = tempData;
 	return status;
 }

+template<uint8_t NUMBER_OF_POOLS>
+inline AccessorPair LocalPool<NUMBER_OF_POOLS>::modifyData(
+		store_address_t storeId) {
+	StorageAccessor accessor(storeId, this);
+	ReturnValue_t status = modifyData(storeId, &accessor.dataPointer,
+			&accessor.size_);
+	accessor.assignConstPointer();
+	return AccessorPair(status, std::move(accessor));
+}
+
+template<uint8_t NUMBER_OF_POOLS>
+inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::modifyData(
+		store_address_t storeId, StorageAccessor& storeAccessor) {
+	storeAccessor.assignStore(this);
+	ReturnValue_t status = modifyData(storeId, &storeAccessor.dataPointer,
+			&storeAccessor.size_);
+	storeAccessor.assignConstPointer();
+	return status;
+}
+
 template<uint8_t NUMBER_OF_POOLS>
 inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::modifyData(
 		store_address_t packet_id, uint8_t** packet_ptr, size_t* size) {
@ -242,8 +287,8 @@ inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::initialize() {
 	}
 	internalErrorReporter = objectManager->get<InternalErrorReporterIF>(
 			objects::INTERNAL_ERROR_REPORTER);
-	if (internalErrorReporter == NULL){
-		return RETURN_FAILED;
+	if (internalErrorReporter == nullptr){
+		return ObjectManagerIF::INTERNAL_ERR_REPORTER_UNINIT;
 	}

 	//Check if any pool size is large than the maximum allowed.
@ -251,10 +296,10 @@ inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::initialize() {
 		if (element_sizes[count] >= STORAGE_FREE) {
 			sif::error << "LocalPool::initialize: Pool is too large! "
 					"Max. allowed size is: " << (STORAGE_FREE - 1) << std::endl;
-			return RETURN_FAILED;
+			return StorageManagerIF::POOL_TOO_LARGE;
 		}
 	}
 	return RETURN_OK;
 }

-#endif
+#endif /* FSFW_STORAGEMANAGER_LOCALPOOL_TPP_ */
--- a/storagemanager/PoolManager.h
+++ b/storagemanager/PoolManager.h
@ -1,17 +1,18 @@
-#ifndef POOLMANAGER_H_
-#define POOLMANAGER_H_
-
+#ifndef FSFW_STORAGEMANAGER_POOLMANAGER_H_
+#define FSFW_STORAGEMANAGER_POOLMANAGER_H_

 #include "LocalPool.h"
+#include "StorageAccessor.h"
 #include "../ipc/MutexHelper.h"

+
 /**
 * @brief	The PoolManager class provides an intermediate data storage with
 * 			a fixed pool size policy for inter-process communication.
 * @details	Uses local pool calls but is thread safe by protecting the call
 * 			with a lock.
+ * @author 	Bastian Baetz
 */
-
 template <uint8_t NUMBER_OF_POOLS = 5>
 class PoolManager : public LocalPool<NUMBER_OF_POOLS> {
 public:
@ -19,16 +20,25 @@ public:
 			const uint16_t element_sizes[NUMBER_OF_POOLS],
 			const uint16_t n_elements[NUMBER_OF_POOLS]);

-	//! @brief	In the PoolManager's destructor all allocated memory is freed.
+	/**
+	 * @brief	In the PoolManager's destructor all allocated memory
+	 * 			is freed.
+	 */
 	virtual ~PoolManager();

-	//! @brief LocalPool overrides for thread-safety.
+	/**
+	 * @brief 	LocalPool overrides for thread-safety. Decorator function
+	 * 			which wraps LocalPool calls with a mutex protection.
+	 */
 	ReturnValue_t deleteData(store_address_t) override;
 	ReturnValue_t deleteData(uint8_t* buffer, size_t size,
-			store_address_t* storeId = NULL) override;
-	ReturnValue_t modifyData(store_address_t packet_id, uint8_t** packet_ptr,
-				size_t* size) override;
+			store_address_t* storeId = nullptr) override;
+
+	void setMutexTimeout(uint32_t mutexTimeoutMs);
 protected:
+	//! Default mutex timeout value to prevent permanent blocking.
+	uint32_t mutexTimeoutMs = 20;
+
 	ReturnValue_t reserveSpace(const uint32_t size, store_address_t* address,
 			bool ignoreFault) override;

@ -43,4 +53,4 @@ protected:

 #include "PoolManager.tpp"

-#endif /* POOLMANAGER_H_ */
+#endif /* FSFW_STORAGEMANAGER_POOLMANAGER_H_ */
--- a/storagemanager/PoolManager.tpp
+++ b/storagemanager/PoolManager.tpp
@ -1,6 +1,10 @@
 #ifndef FRAMEWORK_STORAGEMANAGER_POOLMANAGER_TPP_
 #define FRAMEWORK_STORAGEMANAGER_POOLMANAGER_TPP_

+#ifndef FSFW_STORAGEMANAGER_POOLMANAGER_H_
+#error Include PoolManager.h before PoolManager.tpp!
+#endif
+
 template<uint8_t NUMBER_OF_POOLS>
 inline PoolManager<NUMBER_OF_POOLS>::PoolManager(object_id_t setObjectId,
 		const uint16_t element_sizes[NUMBER_OF_POOLS],
@ -17,7 +21,7 @@ inline PoolManager<NUMBER_OF_POOLS>::~PoolManager(void) {
 template<uint8_t NUMBER_OF_POOLS>
 inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::reserveSpace(
 		const uint32_t size, store_address_t* address, bool ignoreFault) {
-	MutexHelper mutexHelper(mutex,MutexIF::BLOCKING);
+	MutexHelper mutexHelper(mutex,MutexIF::WAITING, mutexTimeoutMs);
 	ReturnValue_t status = LocalPool<NUMBER_OF_POOLS>::reserveSpace(size,
 			address,ignoreFault);
 	return status;
@ -29,7 +33,7 @@ inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::deleteData(
 	// debug << "PoolManager( " << translateObject(getObjectId()) <<
 	//       " )::deleteData from store " << packet_id.pool_index <<
 	//       ". id is "<< packet_id.packet_index << std::endl;
-	MutexHelper mutexHelper(mutex,MutexIF::BLOCKING);
+	MutexHelper mutexHelper(mutex,MutexIF::WAITING, mutexTimeoutMs);
 	ReturnValue_t status = LocalPool<NUMBER_OF_POOLS>::deleteData(packet_id);
 	return status;
 }
@ -37,19 +41,16 @@ inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::deleteData(
 template<uint8_t NUMBER_OF_POOLS>
 inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::deleteData(uint8_t* buffer,
 		size_t size, store_address_t* storeId) {
-	MutexHelper mutexHelper(mutex,MutexIF::BLOCKING);
+	MutexHelper mutexHelper(mutex,MutexIF::WAITING, mutexTimeoutMs);
 	ReturnValue_t status = LocalPool<NUMBER_OF_POOLS>::deleteData(buffer,
 			size, storeId);
 	return status;
 }

 template<uint8_t NUMBER_OF_POOLS>
-inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::modifyData(
-		store_address_t packet_id, uint8_t** packet_ptr, size_t* size) {
-	MutexHelper mutexHelper(mutex,MutexIF::BLOCKING);
-	ReturnValue_t status = LocalPool<NUMBER_OF_POOLS>::modifyData(packet_id,
-				packet_ptr, size);
-	return status;
+inline void PoolManager<NUMBER_OF_POOLS>::setMutexTimeout(
+		uint32_t mutexTimeoutMs) {
+	this->mutexTimeout = mutexTimeoutMs;
 }

-#endif
+#endif /* FRAMEWORK_STORAGEMANAGER_POOLMANAGER_TPP_ */
--- a/storagemanager/StorageAccessor.cpp
+++ b/storagemanager/StorageAccessor.cpp
@ -0,0 +1,67 @@
+#include "StorageAccessor.h"
+#include "StorageManagerIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+
+StorageAccessor::StorageAccessor(store_address_t storeId):
+		ConstStorageAccessor(storeId) {
+}
+
+StorageAccessor::StorageAccessor(store_address_t storeId,
+		StorageManagerIF* store):
+		ConstStorageAccessor(storeId, store) {
+}
+
+StorageAccessor& StorageAccessor::operator =(
+		StorageAccessor&& other) {
+	// Call the parent move assignment and also assign own member.
+	dataPointer = other.dataPointer;
+	StorageAccessor::operator=(std::move(other));
+	return * this;
+}
+
+// Call the parent move ctor and also transfer own member.
+StorageAccessor::StorageAccessor(StorageAccessor&& other):
+		ConstStorageAccessor(std::move(other)), dataPointer(other.dataPointer) {
+}
+
+ReturnValue_t StorageAccessor::getDataCopy(uint8_t *pointer, size_t maxSize) {
+	if(internalState == AccessState::UNINIT) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(size_ > maxSize) {
+		sif::error << "StorageAccessor: Supplied buffer not large "
+				"enough" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(dataPointer, dataPointer + size_, pointer);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+uint8_t* StorageAccessor::data() {
+	if(internalState == AccessState::UNINIT) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+	}
+	return dataPointer;
+}
+
+ReturnValue_t StorageAccessor::write(uint8_t *data, size_t size,
+		uint16_t offset) {
+	if(internalState == AccessState::UNINIT) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	if(offset + size > size_) {
+		sif::error << "StorageAccessor: Data too large for pool "
+				"entry!" << std::endl;
+		return HasReturnvaluesIF::RETURN_FAILED;
+	}
+	std::copy(data, data + size, dataPointer + offset);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+void StorageAccessor::assignConstPointer() {
+	constDataPointer = dataPointer;
+}
+
+
--- a/storagemanager/StorageAccessor.h
+++ b/storagemanager/StorageAccessor.h
@ -0,0 +1,45 @@
+#ifndef FSFW_STORAGEMANAGER_STORAGEACCESSOR_H_
+#define FSFW_STORAGEMANAGER_STORAGEACCESSOR_H_
+
+#include "ConstStorageAccessor.h"
+
+class StorageManagerIF;
+
+/**
+ * @brief   Child class for modifyable data. Also has a normal pointer member.
+ */
+class StorageAccessor: public ConstStorageAccessor {
+	//! StorageManager classes have exclusive access to private variables.
+	template<uint8_t NUMBER_OF_POOLS>
+	friend class PoolManager;
+	template<uint8_t NUMBER_OF_POOLS>
+	friend class LocalPool;
+public:
+	StorageAccessor(store_address_t storeId);
+	StorageAccessor(store_address_t storeId, StorageManagerIF* store);
+
+	/**
+	 * @brief	Move ctor and move assignment allow returning accessors as
+	 * 			a returnvalue. They prevent resource being freed prematurely.
+	 * See: https://github.com/MicrosoftDocs/cpp-docs/blob/master/docs/cpp/
+	 * 		move-constructors-and-move-assignment-operators-cpp.md
+	 * @param
+	 * @return
+	 */
+	StorageAccessor& operator=(StorageAccessor&&);
+	StorageAccessor(StorageAccessor&&);
+
+	ReturnValue_t write(uint8_t *data, size_t size,
+		uint16_t offset = 0);
+	uint8_t* data();
+	ReturnValue_t getDataCopy(uint8_t *pointer, size_t maxSize) override;
+
+private:
+	//! Non-const pointer for modifyable data.
+	uint8_t* dataPointer = nullptr;
+	//! For modifyable data, the const pointer is assigned to the normal
+	//! pointer by the pool manager so both access functions can be used safely
+	void assignConstPointer();
+};
+
+#endif /* FSFW_STORAGEMANAGER_STORAGEACCESSOR_H_ */
--- a/storagemanager/StorageManagerIF.h
+++ b/storagemanager/StorageManagerIF.h
@ -1,60 +1,17 @@
-#ifndef STORAGEMANAGERIF_H_H
-#define STORAGEMANAGERIF_H_H
+#ifndef FSFW_STORAGEMANAGER_STORAGEMANAGERIF_H_
+#define FSFW_STORAGEMANAGER_STORAGEMANAGERIF_H_
+
+#include "StorageAccessor.h"
+#include "storeAddress.h"

 #include "../events/Event.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
-#include <stddef.h>

-/**
- * @brief 	This union defines the type that identifies where a data packet is
- * 			stored in the store.
- * It consists of a raw part to read it as raw value and
- * a structured part to use it in pool-like stores.
- */
-union store_address_t {
-	/**
-	 * Default Constructor, initializing to INVALID_ADDRESS
-	 */
-	store_address_t():raw(0xFFFFFFFF){}
+#include <utility>
+#include <cstddef>

-	/**
-	 * Constructor to create an address object using the raw address
-	 * @param rawAddress
-	 */
-	store_address_t(uint32_t rawAddress):raw(rawAddress){}
-
-	/**
-	 * Constructor to create an address object using pool
-	 * and packet indices
-	 *
-	 * @param poolIndex
-	 * @param packetIndex
-	 */
-	store_address_t(uint16_t poolIndex, uint16_t packetIndex):
-			pool_index(poolIndex),packet_index(packetIndex) {}
-
-	/**
-	 * A structure with two elements to access the store address pool-like.
-	 */
-	struct {
-		/**
-		 * The index in which pool the packet lies.
-		 */
-		uint16_t pool_index;
-		/**
-		 * The position in the chosen pool.
-		 */
-		uint16_t packet_index;
-	};
-	/**
-	 * Alternative access to the raw value.
-	 */
-	uint32_t raw;
-
-	bool operator==(const store_address_t& other) const {
-		return raw == other.raw;
-	}
-};
+using AccessorPair = std::pair<ReturnValue_t, StorageAccessor>;
+using ConstAccessorPair = std::pair<ReturnValue_t, ConstStorageAccessor>;

 /**
 * @brief	This class provides an interface for intermediate data storage.
@ -77,6 +34,7 @@ public:
 	static const ReturnValue_t ILLEGAL_STORAGE_ID = MAKE_RETURN_CODE(3); //!< This return code indicates that data was requested with an illegal storage ID.
 	static const ReturnValue_t DATA_DOES_NOT_EXIST = MAKE_RETURN_CODE(4); //!< This return code indicates that the requested ID was valid, but no data is stored there.
 	static const ReturnValue_t ILLEGAL_ADDRESS = MAKE_RETURN_CODE(5);
+	static const ReturnValue_t POOL_TOO_LARGE = MAKE_RETURN_CODE(6); //!< Pool size too large on initialization.

 	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::OBSW;
 	static const Event GET_DATA_FAILED = MAKE_EVENT(0, SEVERITY::LOW);
@ -122,6 +80,29 @@ public:
 	 */
 	virtual ReturnValue_t deleteData(uint8_t* buffer, size_t size,
 			store_address_t* storeId = nullptr) = 0;
+
+
+	/**
+	 * @brief 	Access the data by supplying a store ID.
+	 * @details
+	 * A pair consisting of the retrieval result and an instance of a
+	 * ConstStorageAccessor class is returned
+	 * @param storeId
+	 * @return Pair of return value and a ConstStorageAccessor instance
+	 */
+	virtual ConstAccessorPair getData(store_address_t storeId) = 0;
+
+	/**
+	 * @brief 	Access the data by supplying a store ID and a helper
+	 * 			instance
+	 * @param storeId
+	 * @param constAccessor Wrapper function to access store data.
+	 * @return
+	 */
+	virtual ReturnValue_t getData(store_address_t storeId,
+			ConstStorageAccessor& constAccessor) = 0;
+
+
 	/**
 	 * @brief	getData returns an address to data and the size of the data
 	 * 			for a given packet_id.
@ -135,8 +116,30 @@ public:
 	 */
 	virtual ReturnValue_t getData(store_address_t packet_id,
 			const uint8_t** packet_ptr, size_t* size) = 0;
+
+
 	/**
-	 * Same as above, but not const and therefore modifiable.
+	 * Modify data by supplying a store ID
+	 * @param storeId
+	 * @return Pair of return value and StorageAccessor helper
+	 */
+	virtual AccessorPair modifyData(store_address_t storeId) = 0;
+
+	/**
+	 * Modify data by supplying a store ID and a StorageAccessor helper instance.
+	 * @param storeId
+	 * @param accessor Helper class to access the modifiable data.
+	 * @return
+	 */
+	virtual ReturnValue_t modifyData(store_address_t storeId,
+				StorageAccessor& accessor) = 0;
+
+	/**
+	 * Get pointer and size of modifiable data by supplying the storeId
+	 * @param packet_id
+	 * @param packet_ptr [out] Pointer to pointer of data to set
+	 * @param size [out] Pointer to size to set
+	 * @return
 	 */
 	virtual ReturnValue_t modifyData(store_address_t packet_id,
 			uint8_t** packet_ptr, size_t* size) = 0;
@ -155,6 +158,7 @@ public:
 	 */
 	virtual ReturnValue_t getFreeElement(store_address_t* storageId,
 			const size_t size, uint8_t** p_data, bool ignoreFault = false ) = 0;
+
 	/**
 	 * Clears the whole store.
 	 * Use with care!
@ -162,4 +166,4 @@ public:
 	virtual void clearStore() = 0;
 };

-#endif /* STORAGEMANAGERIF_H_ */
+#endif /* FSFW_STORAGEMANAGER_STORAGEMANAGERIF_H_ */
--- a/storagemanager/storeAddress.h
+++ b/storagemanager/storeAddress.h
@ -0,0 +1,55 @@
+#ifndef FSFW_STORAGEMANAGER_STOREADDRESS_H_
+#define FSFW_STORAGEMANAGER_STOREADDRESS_H_
+
+#include <cstdint>
+
+/**
+ * This union defines the type that identifies where a data packet is
+ * stored in the store. It comprises of a raw part to read it as raw value and
+ * a structured part to use it in pool-like stores.
+ */
+union store_address_t {
+	/**
+	 * Default Constructor, initializing to INVALID_ADDRESS
+	 */
+	store_address_t():raw(0xFFFFFFFF){}
+	/**
+	 * Constructor to create an address object using the raw address
+	 *
+	 * @param rawAddress
+	 */
+	store_address_t(uint32_t rawAddress):raw(rawAddress){}
+
+	/**
+	 * Constructor to create an address object using pool
+	 * and packet indices
+	 *
+	 * @param poolIndex
+	 * @param packetIndex
+	 */
+	store_address_t(uint16_t poolIndex, uint16_t packetIndex):
+		pool_index(poolIndex),packet_index(packetIndex){}
+	/**
+	 * A structure with two elements to access the store address pool-like.
+	 */
+	struct {
+		/**
+		 * The index in which pool the packet lies.
+		 */
+		uint16_t pool_index;
+		/**
+		 * The position in the chosen pool.
+		 */
+		uint16_t packet_index;
+	};
+	/**
+	 * Alternative access to the raw value.
+	 */
+	uint32_t raw;
+
+	bool operator==(const store_address_t& other) const {
+		return raw == other.raw;
+	}
+};
+
+#endif /* FSFW_STORAGEMANAGER_STOREADDRESS_H_ */
--- a/tasks/FixedSequenceSlot.cpp
+++ b/tasks/FixedSequenceSlot.cpp
@ -0,0 +1,17 @@
+#include "FixedSequenceSlot.h"
+#include "PeriodicTaskIF.h"
+#include <cstddef>
+
+FixedSequenceSlot::FixedSequenceSlot(object_id_t handlerId, uint32_t setTime,
+		int8_t setSequenceId, ExecutableObjectIF* executableObject,
+		PeriodicTaskIF* executingTask) : handlerId(handlerId),
+		pollingTimeMs(setTime), opcode(setSequenceId) {
+    if(executableObject == nullptr) {
+        return;
+    }
+    this->executableObject = executableObject;
+    this->executableObject->setTaskIF(executingTask);
+}
+
+FixedSequenceSlot::~FixedSequenceSlot() {}
+
--- a/devicehandlers/FixedSequenceSlot.h
+++ b/devicehandlers/FixedSequenceSlot.h
@ -1,41 +1,41 @@
-/**
- * @file	FixedSequenceSlot.h
- * @brief	This file defines the PollingSlot class.
- * @date	19.12.2012
- * @author	baetz
- */
-
-#ifndef FIXEDSEQUENCESLOT_H_
-#define FIXEDSEQUENCESLOT_H_
+#ifndef FSFW_TASKS_FIXEDSEQUENCESLOT_H_
+#define FSFW_TASKS_FIXEDSEQUENCESLOT_H_

+#include "ExecutableObjectIF.h"
 #include "../objectmanager/ObjectManagerIF.h"
-#include "../tasks/ExecutableObjectIF.h"
+
 class PeriodicTaskIF;

 /**
- * @brief	This class is the representation of a single polling sequence table entry.
- *
- * @details	The PollingSlot class is the representation of a single polling
- * 			sequence table entry.
+ * @brief	This class is the representation of a single polling sequence
+ *          table entry.
+ * @details
+ * The PollingSlot class is the representation of a single polling
+ * sequence table entry.
+ * @author  baetz
 */
 class FixedSequenceSlot {
 public:
 	FixedSequenceSlot( object_id_t handlerId, uint32_t setTimeMs,
-			int8_t setSequenceId, PeriodicTaskIF* executingTask );
+			int8_t setSequenceId, ExecutableObjectIF* executableObject,
+			PeriodicTaskIF* executingTask);
 	virtual ~FixedSequenceSlot();

+	object_id_t handlerId;
+
 	/**
-	 * @brief	Handler identifies which device handler object is executed in this slot.
+	 * @brief	Handler identifies which object is executed in this slot.
 	 */
-	ExecutableObjectIF*	handler;
+	ExecutableObjectIF*	executableObject = nullptr;

 	/**
 	 * @brief	This attribute defines when a device handler object is executed.
-	 *
-	 * @details	The pollingTime attribute identifies the time the handler is executed in ms.
-	 * 			It must be smaller than the period length of the polling sequence.
+	 * @details
+	 * The pollingTime attribute identifies the time the handler is
+	 * executed in ms. It must be smaller than the period length of the
+	 * polling sequence.
 	 */
-	uint32_t	pollingTimeMs;
+	uint32_t pollingTimeMs;

 	/**
 	 * @brief	This value defines the type of device communication.
@ -43,7 +43,7 @@ public:
 	 * @details	The state of this value decides what communication routine is
 	 * 			called in the PST executable or the device handler object.
 	 */
-	uint8_t			opcode;
+	uint8_t	opcode;

 	/**
 	 * @brief 	Operator overload for the comparison operator to
@ -57,4 +57,4 @@ public:
 };


-#endif /* FIXEDSEQUENCESLOT_H_ */
+#endif /* FSFW_TASKS_FIXEDSEQUENCESLOT_H_ */
--- a/devicehandlers/FixedSlotSequence.cpp
+++ b/devicehandlers/FixedSlotSequence.cpp
@ -1,5 +1,6 @@
 #include "FixedSlotSequence.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
+#include <cstdlib>

 FixedSlotSequence::FixedSlotSequence(uint32_t setLengthMs) :
 		lengthMs(setLengthMs) {
@ -12,7 +13,7 @@ FixedSlotSequence::~FixedSlotSequence() {
 }

 void FixedSlotSequence::executeAndAdvance() {
-	current->handler->performOperation(current->opcode);
+	current->executableObject->performOperation(current->opcode);
 //	if (returnValue != RETURN_OK) {
 //		this->sendErrorMessage( returnValue );
 //	}
@ -80,44 +81,82 @@ uint32_t FixedSlotSequence::getLengthMs() const {
 	return this->lengthMs;
 }

+void FixedSlotSequence::addSlot(object_id_t componentId, uint32_t slotTimeMs,
+        int8_t executionStep, ExecutableObjectIF* executableObject,
+        PeriodicTaskIF* executingTask) {
+    this->slotList.insert(FixedSequenceSlot(componentId, slotTimeMs,
+            executionStep, executableObject, executingTask));
+   this->current = slotList.begin();
+}
+
 ReturnValue_t FixedSlotSequence::checkSequence() const {
 	if(slotList.empty()) {
-		sif::error << "Fixed Slot Sequence: Slot list is empty!" << std::endl;
+		sif::error << "FixedSlotSequence::checkSequence:"
+				<< " Slot list is empty!" << std::endl;
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}

-	auto slotIt = slotList.begin();
-	uint32_t count = 0;
-	uint32_t time = 0;
-	while (slotIt != slotList.end()) {
-		if (slotIt->handler == nullptr) {
-			sif::error << "FixedSlotSequene::initialize: ObjectId does not exist!"
-					<< std::endl;
-			count++;
-		} else if (slotIt->pollingTimeMs < time) {
-			sif::error << "FixedSlotSequence::initialize: Time: "
-					<< slotIt->pollingTimeMs
-					<< " is smaller than previous with " << time << std::endl;
-			count++;
-		} else {
-			// All ok, print slot.
-			//info << "Current slot polling time: " << std::endl;
-			//info << std::dec << slotIt->pollingTimeMs << std::endl;
+	if(customCheckFunction != nullptr) {
+		ReturnValue_t result = customCheckFunction(slotList);
+		if(result != HasReturnvaluesIF::RETURN_OK) {
+			// Continue for now but print error output.
+			sif::error << "FixedSlotSequence::checkSequence:"
+					<< " Custom check failed!" << std::endl;
 		}
-		time = slotIt->pollingTimeMs;
-		slotIt++;
 	}
-	//info << "Number of elements in slot list: "
+
+	uint32_t errorCount = 0;
+	uint32_t time = 0;
+	for(const auto& slot: slotList) {
+		if (slot.executableObject == nullptr) {
+			errorCount++;
+		}
+		else if (slot.pollingTimeMs < time) {
+			sif::error << "FixedSlotSequence::checkSequence: Time: "
+					<< slot.pollingTimeMs << " is smaller than previous with "
+					<< time << std::endl;
+			errorCount++;
+		}
+		else {
+			// All ok, print slot.
+			//sif::info << "Current slot polling time: " << std::endl;
+			//sif::info << std::dec << slotIt->pollingTimeMs << std::endl;
+		}
+		time = slot.pollingTimeMs;
+
+	}
+	//sif::info << "Number of elements in slot list: "
 	//	   << slotList.size() << std::endl;
-	if (count > 0) {
+	if (errorCount > 0) {
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
 	return HasReturnvaluesIF::RETURN_OK;
 }

-void FixedSlotSequence::addSlot(object_id_t componentId, uint32_t slotTimeMs,
-		int8_t executionStep, PeriodicTaskIF* executingTask) {
-	this->slotList.insert(FixedSequenceSlot(componentId, slotTimeMs, executionStep,
-			executingTask));
-	this->current = slotList.begin();
+
+ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const {
+    std::set<ExecutableObjectIF*> uniqueObjects;
+    uint32_t count = 0;
+    for(const auto& slot: slotList) {
+        // Ensure that each unique object is initialized once.
+        if(uniqueObjects.find(slot.executableObject) == uniqueObjects.end()) {
+            ReturnValue_t result =
+                    slot.executableObject->initializeAfterTaskCreation();
+            if(result != HasReturnvaluesIF::RETURN_OK) {
+                count++;
+            }
+            uniqueObjects.emplace(slot.executableObject);
+        }
+    }
+    if (count > 0) {
+        sif::error << "FixedSlotSequence::intializeSequenceAfterTaskCreation:"
+                "Counted " << count << " failed initializations!"  << std::endl;
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void FixedSlotSequence::addCustomCheck(ReturnValue_t
+		(*customCheckFunction)(const SlotList&)) {
+	this->customCheckFunction = customCheckFunction;
 }
--- a/devicehandlers/FixedSlotSequence.h
+++ b/devicehandlers/FixedSlotSequence.h
@ -1,26 +1,30 @@
-#ifndef FRAMEWORK_DEVICEHANDLERS_FIXEDSLOTSEQUENCE_H_
-#define FRAMEWORK_DEVICEHANDLERS_FIXEDSLOTSEQUENCE_H_
+#ifndef FSFW_TASKS_FIXEDSLOTSEQUENCE_H_
+#define FSFW_TASKS_FIXEDSLOTSEQUENCE_H_

 #include "FixedSequenceSlot.h"
 #include "../objectmanager/SystemObject.h"
+
 #include <set>

 /**
- * @brief	This class is the representation of a Polling Sequence Table in software.
- *
+ * @brief	This class is the representation of a
+ * 			Polling Sequence Table in software.
 * @details
 * The FixedSlotSequence object maintains the dynamic execution of
- * device handler objects.
+ * objects with stricter timing requirements for the FixedTimeslotTask.
 *
- * The main idea is to create a list of device handlers, to announce all
- * handlers to thepolling sequence and to maintain a list of
- * polling slot objects. This slot list represents the Polling Sequence Table
- * in software.
+ * The main idea is to create a list of executable objects (for example
+ * device handlers), to announce all handlers to the polling sequence and to
+ * maintain a list of polling slot objects.
+ * This slot list represents the Polling Sequence Table in software.
 *
 * Each polling slot contains information to indicate when and
- * which device handler shall be executed within a given polling period.
- * The sequence is then executed by iterating through this slot list.
- * Handlers are invoking by calling a certain function stored in the handler list.
+ * which executable object shall be executed within a given polling period.
+ * When adding a slot, a pointer to the executing task, a pointer to the
+ * executable object and a step number can be passed. The step number will be
+ * passed to the periodic handler.
+ * The sequence is executed by iterating through the slot sequence and
+ * executing the executable object in the correct timeslot.
 */
 class FixedSlotSequence {
 public:
@ -29,41 +33,44 @@ public:

 	/**
 	 * @brief	The constructor of the FixedSlotSequence object.
-	 *
-	 * @details	The constructor takes two arguments, the period length and the init function.
-	 *
 	 * @param	setLength	The period length, expressed in ms.
 	 */
 	FixedSlotSequence(uint32_t setLengthMs);

 	/**
 	 * @brief	The destructor of the FixedSlotSequence object.
-	 *
-	 * @details	The destructor frees all allocated memory by iterating through the slotList
-	 *  		and deleting all allocated resources.
+	 * @details
+	 * The destructor frees all allocated memory by iterating through the
+	 * slotList and deleting all allocated resources.
 	 */
 	virtual ~FixedSlotSequence();

 	/**
 	 * @brief	This is a method to add an PollingSlot object to slotList.
 	 *
-	 * @details	Here, a polling slot object is added to the slot list. It is appended
-	 * 			to the end of the list. The list is currently NOT reordered.
-	 * 			Afterwards, the iterator current is set to the beginning of the list.
-	 * 	@param Object ID of the object to add
-	 * 	@param setTime Value between (0 to 1) * slotLengthMs, when a FixedTimeslotTask
-	 * 				   will be called inside the slot period.
-	 * 	@param setSequenceId ID which can be used to distinguish
-	 * 	                     different task operations
+	 * @details
+	 * Here, a polling slot object is added to the slot list. It is appended
+	 * to the end of the list. The list is currently NOT reordered.
+	 * Afterwards, the iterator current is set to the beginning of the list.
+	 * 	@param handlerId ID of the object to add
+	 * 	@param setTime
+	 * 	Value between (0 to 1) * slotLengthMs, when a FixedTimeslotTask
+	 * 	will be called inside the slot period.
+	 * 	@param setSequenceId
+	 * 	ID which can be used to distinguish different task operations. This
+	 * 	value will be passed to the executable function.
 	 * 	@param
 	 * 	@param
 	 */
 	void addSlot(object_id_t handlerId, uint32_t setTime, int8_t setSequenceId,
+			ExecutableObjectIF* executableObject,
 			PeriodicTaskIF* executingTask);

 	/**
-	 * Checks if the current slot shall be executed immediately after the one before.
-	 * This allows to distinguish between grouped and not grouped handlers.
+	 * @brief	Checks if the current slot shall be executed immediately
+	 * 			after the one before.
+	 * @details
+	 * This allows to distinguish between grouped and separated handlers.
 	 * @return 	- @c true if the slot has the same polling time as the previous
 	 * 			- @c false else
 	 */
@ -125,12 +132,32 @@ public:
 	SlotListIter current;

 	/**
-	 * Iterate through slotList and check successful creation.
+	 * @brief   Check and initialize slot list.
+	 * @details
 	 * Checks if timing is ok (must be ascending) and if all handlers were found.
 	 * @return
 	 */
 	ReturnValue_t checkSequence() const;

+	/**
+	 * @brief   A custom check can be injected for the respective slot list.
+	 * @details
+	 * This can be used by the developer to check the validity of a certain
+	 * sequence. The function will be run in the #checkSequence function.
+	 * The general check will be continued for now if the custom check function
+	 * fails but a diagnostic debug output will be given.
+	 * @param customCheckFunction
+	 */
+	void addCustomCheck(ReturnValue_t (*customCheckFunction)(const SlotList &));
+
+	/**
+	 * @brief 	Perform any initialization steps required after the executing
+	 * 			task has been created. This function should be called from the
+	 * 			executing task!
+	 * @return
+	 */
+	ReturnValue_t intializeSequenceAfterTaskCreation() const;
+
 protected:

 	/**
@ -146,7 +173,9 @@ protected:
 	 */
 	SlotList slotList;

+	ReturnValue_t (*customCheckFunction)(const SlotList&) = nullptr;
+
 	uint32_t lengthMs;
 };

-#endif /* FIXEDSLOTSEQUENCE_H_ */
+#endif /* FSFW_TASKS_FIXEDSLOTSEQUENCE_H_ */
--- a/tmtcservices/PusVerificationReport.h
+++ b/tmtcservices/PusVerificationReport.h
@ -1,9 +1,11 @@
-#ifndef PUSVERIFICATIONREPORT_H_
-#define PUSVERIFICATIONREPORT_H_
+#ifndef FSFW_TMTCSERVICES_PUSVERIFICATIONREPORT_H_
+#define FSFW_TMTCSERVICES_PUSVERIFICATIONREPORT_H_
+
+#include "VerificationCodes.h"

 #include "../ipc/MessageQueueMessage.h"
 #include "../tmtcpacket/pus/TcPacketBase.h"
-#include "VerificationCodes.h"
+#include "../returnvalues/HasReturnvaluesIF.h"

 class PusVerificationMessage: public MessageQueueMessage {
 private:
@ -30,7 +32,7 @@ private:
 public:
 	static const uint8_t VERIFICATION_MIN_SIZE = 6;
 	PusVerificationMessage();
-//	PusVerificationMessage( uint8_t set_report_id, TcPacketBase* current_packet, ReturnValue_t set_error_code = 0, uint8_t set_step = 0, uint32_t parameter1 = 0, uint32_t parameter2 = 0 );
+
 	PusVerificationMessage(uint8_t set_report_id, uint8_t ackFlags,
 			uint16_t tcPacketId, uint16_t tcSequenceControl,
 			ReturnValue_t set_error_code = 0, uint8_t set_step = 0,
@ -74,4 +76,4 @@ public:
 	uint8_t* getReport();
 };

-#endif /* PUSVERIFICATIONREPORT_H_ */
+#endif /* FSFW_TMTCSERVICES_PUSVERIFICATIONREPORT_H_ */
--- a/tmtcservices/VerificationReporter.cpp
+++ b/tmtcservices/VerificationReporter.cpp
@ -1,55 +1,59 @@
-#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "VerificationReporter.h"
 #include "AcceptsVerifyMessageIF.h"
 #include "PusVerificationReport.h"
-#include "VerificationReporter.h"

-object_id_t VerificationReporter::messageReceiver = 0;
+#include "../ipc/MessageQueueIF.h"
+#include "../serviceinterface/ServiceInterfaceStream.h"
+#include "../objectmanager/frameworkObjects.h"
+
+object_id_t VerificationReporter::messageReceiver =
+		objects::PUS_SERVICE_1_VERIFICATION;

 VerificationReporter::VerificationReporter() :
-		acknowledgeQueue() {
+		acknowledgeQueue(MessageQueueIF::NO_QUEUE) {
 }

-VerificationReporter::~VerificationReporter() {
-	//Default, empty
-}
+VerificationReporter::~VerificationReporter() {}

 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 		TcPacketBase* current_packet, uint8_t set_step) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(set_report_id,
 			current_packet->getAcknowledgeFlags(),
 			current_packet->getPacketId(),
 			current_packet->getPacketSequenceControl(), 0, set_step);
-	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
+	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
+			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
-		sif::error
-				<< "VerificationReporter::sendSuccessReport: Error writing to queue. Code: "
-				<< (uint16_t) status << std::endl;
+		sif::error << "VerificationReporter::sendSuccessReport: Error writing "
+				<< "to queue. Code: " << std::hex << status << std::dec
+				<< std::endl;
 	}
 }

 void VerificationReporter::sendSuccessReport(uint8_t set_report_id,
 		uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
 		uint8_t set_step) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE)  {
 		this->initialize();
 	}
 	PusVerificationMessage message(set_report_id, ackFlags, tcPacketId,
 			tcSequenceControl, 0, set_step);
-	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
+	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
+			&message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
-		sif::error
-				<< "VerificationReporter::sendSuccessReport: Error writing to queue. Code: "
-				<< (uint16_t) status << std::endl;
+		sif::error << "VerificationReporter::sendSuccessReport: Error writing "
+				<< "to queue. Code: " << std::hex << status << std::dec
+				<< std::endl;
 	}
 }

 void VerificationReporter::sendFailureReport(uint8_t report_id,
 		TcPacketBase* current_packet, ReturnValue_t error_code, uint8_t step,
 		uint32_t parameter1, uint32_t parameter2) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(report_id,
@ -57,11 +61,12 @@ void VerificationReporter::sendFailureReport(uint8_t report_id,
 			current_packet->getPacketId(),
 			current_packet->getPacketSequenceControl(), error_code, step,
 			parameter1, parameter2);
-	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
+	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
+	        &message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
-		sif::error
-				<< "VerificationReporter::sendFailureReport Error writing to queue. Code: "
-				<< (uint16_t) status << std::endl;
+		sif::error << "VerificationReporter::sendFailureReport: Error writing "
+				<< "to queue. Code: " << std::hex  << "0x" << status << std::dec
+				<< std::endl;
 	}
 }

@ -69,27 +74,33 @@ void VerificationReporter::sendFailureReport(uint8_t report_id,
 		uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
 		ReturnValue_t error_code, uint8_t step, uint32_t parameter1,
 		uint32_t parameter2) {
-	if (this->acknowledgeQueue == 0) {
+	if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
 		this->initialize();
 	}
 	PusVerificationMessage message(report_id, ackFlags, tcPacketId,
 			tcSequenceControl, error_code, step, parameter1, parameter2);
-	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
+	ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue,
+	        &message);
 	if (status != HasReturnvaluesIF::RETURN_OK) {
-		sif::error
-				<< "VerificationReporter::sendFailureReport Error writing to queue. Code: "
-				<< (uint16_t) status << std::endl;
+		sif::error << "VerificationReporter::sendFailureReport: Error writing "
+				<< "to queue. Code: " << std::hex << "0x" << status << std::dec
+				<< std::endl;
 	}
 }

 void VerificationReporter::initialize() {
+	if(messageReceiver == objects::NO_OBJECT) {
+		sif::warning << "VerificationReporter::initialize: Verification message"
+				" receiver object ID not set yet in Factory!" << std::endl;
+		return;
+	}
 	AcceptsVerifyMessageIF* temp = objectManager->get<AcceptsVerifyMessageIF>(
 			messageReceiver);
-	if (temp != NULL) {
-		this->acknowledgeQueue = temp->getVerificationQueue();
-	} else {
-		sif::error
-				<< "VerificationReporter::VerificationReporter: Configuration error."
-				<< std::endl;
+	if (temp == nullptr) {
+		sif::error << "VerificationReporter::initialize: Message "
+				<< "receiver invalid. Make sure it is set up properly and "
+				<< "implementsAcceptsVerifyMessageIF" << std::endl;
+		return;
 	}
+	this->acknowledgeQueue = temp->getVerificationQueue();
 }
--- a/tmtcservices/VerificationReporter.h
+++ b/tmtcservices/VerificationReporter.h
@ -1,31 +1,50 @@
-#ifndef VERIFICATIONREPORTER_H_
-#define VERIFICATIONREPORTER_H_
+#ifndef FSFW_TMTCSERVICES_VERIFICATIONREPORTER_H_
+#define FSFW_TMTCSERVICES_VERIFICATIONREPORTER_H_

-#include "../objectmanager/ObjectManagerIF.h"
 #include "PusVerificationReport.h"
+#include "../objectmanager/ObjectManagerIF.h"

 namespace Factory{
 void setStaticFrameworkObjectIds();
 }

+/**
+ * @brief 	This helper object is used to forward verification messages
+ * 			which are generated by the Flight Software Framework.
+ * @details
+ * The messages can be relayed to an arbitrary object, for example a dedicated
+ * Verification Reporter. The destination is set by setting the static framework
+ * Id VerificationReporter::messageReceiver. The default verification reporter
+ * will be the PUS service 1, which sends verification messages according
+ * to the PUS standard.
+ *
+ */
 class VerificationReporter {
 	friend void (Factory::setStaticFrameworkObjectIds)();
 public:
 	VerificationReporter();
 	virtual ~VerificationReporter();
-	void sendSuccessReport( uint8_t set_report_id, TcPacketBase* current_packet, uint8_t set_step = 0 );
-	void sendSuccessReport(uint8_t set_report_id, uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, uint8_t set_step = 0);
-	void sendFailureReport( uint8_t report_id, TcPacketBase* current_packet, ReturnValue_t error_code = 0,
-			uint8_t step = 0, uint32_t parameter1 = 0, uint32_t parameter2 = 0 );
+
+	void sendSuccessReport( uint8_t set_report_id, TcPacketBase* current_packet,
+			uint8_t set_step = 0 );
+	void sendSuccessReport(uint8_t set_report_id, uint8_t ackFlags,
+			uint16_t tcPacketId, uint16_t tcSequenceControl,
+			uint8_t set_step = 0);
+
+	void sendFailureReport( uint8_t report_id, TcPacketBase* current_packet,
+			ReturnValue_t error_code = 0,
+			uint8_t step = 0, uint32_t parameter1 = 0,
+			uint32_t parameter2 = 0 );
 	void sendFailureReport(uint8_t report_id,
-			uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl, ReturnValue_t error_code = 0, uint8_t step = 0,
+			uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
+			ReturnValue_t error_code = 0, uint8_t step = 0,
 			uint32_t parameter1 = 0, uint32_t parameter2 = 0);
+
 	void initialize();
+
 private:
 	static object_id_t messageReceiver;
 	MessageQueueId_t acknowledgeQueue;
-
-
 };

-#endif /* VERIFICATIONREPORTER_H_ */
+#endif /* FSFW_TMTCSERVICES_VERIFICATIONREPORTER_H_ */