diff --git a/devicehandlers/CommunicationMessage.h b/devicehandlers/CommunicationMessage.h
index a92c4b5c..95266319 100644
--- a/devicehandlers/CommunicationMessage.h
+++ b/devicehandlers/CommunicationMessage.h
@@ -13,15 +13,11 @@
 #include <framework/devicehandlers/DeviceHandlerBase.h>
 
 /**
- * @brief Used to pass communication information between tasks
+ * @brief Message type to send larger messages
  *
  * @details
  * Can be used to pass information like data pointers and
- * data sizes between communication tasks
- * like the Device Handler Comm Interfaces and Polling Tasks.
- *
- * Can also be used to exchange actual data if its not too large
- * (e.g. Sensor values).
+ * data sizes between communication tasks.
  *
  */
 class CommunicationMessage: public MessageQueueMessage {
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index 710e97d7..16be66fa 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -1,14 +1,14 @@
-#include <framework/datapool/DataSet.h>
-#include <framework/datapool/PoolVariable.h>
-#include <framework/datapool/PoolVector.h>
-#include <framework/devicehandlers/AcceptsDeviceResponsesIF.h>
 #include <framework/devicehandlers/DeviceHandlerBase.h>
-#include <framework/devicehandlers/DeviceTmReportingWrapper.h>
-#include <framework/globalfunctions/CRC.h>
 #include <framework/objectmanager/ObjectManager.h>
 #include <framework/storagemanager/StorageManagerIF.h>
-#include <framework/subsystem/SubsystemBase.h>
 #include <framework/thermal/ThermalComponentIF.h>
+#include <framework/devicehandlers/AcceptsDeviceResponsesIF.h>
+
+#include <framework/datapool/DataSet.h>
+#include <framework/datapool/PoolVariable.h>
+#include <framework/devicehandlers/DeviceTmReportingWrapper.h>
+#include <framework/globalfunctions/CRC.h>
+#include <framework/subsystem/SubsystemBase.h>
 #include <framework/ipc/QueueFactory.h>
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 
@@ -17,33 +17,34 @@ object_id_t DeviceHandlerBase::rawDataReceiverId = 0;
 object_id_t DeviceHandlerBase::defaultFDIRParentId = 0;
 
 DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId,
-		object_id_t deviceCommunication, CookieIF * comCookie_,
+		object_id_t deviceCommunication, CookieIF * comCookie,
 		uint8_t setDeviceSwitch, uint32_t thermalStatePoolId,
 		uint32_t thermalRequestPoolId, FailureIsolationBase* fdirInstance,
 		size_t cmdQueueSize) :
 		SystemObject(setObjectId), mode(MODE_OFF), submode(SUBMODE_NONE),
 		wiretappingMode(OFF), storedRawData(StorageManagerIF::INVALID_ADDRESS),
-	    deviceCommunicationId(deviceCommunication), comCookie(comCookie_),
-		deviceThermalStatePoolId(thermalStatePoolId), deviceThermalRequestPoolId(thermalRequestPoolId),
-		healthHelper(this, setObjectId), modeHelper(this), parameterHelper(this),
-		fdirInstance(fdirInstance), hkSwitcher(this),
-		defaultFDIRUsed(fdirInstance == nullptr), switchOffWasReported(false),
-		executingTask(nullptr), actionHelper(this, nullptr), cookieInfo(),
-		childTransitionDelay(5000), transitionSourceMode(_MODE_POWER_DOWN),
-		transitionSourceSubMode(SUBMODE_NONE), deviceSwitch(setDeviceSwitch)
-{
-	commandQueue = QueueFactory::instance()->
-			createMessageQueue(cmdQueueSize, CommandMessage::MAX_MESSAGE_SIZE);
+		deviceCommunicationId(deviceCommunication), comCookie(comCookie),
+		deviceThermalStatePoolId(thermalStatePoolId),
+		deviceThermalRequestPoolId(thermalRequestPoolId),
+		healthHelper(this,setObjectId), modeHelper(this), parameterHelper(this),
+		childTransitionFailure(RETURN_OK), fdirInstance(fdirInstance),
+		hkSwitcher(this), defaultFDIRUsed(fdirInstance == nullptr),
+		switchOffWasReported(false), actionHelper(this, nullptr), cookieInfo(),
+		childTransitionDelay(5000),
+		transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode(
+		SUBMODE_NONE), deviceSwitch(setDeviceSwitch) {
+	commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
+			CommandMessage::MAX_MESSAGE_SIZE);
 	cookieInfo.state = COOKIE_UNUSED;
 	insertInCommandMap(RAW_COMMAND_ID);
 	if (this->fdirInstance == nullptr) {
-		this->fdirInstance =
-				new DeviceHandlerFailureIsolation(setObjectId,
-						defaultFDIRParentId);
+		this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId,
+				defaultFDIRParentId);
 	}
 }
 
 DeviceHandlerBase::~DeviceHandlerBase() {
+	//communicationInterface->close(cookie);
 	delete comCookie;
 	if (defaultFDIRUsed) {
 		delete fdirInstance;
@@ -53,7 +54,8 @@ DeviceHandlerBase::~DeviceHandlerBase() {
 
 ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
 	this->pstStep = counter;
-	if (counter == 0) {
+
+	if (getComAction() == SEND_WRITE) {
 		cookieInfo.state = COOKIE_UNUSED;
 		readCommandQueue();
 		doStateMachine();
@@ -66,7 +68,6 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
 	if (mode == MODE_OFF) {
 		return RETURN_OK;
 	}
-
 	switch (getComAction()) {
 	case SEND_WRITE:
 		if ((cookieInfo.state == COOKIE_UNUSED)) {
@@ -87,7 +88,6 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
 	default:
 		break;
 	}
-
 	return RETURN_OK;
 }
 
@@ -116,7 +116,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 	AcceptsDeviceResponsesIF *rawReceiver = objectManager->get<
 			AcceptsDeviceResponsesIF>(rawDataReceiverId);
 
-	if (rawReceiver == nullptr) {
+	if (rawReceiver == NULL) {
 		return RETURN_FAILED;
 	}
 
@@ -166,6 +166,7 @@ ReturnValue_t DeviceHandlerBase::initialize() {
 	mySet.commit(PoolVariableIF::VALID);
 
 	return RETURN_OK;
+
 }
 
 void DeviceHandlerBase::decrementDeviceReplyMap() {
@@ -353,8 +354,8 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId,
 	info.delayCycles = 0;
 	info.replyLen = replyLen;
 	info.command = deviceCommandMap.end();
-	std::pair<DeviceReplyIter, bool> result = deviceReplyMap.emplace(replyId, info);
-	if (result.second) {
+	auto resultPair = deviceReplyMap.emplace(replyId, info);
+	if (resultPair.second) {
 		return RETURN_OK;
 	} else {
 		return RETURN_FAILED;
@@ -366,8 +367,8 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm
 	info.expectedReplies = 0;
 	info.isExecuting = false;
 	info.sendReplyTo = NO_COMMANDER;
-	std::pair<DeviceCommandIter, bool> result = deviceCommandMap.emplace(deviceCommand,info);
-	if (result.second) {
+	auto resultPair = deviceCommandMap.emplace(deviceCommand, info);
+	if (resultPair.second) {
 		return RETURN_OK;
 	} else {
 		return RETURN_FAILED;
@@ -487,7 +488,7 @@ void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter,
 		return;
 	}
 //Check if more replies are expected. If so, do nothing.
-	DeviceCommandInfo * info = &(iter->second.command->second);
+	DeviceCommandInfo* info = &(iter->second.command->second);
 	if (--info->expectedReplies == 0) {
 		//Check if it was transition or internal command. Don't send any replies in that case.
 		if (info->sendReplyTo != NO_COMMANDER) {
@@ -526,6 +527,7 @@ void DeviceHandlerBase::doGetWrite() {
 		if (wiretappingMode == RAW) {
 			replyRawData(rawPacket, rawPacketLen, requestedRawTraffic, true);
 		}
+
 		//We need to distinguish here, because a raw command never expects a reply.
 		//(Could be done in eRIRM, but then child implementations need to be careful.
 		result = enableReplyInReplyMap(cookieInfo.pendingCommand);
@@ -541,25 +543,22 @@ void DeviceHandlerBase::doGetWrite() {
 }
 
 void DeviceHandlerBase::doSendRead() {
-	ReturnValue_t result = RETURN_FAILED;
+	ReturnValue_t result;
+
 	size_t requestLen = 0;
-	// If the device handler can only request replies after a command
-	// has been sent, there should be only one reply enabled and the
-	// correct reply length will be mapped.
-	for(DeviceReplyIter iter = deviceReplyMap.begin();
-			iter != deviceReplyMap.end();iter++)
-	{
-		if(iter->second.delayCycles != 0) {
-			requestLen = iter->second.replyLen;
-			break;
-		}
+	DeviceReplyIter iter = deviceReplyMap.find(cookieInfo.pendingCommand->first);
+	if(iter != deviceReplyMap.end()) {
+		requestLen = iter->second.replyLen;
+	}
+	else {
+		requestLen = 0;
 	}
 
 	result = communicationInterface->requestReceiveMessage(comCookie, requestLen);
+
 	if (result == RETURN_OK) {
 		cookieInfo.state = COOKIE_READ_SENT;
-	}
-	else {
+	} else {
 		triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result);
 		//We can't inform anyone, because we don't know which command was sent last.
 		//So, we need to wait for a timeout.
@@ -583,8 +582,8 @@ void DeviceHandlerBase::doGetRead() {
 
 	cookieInfo.state = COOKIE_UNUSED;
 
-	result = communicationInterface->readReceivedMessage(comCookie, &receivedData,
-			&receivedDataLen);
+	result = communicationInterface->readReceivedMessage(comCookie,
+			&receivedData, &receivedDataLen);
 
 	if (result != RETURN_OK) {
 		triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result);
@@ -644,7 +643,7 @@ void DeviceHandlerBase::doGetRead() {
 }
 
 ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress,
-		uint8_t ** data, size_t * len) {
+		uint8_t * *data, uint32_t * len) {
 	size_t lenTmp;
 
 	if (IPCStore == NULL) {
@@ -663,10 +662,8 @@ ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress,
 		*len = 0;
 		return result;
 	}
-
 }
 
-
 void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len,
 		MessageQueueId_t sendTo, bool isCommand) {
 	if (IPCStore == NULL || len == 0) {
@@ -681,6 +678,7 @@ void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len,
 	}
 
 	CommandMessage message;
+
 	DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&message,
 			getObjectId(), address, isCommand);
 
@@ -690,14 +688,12 @@ void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len,
 
 	if (result != RETURN_OK) {
 		IPCStore->deleteData(address);
-		// Silently discard data, this indicates heavy TM traffic which should
-		// not be increased by additional events.
+		//Silently discard data, this indicates heavy TM traffic which should not be increased by additional events.
 	}
 }
 
 //Default child implementations
-
-DeviceHandlerBase::CommunicationAction_t DeviceHandlerBase::getComAction() {
+DeviceHandlerIF::CommunicationAction_t DeviceHandlerBase::getComAction() {
 	switch (pstStep) {
 	case 0:
 		return SEND_WRITE;
@@ -762,8 +758,8 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData,
 //			DeviceCommunicationIF>(newChannelId);
 //
 //	if (newCommunication != NULL) {
-//		ReturnValue_t result = newCommunication->reOpen(cookie, logicalAddress,
-//				maxDeviceReplyLen, comParameter1, comParameter2);
+//		ReturnValue_t result = newCommunication->reOpen(cookie, ioBoardAddress,
+//				maxDeviceReplyLen);
 //		if (result != RETURN_OK) {
 //			return result;
 //		}
@@ -780,8 +776,8 @@ void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) {
 		replyReturnvalueToCommand(result, RAW_COMMAND_ID);
 		storedRawData.raw = StorageManagerIF::INVALID_ADDRESS;
 	} else {
-		cookieInfo.pendingCommand = deviceCommandMap.
-				find((DeviceCommandId_t) RAW_COMMAND_ID);
+		cookieInfo.pendingCommand = deviceCommandMap.find(
+				(DeviceCommandId_t) RAW_COMMAND_ID);
 		cookieInfo.pendingCommand->second.isExecuting = true;
 		cookieInfo.state = COOKIE_WRITE_READY;
 	}
@@ -820,7 +816,7 @@ ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(
 		iter = deviceReplyMap.find(command->first);
 	}
 	if (iter != deviceReplyMap.end()) {
-		DeviceReplyInfo * info = &(iter->second);
+		DeviceReplyInfo *info = &(iter->second);
 		info->delayCycles = info->maxDelayCycles;
 		info->command = command;
 		command->second.expectedReplies = expectedReplies;
@@ -846,9 +842,8 @@ ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) {
 	ReturnValue_t result = getSwitches(&switches, &numberOfSwitches);
 	if ((result == RETURN_OK) && (numberOfSwitches != 0)) {
 		while (numberOfSwitches > 0) {
-			if (powerSwitcher-> getSwitchState(switches[numberOfSwitches - 1])
-				== PowerSwitchIF::SWITCH_OFF)
-			{
+			if (powerSwitcher->getSwitchState(switches[numberOfSwitches - 1])
+					== PowerSwitchIF::SWITCH_OFF) {
 				return PowerSwitchIF::SWITCH_OFF;
 			}
 			numberOfSwitches--;
@@ -1032,7 +1027,6 @@ void DeviceHandlerBase::replyRawReplyIfnotWiretapped(const uint8_t* data,
 
 ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage(
 		CommandMessage * message) {
-	ReturnValue_t result;
 	switch (message->getCommand()) {
 	case DeviceHandlerMessage::CMD_WIRETAPPING:
 		switch (DeviceHandlerMessage::getWiretappingMode(message)) {
@@ -1054,21 +1048,19 @@ ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage(
 		}
 		replyReturnvalueToCommand(RETURN_OK);
 		return RETURN_OK;
-	case DeviceHandlerMessage::CMD_SWITCH_ADDRESS:
-		if (mode != MODE_OFF) {
-			replyReturnvalueToCommand(WRONG_MODE_FOR_COMMAND);
-		} else {
-			// rework in progress
-			result = RETURN_OK;
-			//result = switchCookieChannel(
-			//		DeviceHandlerMessage::getIoBoardObjectId(message));
-			if (result == RETURN_OK) {
-				replyReturnvalueToCommand(RETURN_OK);
-			} else {
-				replyReturnvalueToCommand(CANT_SWITCH_ADDRESS);
-			}
-		}
-		return RETURN_OK;
+//	case DeviceHandlerMessage::CMD_SWITCH_IOBOARD:
+//		if (mode != MODE_OFF) {
+//			replyReturnvalueToCommand(WRONG_MODE_FOR_COMMAND);
+//		} else {
+//			result = switchCookieChannel(
+//					DeviceHandlerMessage::getIoBoardObjectId(message));
+//			if (result == RETURN_OK) {
+//				replyReturnvalueToCommand(RETURN_OK);
+//			} else {
+//				replyReturnvalueToCommand(CANT_SWITCH_IO_ADDRESS);
+//			}
+//		}
+//		return RETURN_OK;
 	case DeviceHandlerMessage::CMD_RAW:
 		if ((mode != MODE_RAW)) {
 			DeviceHandlerMessage::clear(message);
@@ -1124,7 +1116,8 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* data,
 
 			// hiding of sender needed so the service will handle it as unexpected Data, no matter what state
 			//(progress or completed) it is in
-			actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, true);
+			actionHelper.reportData(defaultRawReceiver, replyId, &wrapper,
+			true);
 
 		}
 	} else { //unrequested/aperiodic replies
@@ -1152,7 +1145,6 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId,
 	if (result != HasReturnvaluesIF::RETURN_OK) {
 		return result;
 	}
-
 	DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId);
 	if (iter == deviceCommandMap.end()) {
 		result = COMMAND_NOT_SUPPORTED;
@@ -1171,7 +1163,7 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId,
 }
 
 void DeviceHandlerBase::buildInternalCommand(void) {
-	// Neither Raw nor Direct could build a command
+//Neither Raw nor Direct could build a command
 	ReturnValue_t result = NOTHING_TO_SEND;
 	DeviceCommandId_t deviceCommandId = NO_COMMAND_ID;
 	if (mode == MODE_NORMAL) {
@@ -1189,13 +1181,12 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 	} else {
 		return;
 	}
-
 	if (result == NOTHING_TO_SEND) {
 		return;
 	}
 	if (result == RETURN_OK) {
-		DeviceCommandMap::iterator iter =
-				deviceCommandMap.find(deviceCommandId);
+		DeviceCommandMap::iterator iter = deviceCommandMap.find(
+				deviceCommandId);
 		if (iter == deviceCommandMap.end()) {
 			result = COMMAND_NOT_SUPPORTED;
 		} else if (iter->second.isExecuting) {
@@ -1210,7 +1201,6 @@ void DeviceHandlerBase::buildInternalCommand(void) {
 			cookieInfo.state = COOKIE_WRITE_READY;
 		}
 	}
-
 	if (result != RETURN_OK) {
 		triggerEvent(DEVICE_BUILDING_COMMAND_FAILED, result, deviceCommandId);
 	}
@@ -1285,8 +1275,8 @@ void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_){
 			executingTask = task_;
 }
 
-void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId, uint32_t parameter) {
-}
+// Default implementations empty.
+void DeviceHandlerBase::debugInterface(uint8_t positionTracker,
+		object_id_t objectId, uint32_t parameter) {}
 
-void DeviceHandlerBase::performOperationHook() {
-}
+void DeviceHandlerBase::performOperationHook() {}
diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h
index 729eac0b..454f0751 100644
--- a/devicehandlers/DeviceHandlerBase.h
+++ b/devicehandlers/DeviceHandlerBase.h
@@ -1,26 +1,23 @@
 #ifndef DEVICEHANDLERBASE_H_
 #define DEVICEHANDLERBASE_H_
 
-#include <framework/action/ActionHelper.h>
+#include <framework/objectmanager/SystemObject.h>
+#include <framework/tasks/ExecutableObjectIF.h>
+#include <framework/devicehandlers/DeviceHandlerIF.h>
+#include <framework/returnvalues/HasReturnvaluesIF.h>
 #include <framework/action/HasActionsIF.h>
-#include <framework/datapool/DataSet.h>
 #include <framework/datapool/PoolVariableIF.h>
 #include <framework/devicehandlers/DeviceCommunicationIF.h>
-#include <framework/devicehandlers/DeviceHandlerIF.h>
-#include <framework/health/HealthHelper.h>
 #include <framework/modes/HasModesIF.h>
-#include <framework/objectmanager/SystemObject.h>
-#include <framework/objectmanager/SystemObjectIF.h>
-#include <framework/parameters/ParameterHelper.h>
 #include <framework/power/PowerSwitchIF.h>
-#include <framework/returnvalues/HasReturnvaluesIF.h>
-#include <framework/tasks/ExecutableObjectIF.h>
-#include <framework/devicehandlers/DeviceHandlerFailureIsolation.h>
-#include <framework/datapool/HkSwitchHelper.h>
-#include <framework/devicehandlers/CookieIF.h>
-#include <framework/serialize/SerialFixedArrayListAdapter.h>
 #include <framework/ipc/MessageQueueIF.h>
-#include <framework/tasks/PeriodicTaskIF.h>
+
+#include <framework/action/ActionHelper.h>
+#include <framework/health/HealthHelper.h>
+#include <framework/parameters/ParameterHelper.h>
+#include <framework/datapool/HkSwitchHelper.h>
+#include <framework/devicehandlers/DeviceHandlerFailureIsolation.h>
+
 #include <map>
 
 namespace Factory{
@@ -92,21 +89,22 @@ public:
 	 * The constructor passes the objectId to the SystemObject().
 	 *
 	 * @param setObjectId the ObjectId to pass to the SystemObject() Constructor
-	 * @param maxDeviceReplyLen the largest allowed reply size
+	 * @param maxDeviceReplyLen the length the RMAP getRead call will be sent with
 	 * @param setDeviceSwitch the switch the device is connected to,
-	 *    for devices using two switches, overwrite getSwitches()
-	 * @param deviceCommuncation Communcation Interface object which is
-	 *    used to implement communication functions
+	 * for devices using two switches, overwrite getSwitches()
+	 * @param deviceCommuncation Communcation Interface object which is used
+	 * to implement communication functions
 	 * @param thermalStatePoolId
 	 * @param thermalRequestPoolId
 	 * @param fdirInstance
 	 * @param cmdQueueSize
 	 */
 	DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication,
-			CookieIF * comCookie_, uint8_t setDeviceSwitch,
+			CookieIF * comCookie, uint8_t setDeviceSwitch,
 			uint32_t thermalStatePoolId = PoolVariableIF::NO_PARAMETER,
 			uint32_t thermalRequestPoolId = PoolVariableIF::NO_PARAMETER,
-			FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20);
+			FailureIsolationBase* fdirInstance = nullptr,
+			size_t cmdQueueSize = 20);
 
 	/**
 	 * @brief 	This function is the device handler base core component and is
@@ -213,12 +211,13 @@ protected:
 	/**
 	 * Build the device command to send for normal mode.
 	 *
-	 * This is only called in @c MODE_NORMAL. If multiple submodes for @c MODE_NORMAL are supported,
-	 * different commands can built returned depending on the submode.
+	 * This is only called in @c MODE_NORMAL. If multiple submodes for
+	 * @c MODE_NORMAL are supported, different commands can built,
+	 * depending on the submode.
 	 *
-	 * #rawPacket and #rawPacketLen must be set by this method to the packet to be sent.
-	 * If variable command frequence is required, a counter can be used and
-	 * the frequency in the reply map has to be set manually
+	 * #rawPacket and #rawPacketLen must be set by this method to the
+	 * packet to be sent. If variable command frequence is required, a counter
+	 * can be used and the frequency in the reply map has to be set manually
 	 * by calling updateReplyMap().
 	 *
 	 * @param[out] id the device command id that has been built
@@ -233,10 +232,13 @@ protected:
 	 * Build the device command to send for a transitional mode.
 	 *
 	 * This is only called in @c _MODE_TO_NORMAL, @c _MODE_TO_ON, @c _MODE_TO_RAW,
-	 * @c _MODE_START_UP and @c _MODE_TO_POWER_DOWN. So it is used by doStartUp() and doShutDown() as well as doTransition()
+	 * @c _MODE_START_UP and @c _MODE_TO_POWER_DOWN. So it is used by doStartUp()
+	 * and doShutDown() as well as doTransition()
 	 *
-	 * A good idea is to implement a flag indicating a command has to be built and a variable containing the command number to be built
-	 * and filling them in doStartUp(), doShutDown() and doTransition() so no modes have to be checked here.
+	 * A good idea is to implement a flag indicating a command has to be built
+	 * and a variable containing the command number to be built
+	 * and filling them in doStartUp(), doShutDown() and doTransition() so no
+	 * modes have to be checked here.
 	 *
 	 * #rawPacket and #rawPacketLen must be set by this method to the packet to be sent.
 	 *
@@ -266,6 +268,63 @@ protected:
 	virtual ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
 			const uint8_t * commandData, size_t commandDataLen) = 0;
 
+	/**
+	 * @brief Scans a buffer for a valid reply.
+	 * @details
+	 * This is used by the base class to check the data received for valid packets.
+	 * It only checks if a valid packet starts at @c start.
+	 * It also only checks the structural validy of the packet,
+	 * e.g. checksums lengths and protocol data. No information check is done,
+	 * e.g. range checks etc.
+	 *
+	 * Errors should be reported directly, the base class does NOT report any
+	 * errors based on the return value of this function.
+	 *
+	 * @param start start of remaining buffer to be scanned
+	 * @param len length of remaining buffer to be scanned
+	 * @param[out] foundId the id of the data found in the buffer.
+	 * @param[out] foundLen length of the data found. Is to be set in function,
+	 *                       buffer is scanned at previous position + foundLen.
+	 * @return
+	 *  - @c RETURN_OK a valid packet was found at @c start, @c foundLen is valid
+	 *  - @c RETURN_FAILED no reply could be found starting at @c start,
+	 *    implies @c foundLen is not valid, base class will call scanForReply()
+	 *    again with ++start
+	 *  - @c DeviceHandlerIF::INVALID_DATA a packet was found but it is invalid,
+	 *    e.g. checksum error, implies @c foundLen is valid, can be used to
+	 *    skip some bytes
+	 *  - @c DeviceHandlerIF::LENGTH_MISSMATCH @c len is invalid
+	 *  - @c DeviceHandlerIF::IGNORE_REPLY_DATA Ignore this specific part of
+	 *    the packet
+	 *  - @c DeviceHandlerIF::IGNORE_FULL_PACKET Ignore the packet
+	 *  - @c APERIODIC_REPLY if a valid reply is received that has not been
+	 *    requested by a command, but should be handled anyway
+	 *    (@see also fillCommandAndCookieMap() )
+	 */
+	virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len,
+			DeviceCommandId_t *foundId, size_t *foundLen) = 0;
+
+	/**
+	 * @brief Interpret a reply from the device.
+	 * @details
+	 * This is called after scanForReply() found a valid packet, it can be
+	 * assumed that the length and structure is valid.
+	 * This routine extracts the data from the packet into a DataSet and then
+	 * calls handleDeviceTM(), which either sends a TM packet or stores the
+	 * data in the DataPool depending on whether it was an external command.
+	 * No packet length is given, as it should be defined implicitly by the id.
+	 *
+	 * @param id the id found by scanForReply()
+	 * @param packet
+	 * @return
+	 *     - @c RETURN_OK when the reply was interpreted.
+	 *     - @c RETURN_FAILED when the reply could not be interpreted,
+	 *     e.g. logical errors or range violations occurred
+	 */
+
+	virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
+			const uint8_t *packet) = 0;
+
 	/**
 	 * @brief fill the #deviceCommandMap
 	 *	 	  called by the initialize() of the base class
@@ -312,85 +371,83 @@ protected:
 	virtual void fillCommandAndReplyMap() = 0;
 
 	/**
-	 * @brief Scans a buffer for a valid reply.
-	 * @details
-	 * This is used by the base class to check the data received for valid packets.
-	 * It only checks if a valid packet starts at @c start.
-	 * It also only checks the structural validy of the packet,
-	 * e.g. checksums lengths and protocol data.
-	 * No information check is done, e.g. range checks etc.
-	 *
-	 * Errors should be reported directly,  the base class does NOT report
-	 * any errors based on the returnvalue of this function.
-	 *
-	 * @param start start of remaining buffer to be scanned
-	 * @param len length of remaining buffer to be scanned
-	 * @param[out] foundId the id of the data found in the buffer.
-	 * @param[out] foundLen length of the data found. Is to be set in function,
-	 * 						buffer is scanned at previous position + foundLen.
-	 * @return
-	 *     - @c RETURN_OK a valid packet was found at @c start, @c foundLen is valid
-	 *     - @c RETURN_FAILED no reply could be found starting at @c start,
-	 *             implies @c foundLen is not valid,
-	 *             base class will call scanForReply() again with ++start
-	 *     - @c DeviceHandlerIF::INVALID_DATA a packet was found but it is invalid,
-	 *             e.g. checksum error, implies @c foundLen is valid, can be used to skip some bytes
-	 *     - @c DeviceHandlerIF::LENGTH_MISSMATCH @c len is invalid
-	 *     - @c DeviceHandlerIF::IGNORE_REPLY_DATA Ignore this specific part of the packet
-	 *     - @c DeviceHandlerIF::IGNORE_FULL_PACKET Ignore the packet
-	 *     - @c APERIODIC_REPLY if a valid reply is received that has not been
-	 *             requested by a command, but should be handled anyway
-	 *             (@see also fillCommandAndCookieMap() )
+	 * This is a helper method to facilitate inserting entries in the command map.
+	 * @param deviceCommand	Identifier of the command to add.
+	 * @param maxDelayCycles The maximum number of delay cycles the command
+	 * waits until it times out.
+	 * @param periodic	Indicates if the command is periodic (i.e. it is sent
+	 * by the device repeatedly without request) or not. Default is aperiodic (0)
+	 * @return	- @c RETURN_OK when the command was successfully inserted,
+	 *          - @c RETURN_FAILED else.
 	 */
-	virtual ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
-			DeviceCommandId_t *foundId, size_t *foundLen) = 0;
+	ReturnValue_t insertInCommandAndReplyMap(DeviceCommandId_t deviceCommand,
+			uint16_t maxDelayCycles, size_t replyLen = 0, uint8_t periodic = 0,
+			bool hasDifferentReplyId = false, DeviceCommandId_t replyId = 0);
 
 	/**
-	 * @brief Interpret a reply from the device.
-	 * @details
-	 * This is called after scanForReply() found a valid packet, it can be assumed that the length and structure is valid.
-	 * This routine extracts the data from the packet into a DataSet and then calls handleDeviceTM(), which either sends
-	 * a TM packet or stores the data in the DataPool depending on whether the it was an external command.
-	 * No packet length is given, as it should be defined implicitly by the id.
-	 *
-	 * @param id the id found by scanForReply()
-	 * @param packet
-	 * @return
-	 *     - @c RETURN_OK when the reply was interpreted.
-	 *     - @c RETURN_FAILED when the reply could not be interpreted, eg. logical errors or range violations occurred
+	 * @brief 	This is a helper method to insert replies in the reply map.
+	 * @param deviceCommand	Identifier of the reply to add.
+	 * @param maxDelayCycles The maximum number of delay cycles the reply waits
+	 * until it times out.
+	 * @param periodic	Indicates if the command is periodic (i.e. it is sent
+	 * by the device repeatedly without request) or not. Default is aperiodic (0)
+	 * @return	- @c RETURN_OK when the command was successfully inserted,
+	 *          - @c RETURN_FAILED else.
 	 */
-	virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
-			const uint8_t *packet) = 0;
+	ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand,
+			uint16_t maxDelayCycles, size_t replyLen = 0, uint8_t periodic = 0);
 
 	/**
-	 * set all datapool variables that are update periodically in normal mode invalid
-	 *
-	 * Child classes should provide an implementation which sets all those variables invalid
-	 * which are set periodically during any normal mode.
+	 * @brief 	A simple command to add a command to the commandList.
+	 * @param deviceCommand The command to add
+	 * @return - @c RETURN_OK when the command was successfully inserted,
+	 *         - @c RETURN_FAILED else.
 	 */
-	virtual void setNormalDatapoolEntriesInvalid() = 0;
+	ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand);
+	/**
+	 * @brief 	This is a helper method to facilitate updating entries
+	 *        	in the reply map.
+	 * @param deviceCommand	Identifier of the reply to update.
+	 * @param delayCycles The current number of delay cycles to wait.
+	 * As stated in #fillCommandAndCookieMap, to disable periodic commands,
+	 * this is set to zero.
+	 * @param maxDelayCycles The maximum number of delay cycles the reply waits
+	 * until it times out. By passing 0 the entry remains untouched.
+	 * @param periodic Indicates if the command is periodic (i.e. it is sent
+	 * by the device repeatedly without request) or not.Default is aperiodic (0).
+	 * Warning: The setting always overrides the value that was entered in the map.
+	 * @return - @c RETURN_OK when the command was successfully inserted,
+	 *         - @c RETURN_FAILED else.
+	 */
+	ReturnValue_t updateReplyMapEntry(DeviceCommandId_t deviceReply,
+			uint16_t delayCycles, uint16_t maxDelayCycles,
+			uint8_t periodic = 0);
 
 	/**
 	 * @brief   Can be implemented by child handler to
 	 *          perform debugging
 	 * @details Example: Calling this in performOperation
 	 *          to track values like mode.
-	 * @param 	positionTracker Provide the child handler a way to know where the debugInterface was called
-	 * @param 	objectId Provide the child handler object Id to specify actions for spefic devices
-	 * @param 	parameter Supply a parameter of interest
+	 * @param positionTracker Provide the child handler a way to know
+	 * where the debugInterface was called
+	 * @param objectId Provide the child handler object Id to
+	 * specify actions for spefic devices
+	 * @param parameter Supply a parameter of interest
 	 * Please delete all debugInterface calls in DHB after debugging is finished !
 	 */
-	virtual void debugInterface(uint8_t positionTracker = 0, object_id_t objectId = 0, uint32_t parameter = 0);
+	virtual void debugInterface(uint8_t positionTracker = 0,
+			object_id_t objectId = 0, uint32_t parameter = 0);
 
 	/**
 	 * Get the time needed to transit from modeFrom to modeTo.
 	 *
 	 * Used for the following transitions:
 	 * modeFrom -> modeTo:
-	 *  - MODE_ON -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
-	 *  - MODE_NORMAL -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
-	 *  - MODE_RAW -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
-	 *  - _MODE_START_UP -> MODE_ON (do not include time to set the switches, the base class got you covered)
+	 * MODE_ON -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
+	 * MODE_NORMAL -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
+	 * MODE_RAW -> [MODE_ON, MODE_NORMAL, MODE_RAW, _MODE_POWER_DOWN]
+	 * _MODE_START_UP -> MODE_ON (do not include time to set the switches,
+	 * the base class got you covered)
 	 *
 	 * The default implementation returns 0 !
 	 * @param modeFrom
@@ -408,39 +465,26 @@ protected:
 	 * @param[out] numberOfSwitches length of returned array
 	 * @return
 	 *      - @c RETURN_OK if the parameters were set
-	 *      - @c NO_SWITCH or any other returnvalue if no switches exist
+	 *      - @c RETURN_FAILED if no switches exist
 	 */
 	virtual ReturnValue_t getSwitches(const uint8_t **switches,
 			uint8_t *numberOfSwitches);
 
 	/**
-	 * Can be used to perform device specific periodic operations.
-	 * This is called on the SEND_READ step of the performOperation() call
+	 * @brief 	Hook function for child handlers which is called once per
+	 * 			performOperation(). Default implementation is empty.
 	 */
 	virtual void performOperationHook();
-
-	/**
-	 * The Returnvalues id of this class, required by HasReturnvaluesIF
-	 */
-	static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE;
-
 public:
 	/**
 	 * @param parentQueueId
 	 */
 	virtual void setParentQueue(MessageQueueId_t parentQueueId);
 
-	/**
-	 * This function call handles the execution of external commands as required
-	 * by the HasActionIF.
-	 * @param actionId
-	 * @param commandedBy
-	 * @param data
-	 * @param size
-	 * @return
-	 */
+	/** @brief 	Implementation required for HasActionIF */
 	ReturnValue_t executeAction(ActionId_t actionId,
-			MessageQueueId_t commandedBy, const uint8_t* data, size_t size);
+			MessageQueueId_t commandedBy, const uint8_t* data,
+			size_t size) override;
 
 	Mode_t getTransitionSourceMode() const;
 	Submode_t getTransitionSourceSubMode() const;
@@ -460,6 +504,28 @@ public:
 	virtual MessageQueueId_t getCommandQueue(void) const;
 
 protected:
+	/**
+	 * The Returnvalues id of this class, required by HasReturnvaluesIF
+	 */
+	static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE;
+
+	/**
+	 * These returnvalues can be returned from abstract functions
+	 * to alter the behaviour of DHB.For error values, refer to
+	 * DeviceHandlerIF.h returnvalues.
+	 */
+	static const ReturnValue_t INVALID_CHANNEL = MAKE_RETURN_CODE(4);
+	// Returnvalues for scanForReply()
+	static const ReturnValue_t APERIODIC_REPLY = MAKE_RETURN_CODE(5); //!< This is used to specify for replies from a device which are not replies to requests
+	static const ReturnValue_t IGNORE_REPLY_DATA = MAKE_RETURN_CODE(6); //!< Ignore parts of the received packet
+	static const ReturnValue_t IGNORE_FULL_PACKET = MAKE_RETURN_CODE(7); //!< Ignore full received packet
+
+//	static const ReturnValue_t ONE_SWITCH = MAKE_RETURN_CODE(8);
+//	static const ReturnValue_t TWO_SWITCHES = MAKE_RETURN_CODE(9);
+	static const ReturnValue_t NO_SWITCH = MAKE_RETURN_CODE(10);
+	static const ReturnValue_t COMMAND_MAP_ERROR = MAKE_RETURN_CODE(11);
+	static const ReturnValue_t NOTHING_TO_SEND = MAKE_RETURN_CODE(12);
+
 	//Mode handling error Codes
 	static const ReturnValue_t CHILD_TIMEOUT = MAKE_RETURN_CODE(0xE1);
 	static const ReturnValue_t SWITCH_FAILED = MAKE_RETURN_CODE(0xE2);
@@ -475,7 +541,7 @@ protected:
 	/**
 	 * Size of the #rawPacket.
 	 */
-	size_t rawPacketLen = 0;
+	uint32_t rawPacketLen = 0;
 
 	/**
 	 * The mode the device handler is currently in.
@@ -502,7 +568,7 @@ protected:
 	 * indicates either that all raw messages to and from the device should be sent to #theOneWhoWantsToReadRawTraffic
 	 * or that all device TM should be downlinked to #theOneWhoWantsToReadRawTraffic
 	 */
-	enum WiretappingMode: uint8_t {
+	enum WiretappingMode {
 		OFF = 0, RAW = 1, TM = 2
 	} wiretappingMode;
 
@@ -543,21 +609,19 @@ protected:
 	/**
 	 * Communication object used for device communication
 	 */
-	DeviceCommunicationIF *communicationInterface = nullptr;
+	DeviceCommunicationIF * communicationInterface = nullptr;
 
 	/**
-	 * Cookie used for communication. This is passed to the communication
-	 * interface.
+	 * Cookie used for communication
 	 */
-	CookieIF *comCookie;
+	CookieIF * comCookie;
 
 	struct DeviceCommandInfo {
 		bool isExecuting; //!< Indicates if the command is already executing.
 		uint8_t expectedReplies; //!< Dynamic value to indicate how many replies are expected. Inititated with 0.
 		MessageQueueId_t sendReplyTo; //!< if this is != NO_COMMANDER, DHB was commanded externally and shall report everything to commander.
 	};
-	typedef std::map<DeviceCommandId_t, DeviceCommandInfo> DeviceCommandMap;
-	typedef DeviceCommandMap::iterator DeviceCommandIter;
+	using DeviceCommandMap = std::map<DeviceCommandId_t, DeviceCommandInfo> ;
 
 	/**
 	 * @brief Information about expected replies
@@ -568,15 +632,12 @@ protected:
 		uint16_t maxDelayCycles; //!< The maximum number of cycles the handler should wait for a reply to this command.
 		uint16_t delayCycles; //!< The currently remaining cycles the handler should wait for a reply, 0 means there is no reply expected
 		size_t replyLen = 0; //!< Expected size of the reply.
-		//(Robin): This is a flag, isnt it? could we declare it bool? uint8_t always
-		// gives away the impression that this variable is more than a simple flag
-		// and true/false are also more explicit.
 		uint8_t periodic; //!< if this is !=0, the delayCycles will not be reset to 0 but to maxDelayCycles
 		DeviceCommandMap::iterator command; //!< The command that expects this reply.
 	};
 
-	typedef std::map<DeviceCommandId_t, DeviceReplyInfo> DeviceReplyMap;
-	typedef DeviceReplyMap::iterator DeviceReplyIter;
+	using DeviceReplyMap = std::map<DeviceCommandId_t, DeviceReplyInfo> ;
+	using DeviceReplyIter = DeviceReplyMap::iterator;
 
 	/**
 	 * The MessageQueue used to receive device handler commands and to send replies.
@@ -610,7 +671,7 @@ protected:
 	 * Optional Error code
 	 * Can be set in doStartUp(), doShutDown() and doTransition() to signal cause for Transition failure.
 	 */
-	ReturnValue_t childTransitionFailure = RETURN_OK;
+	ReturnValue_t childTransitionFailure;
 
 	uint32_t ignoreMissedRepliesCount = 0; //!< Counts if communication channel lost a reply, so some missed replys can be ignored.
 
@@ -622,35 +683,13 @@ protected:
 
 	bool switchOffWasReported; //!< Indicates if SWITCH_WENT_OFF was already thrown.
 
-	PeriodicTaskIF* executingTask;//!< Pointer to the task which executes this component, is invalid before setTaskIF was called.
+	PeriodicTaskIF* executingTask = nullptr;//!< Pointer to the task which executes this component, is invalid before setTaskIF was called.
 
 	static object_id_t powerSwitcherId; //!< Object which switches power on and off.
 
 	static object_id_t rawDataReceiverId; //!< Object which receives RAW data by default.
 
 	static object_id_t defaultFDIRParentId; //!< Object which may be the root cause of an identified fault.
-
-	/**
-	 * Set the device handler mode
-	 *
-	 * Sets #timeoutStart with every call.
-	 *
-	 * Sets #transitionTargetMode if necessary so transitional states can be entered from everywhere without breaking the state machine
-	 * (which relies on a correct #transitionTargetMode).
-	 *
-	 * The submode is left unchanged.
-	 *
-	 *
-	 * @param newMode
-	 */
-	void setMode(Mode_t newMode);
-
-	/**
-	 * @overload
-	 * @param submode
-	 */
-	void setMode(Mode_t newMode, Submode_t submode);
-
 	/**
 	 * Helper function to report a missed reply
 	 *
@@ -671,14 +710,30 @@ protected:
 	void replyReturnvalueToCommand(ReturnValue_t status,
 			uint32_t parameter = 0);
 
-	/**
-	 * Send reply to a command, differentiate between raw command
-	 * and normal command.
-	 * @param status
-	 * @param parameter
-	 */
 	void replyToCommand(ReturnValue_t status, uint32_t parameter = 0);
 
+	/**
+	 * Set the device handler mode
+	 *
+	 * Sets #timeoutStart with every call.
+	 *
+	 * Sets #transitionTargetMode if necessary so transitional states can be
+	 * entered from everywhere without breaking the state machine
+	 * (which relies on a correct #transitionTargetMode).
+	 *
+	 * The submode is left unchanged.
+	 *
+	 *
+	 * @param newMode
+	 */
+	void setMode(Mode_t newMode);
+
+	/**
+	 * @overload
+	 * @param submode
+	 */
+	void setMode(Mode_t newMode, Submode_t submode);
+
 	/**
 	 * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW).
 	 *
@@ -708,55 +763,6 @@ protected:
 	 */
 	virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom);
 
-	/**
-	 * This is a helper method to facilitate inserting entries in the command map.
-	 * @param deviceCommand	Identifier of the command to add.
-	 * @param maxDelayCycles The maximum number of delay cycles the command waits until it times out.
-	 * @param periodic	Indicates if the reply is periodic (i.e. it is sent by the device repeatedly without request) or not.
-	 *		 			Default is aperiodic (0)
-	 * @param hasDifferentReplyId
-	 * @param replyId
-	 * @return	RETURN_OK when the command was successfully inserted, COMMAND_MAP_ERROR else.
-	 */
-	ReturnValue_t insertInCommandAndReplyMap(DeviceCommandId_t deviceCommand,
-			uint16_t maxDelayCycles, size_t replyLen = 0, uint8_t periodic = 0,
-			bool hasDifferentReplyId = false, DeviceCommandId_t replyId = 0);
-	/**
-	 * This is a helper method to insert replies in the reply map.
-	 * @param deviceCommand	Identifier of the reply to add.
-	 * @param maxDelayCycles The maximum number of delay cycles the reply waits until it times out.
-	 * @param periodic	Indicates if the command is periodic (i.e. it is sent by the device repeatedly without request) or not.
-	 *		 			Default is aperiodic (0)
-	 * @return	RETURN_OK when the command was successfully inserted, COMMAND_MAP_ERROR else.
-	 */
-	ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand,
-			uint16_t maxDelayCycles, size_t replyLen = 0, uint8_t periodic = 0);
-	/**
-	 * A simple command to add a command to the commandList.
-	 * @param deviceCommand The command to add
-	 * @return RETURN_OK if the command was successfully inserted, RETURN_FAILED else.
-	 */
-	ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand);
-	/**
-	 * This is a helper method to facilitate updating entries in the reply map.
-	 * @param deviceCommand		Identifier of the reply to update.
-	 * @param delayCycles		The current number of delay cycles to wait. As stated in #fillCommandAndCookieMap, to disable periodic commands, this is set to zero.
-	 * @param maxDelayCycles	The maximum number of delay cycles the reply waits until it times out. By passing 0 the entry remains untouched.
-	 * @param periodic			Indicates if the command is periodic (i.e. it is sent by the device repeatedly without request) or not.
-	 *		 					Default is aperiodic (0). Warning: The setting always overrides the value that was entered in the map.
-	 * @return	RETURN_OK when the reply was successfully updated, COMMAND_MAP_ERROR else.
-	 */
-	ReturnValue_t updateReplyMapEntry(DeviceCommandId_t deviceReply,
-			uint16_t delayCycles, uint16_t maxDelayCycles,
-			uint8_t periodic = 0);
-	/**
-	 * Returns the delay cycle count of a reply.
-	 * A count != 0 indicates that the command is already executed.
-	 * @param deviceCommand	The command to look for
-	 * @return	The current delay count. If the command does not exist (should never happen) it returns 0.
-	 */
-	uint8_t getReplyDelayCycles(DeviceCommandId_t deviceCommand);
-
 	/**
 	 * Is the combination of mode and submode valid?
 	 *
@@ -776,6 +782,7 @@ protected:
 	 *
 	 * @return The Rmap action to execute in this step
 	 */
+
 	virtual CommunicationAction_t getComAction();
 
 	/**
@@ -798,6 +805,14 @@ protected:
 	 */
 	virtual ReturnValue_t buildChildRawCommand();
 
+	/**
+	 * Returns the delay cycle count of a reply.
+	 * A count != 0 indicates that the command is already executed.
+	 * @param deviceCommand	The command to look for
+	 * @return	The current delay count. If the command does not exist (should never happen) it returns 0.
+	 */
+	uint8_t getReplyDelayCycles(DeviceCommandId_t deviceCommand);
+
 	/**
 	 * Construct a command reply containing a raw reply.
 	 *
@@ -861,6 +876,14 @@ protected:
 	 */
 	ReturnValue_t getStateOfSwitches(void);
 
+	/**
+	 * set all datapool variables that are update periodically in normal mode invalid
+	 *
+	 * Child classes should provide an implementation which sets all those variables invalid
+	 * which are set periodically during any normal mode.
+	 */
+	virtual void setNormalDatapoolEntriesInvalid() = 0;
+
 	/**
 	 * build a list of sids and pass it to the #hkSwitcher
 	 */
@@ -889,6 +912,7 @@ protected:
 	virtual void startTransition(Mode_t mode, Submode_t submode);
 	virtual void setToExternalControl();
 	virtual void announceMode(bool recursive);
+
 	virtual ReturnValue_t letChildHandleMessage(CommandMessage *message);
 
 	/**
@@ -954,13 +978,12 @@ protected:
 	DeviceCommandMap deviceCommandMap;
 
 	ActionHelper actionHelper;
-
 private:
 
 	/**
 	 * State a cookie is in.
 	 *
-	 * Used to keep track of the state of the communication.
+	 * Used to keep track of the state of the RMAP communication.
 	 */
 	enum CookieState_t {
 		COOKIE_UNUSED,    //!< The Cookie is unused
@@ -1034,20 +1057,22 @@ private:
 	 * - checks whether commanded mode transitions are required and calls handleCommandedModeTransition()
 	 * - does the necessary action for the current mode or calls doChildStateMachine in modes @c MODE_TO_ON and @c MODE_TO_OFF
 	 * - actions that happen in transitions (eg setting a timeout) are handled in setMode()
-	 * - Maybe export this into own class to increase modularity of software
-	 *   and reduce the massive class size ?
 	 */
 	void doStateMachine(void);
 
 	void buildRawDeviceCommand(CommandMessage* message);
 	void buildInternalCommand(void);
 
+//	/**
+//	 * Send a reply with the current mode and submode.
+//	 */
+//	void announceMode(void);
+
 	/**
 	 * Decrement the counter for the timout of replies.
 	 *
 	 * This is called at the beginning of each cycle. It checks whether a reply has timed out (that means a reply was expected
 	 * but not received).
-	 * In case the reply is periodic, the counter is simply set back to a specified value.
 	 */
 	void decrementDeviceReplyMap(void);
 
@@ -1114,8 +1139,8 @@ private:
 	 *   - @c RETURN_FAILED IPCStore is NULL
 	 *   - the return value from the IPCStore if it was not @c RETURN_OK
 	 */
-	ReturnValue_t getStorageData(store_address_t storageAddress,
-			uint8_t ** data, size_t * len);
+	ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data,
+			uint32_t *len);
 
 	/**
 	 * set all switches returned by getSwitches()
@@ -1144,16 +1169,9 @@ private:
 	 *     - @c RETURN_FAILED when cookies could not be changed, eg because the newChannel is not enabled
 	 *     - @c returnvalues of RMAPChannelIF::isActive()
 	 */
-	//ReturnValue_t switchCookieChannel(object_id_t newChannelId);
+	ReturnValue_t switchCookieChannel(object_id_t newChannelId);
 
-	/**
-	 * Handle device handler messages (e.g. commands sent by PUS Service 2)
-	 * @param message
-	 * @return
-	 */
 	ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message);
-
-
 };
 
 #endif /* DEVICEHANDLERBASE_H_ */
diff --git a/devicehandlers/FixedSequenceSlot.cpp b/devicehandlers/FixedSequenceSlot.cpp
index 29e7ee6a..bb97a8e5 100644
--- a/devicehandlers/FixedSequenceSlot.cpp
+++ b/devicehandlers/FixedSequenceSlot.cpp
@@ -16,6 +16,5 @@ FixedSequenceSlot::FixedSequenceSlot(object_id_t handlerId, uint32_t setTime,
 	handler->setTaskIF(executingTask);
 }
 
-FixedSequenceSlot::~FixedSequenceSlot() {
-}
+FixedSequenceSlot::~FixedSequenceSlot() {}
 
diff --git a/devicehandlers/FixedSequenceSlot.h b/devicehandlers/FixedSequenceSlot.h
index b3bac08f..7868cce3 100644
--- a/devicehandlers/FixedSequenceSlot.h
+++ b/devicehandlers/FixedSequenceSlot.h
@@ -13,9 +13,10 @@
 class PeriodicTaskIF;
 
 /**
- * \brief	This 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.
+ * @details	The PollingSlot class is the representation of a single polling
+ * 			sequence table entry.
  */
 class FixedSequenceSlot {
 public:
@@ -37,13 +38,19 @@ public:
 	uint32_t	pollingTimeMs;
 
 	/**
-	 * \brief	This value defines the type of device communication.
+	 * @brief	This value defines the type of device communication.
 	 *
-	 * \details	The state of this value decides what communication routine is
+	 * @details	The state of this value decides what communication routine is
 	 * 			called in the PST executable or the device handler object.
 	 */
 	uint8_t			opcode;
 
+	/**
+	 * @brief 	Operator overload for the comparison operator to
+	 * 			allow sorting by polling time.
+	 * @param fixedSequenceSlot
+	 * @return
+	 */
 	bool operator <(const FixedSequenceSlot & fixedSequenceSlot) const {
 		return pollingTimeMs < fixedSequenceSlot.pollingTimeMs;
 	}
diff --git a/devicehandlers/FixedSlotSequence.cpp b/devicehandlers/FixedSlotSequence.cpp
index 8974510e..aeb97f3a 100644
--- a/devicehandlers/FixedSlotSequence.cpp
+++ b/devicehandlers/FixedSlotSequence.cpp
@@ -8,14 +8,8 @@ FixedSlotSequence::FixedSlotSequence(uint32_t setLengthMs) :
 }
 
 FixedSlotSequence::~FixedSlotSequence() {
-	// This should call the destructor on each list entry.
+	// Call the destructor on each list entry.
 	slotList.clear();
-//	SlotListIter slotListIter = this->slotList.begin();
-//	//Iterate through slotList and delete all entries.
-//	while (slotListIter != this->slotList.end()) {
-//		delete (*slotIt);
-//		slotIt++;
-//	}
 }
 
 void FixedSlotSequence::executeAndAdvance() {
@@ -89,14 +83,15 @@ uint32_t FixedSlotSequence::getLengthMs() const {
 
 ReturnValue_t FixedSlotSequence::checkSequence() const {
 	if(slotList.empty()) {
-	    sif::error << "Fixed Slot Sequence: Slot list is empty!" << std::endl;
-	    return HasReturnvaluesIF::RETURN_FAILED;
+		sif::error << "Fixed Slot Sequence: 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 == NULL) {
+		if (slotIt->handler == nullptr) {
 			sif::error << "FixedSlotSequene::initialize: ObjectId does not exist!"
 					<< std::endl;
 			count++;
diff --git a/devicehandlers/FixedSlotSequence.h b/devicehandlers/FixedSlotSequence.h
index dd9636c1..145cdbc3 100644
--- a/devicehandlers/FixedSlotSequence.h
+++ b/devicehandlers/FixedSlotSequence.h
@@ -3,8 +3,7 @@
 
 #include <framework/devicehandlers/FixedSequenceSlot.h>
 #include <framework/objectmanager/SystemObject.h>
-#include <framework/returnvalues/HasReturnvaluesIF.h>
-#include <list>
+
 #include <set>
 
 using SlotList = std::multiset<FixedSequenceSlot>;
@@ -72,9 +71,10 @@ public:
 	bool slotFollowsImmediately();
 
 	/**
-	 * \brief	This method returns the time until the next software component is invoked.
+	 * @brief	This method returns the time until the next software
+	 * 			component is invoked.
 	 *
-	 * \details
+	 * @details
 	 * This method is vitally important for the operation of the PST.
 	 * By fetching the polling time of the current slot and that of the
 	 * next one (or the first one, if the list end is reached)
@@ -86,11 +86,15 @@ public:
 	uint32_t getIntervalToNextSlotMs();
 
 	/**
-	 * \brief	This method returns the time difference between the current slot and the previous slot
+	 * @brief	This method returns the time difference between the current
+	 * 			slot and the previous slot
 	 *
-	 * \details	This method is vitally important for the operation of the PST. By fetching the polling time
-	 * 			of the current slot and that of the prevous one (or the last one, if the slot is the first one)
-	 * 			it calculates and returns the interval in milliseconds that the handler execution shall be delayed.
+	 * @details
+	 * This method is vitally important for the operation of the PST.
+	 * By fetching the polling time of the current slot and that of the previous
+	 * one (or the last one, if the slot is the first one) it calculates and
+	 * returns the interval in milliseconds that the handler execution shall
+	 * be delayed.
 	 */
 	uint32_t getIntervalToPreviousSlotMs();
 
@@ -100,20 +104,24 @@ public:
 	uint32_t getLengthMs() const;
 
 	/**
-	 * \brief	The method to execute the device handler entered in the current PollingSlot object.
+	 * @brief	The method to execute the device handler entered in the current
+	 * 			PollingSlot object.
 	 *
-	 * \details	Within this method the device handler object to be executed is chosen by looking up the
-	 * 			handler address of the current slot in the handlerMap. Either the device handler's
-	 * 			talkToInterface or its listenToInterface method is invoked, depending on the isTalking flag
-	 * 			of the polling slot. After execution the iterator current is increased or, by reaching the
-	 * 			end of slotList, reset to the beginning.
+	 * @details
+	 * Within this method the device handler object to be executed is chosen by
+	 * looking up the handler address of the current slot in the handlerMap.
+	 * Either the device handler's talkToInterface or its listenToInterface
+	 * method is invoked, depending on the isTalking flag of the polling slot.
+	 * After execution the iterator current is increased or, by reaching the
+	 * end of slotList, reset to the beginning.
 	 */
 	void executeAndAdvance();
 
 	/**
 	 * @brief	An iterator that indicates the current polling slot to execute.
 	 *
-	 * @details	This is an iterator for slotList and always points to the polling slot which is executed next.
+	 * @details	This is an iterator for slotList and always points to the
+	 * 			polling slot which is executed next.
 	 */
 	SlotListIter current;
 
@@ -127,13 +135,15 @@ public:
 protected:
 
 	/**
-	 * @brief	This list contains all PollingSlot objects, defining order and execution time of the
-	 * 			device handler objects.
+	 * @brief	This list contains all PollingSlot objects, defining order and
+	 * 			execution time of the device handler objects.
 	 *
-	 * @details	The slot list is a std:list object that contains all created PollingSlot instances.
-	 * 			They are NOT ordered automatically, so by adding entries, the correct order needs to be ensured.
-	 * 			By iterating through this list the polling sequence is executed. Two entries with identical
-	 * 			polling times are executed immediately one after another.
+	 * @details
+	 * The slot list is a std:list object that contains all created
+	 * PollingSlot instances. They are NOT ordered automatically, so by
+	 * adding entries, the correct order needs to be ensured. By iterating
+	 * through this list the polling sequence is executed. Two entries with
+	 * identical polling times are executed immediately one after another.
 	 */
 	SlotList slotList;
 
diff --git a/globalfunctions/printer.cpp b/globalfunctions/printer.cpp
new file mode 100644
index 00000000..bf4c5848
--- /dev/null
+++ b/globalfunctions/printer.cpp
@@ -0,0 +1,36 @@
+#include <framework/globalfunctions/printer.h>
+#include <framework/serviceinterface/ServiceInterfaceStream.h>
+
+void printer::print(uint8_t *data, size_t size, OutputType type) {
+	sif::info << "Printing data with size " << size << ": [";
+	if(type == OutputType::HEX) {
+		printer::printHex(data, size);
+	}
+	else {
+		printer::printDec(data, size);
+	}
+}
+
+void printer::printHex(uint8_t *data, size_t size) {
+	sif::info << std::hex;
+	for(size_t i = 0; i < size; i++) {
+		sif::info << "0x" << static_cast<int>(data[i]);
+		if(i < size - 1){
+			sif::info << " , ";
+		}
+	}
+	sif::info << std::dec;
+	sif::info << "]" << std::endl;
+}
+
+void printer::printDec(uint8_t *data, size_t size) {
+	sif::info << std::dec;
+	for(size_t i = 0; i < size; i++) {
+		sif::info << "0x" << static_cast<int>(data[i]);
+		if(i < size - 1){
+			sif::info << " , ";
+		}
+	}
+	sif::info << "]" << std::endl;
+}
+
diff --git a/globalfunctions/printer.h b/globalfunctions/printer.h
new file mode 100644
index 00000000..3c9d6aa0
--- /dev/null
+++ b/globalfunctions/printer.h
@@ -0,0 +1,18 @@
+#ifndef FRAMEWORK_GLOBALFUNCTIONS_PRINTER_H_
+#define FRAMEWORK_GLOBALFUNCTIONS_PRINTER_H_
+#include <cstdint>
+#include <cstddef>
+
+namespace printer {
+
+enum class OutputType {
+	DEC,
+	HEX
+};
+
+void print(uint8_t* data, size_t size, OutputType type = OutputType::HEX);
+void printHex(uint8_t* data, size_t size);
+void printDec(uint8_t* data, size_t size);
+}
+
+#endif /* FRAMEWORK_GLOBALFUNCTIONS_PRINTER_H_ */
diff --git a/ipc/FwMessageTypes.h b/ipc/FwMessageTypes.h
index ec1c9aa2..07e3d245 100644
--- a/ipc/FwMessageTypes.h
+++ b/ipc/FwMessageTypes.h
@@ -14,7 +14,8 @@ enum FW_MESSAGE_TYPE {
 	MONITORING,
 	MEMORY,
 	PARAMETER,
-	FW_MESSAGES_COUNT
+	FW_MESSAGES_COUNT,
+	FILE_SYSTEM_MESSAGE
 };
 }
 
diff --git a/ipc/MessageQueueIF.h b/ipc/MessageQueueIF.h
index 47da694c..c67f3870 100644
--- a/ipc/MessageQueueIF.h
+++ b/ipc/MessageQueueIF.h
@@ -36,8 +36,8 @@ public:
 	 * 			lastParnter information as destination. If there was no message received yet
 	 * 			(i.e. lastPartner is zero), an error code is returned.
 	 * @param message	A pointer to a previously created message, which is sent.
-	 * \return RETURN_OK if ok
-	 * \return NO_REPLY_PARTNER Should return NO_REPLY_PARTNER if partner was found
+	 * @return RETURN_OK if ok
+	 * @return NO_REPLY_PARTNER Should return NO_REPLY_PARTNER if partner was found
 	 */
 	virtual ReturnValue_t reply( MessageQueueMessage* message ) = 0;
 
@@ -53,10 +53,12 @@ public:
 
 	/**
 	 * @brief	This function reads available messages from the message queue.
-	 * @details	If data is available it is stored in the passed message pointer. The message's
-	 * 			original content is overwritten and the sendFrom information is stored in the
-	 * 			lastPartner attribute. Else, the lastPartner information remains untouched, the
-	 * 			message's content is cleared and the function returns immediately.
+	 * @details
+	 * If data is available it is stored in the passed message pointer.
+	 * The message's original content is overwritten and the sendFrom
+	 * information is stored in theblastPartner attribute. Else, the lastPartner
+	 * information remains untouched, the message's content is cleared and the
+	 * function returns immediately.
 	 * @param message	A pointer to a message in which the received data is stored.
 	 * @return -@c RETURN_OK on success
 	 *         -@c MessageQueueIF::EMPTY if queue is empty
@@ -90,7 +92,9 @@ public:
 	 * @return -@c RETURN_OK on success
 	 *         -@c MessageQueueIF::FULL if queue is full
 	 */
-	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0;
+	virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo,
+			MessageQueueMessage* message, MessageQueueId_t sentFrom,
+			bool ignoreFault = false ) = 0;
 
 	/**
 	 * @brief	This operation sends a message to the given destination.
@@ -100,7 +104,8 @@ public:
 	 * @param message	A pointer to a previously created message, which is sent.
 	 * @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 ) = 0;
+	virtual ReturnValue_t sendMessage( MessageQueueId_t sendTo,
+			MessageQueueMessage* message, bool ignoreFault = false ) = 0;
 
 	/**
 	 * @brief	The sendToDefaultFrom method sends a queue message to the default destination.
@@ -111,7 +116,8 @@ public:
 	 * @return -@c RETURN_OK on success
 	 *         -@c MessageQueueIF::FULL if queue is full
 	 */
-	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0;
+	virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message,
+			MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0;
 	/**
 	 * @brief	This operation sends a message to the default destination.
 	 * @details	As in the sendMessage method, this function uses the sendToDefault call of the
diff --git a/ipc/MessageQueueMessage.cpp b/ipc/MessageQueueMessage.cpp
index 30e0325c..544d18e8 100644
--- a/ipc/MessageQueueMessage.cpp
+++ b/ipc/MessageQueueMessage.cpp
@@ -8,6 +8,20 @@ MessageQueueMessage::MessageQueueMessage() :
 	memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
 }
 
+MessageQueueMessage::MessageQueueMessage(uint8_t* data, size_t size) :
+		messageSize(this->HEADER_SIZE + size) {
+	if (size <= this->MAX_DATA_SIZE) {
+		memcpy(this->getData(), data, size);
+		this->messageSize = this->HEADER_SIZE + size;
+	}
+	else {
+		sif::warning << "MessageQueueMessage: Passed size larger than maximum"
+				"allowed size! Setting content to 0" << std::endl;
+		memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
+		this->messageSize = this->HEADER_SIZE;
+	}
+}
+
 MessageQueueMessage::~MessageQueueMessage() {
 }
 
@@ -37,23 +51,13 @@ void MessageQueueMessage::setSender(MessageQueueId_t setId) {
 	memcpy(this->internalBuffer, &setId, sizeof(MessageQueueId_t));
 }
 
-MessageQueueMessage::MessageQueueMessage(uint8_t* data, uint32_t size) :
-		messageSize(this->HEADER_SIZE + size) {
-	if (size <= this->MAX_DATA_SIZE) {
-		memcpy(this->getData(), data, size);
-	} else {
-		memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
-		this->messageSize = this->HEADER_SIZE;
-	}
-}
-
 size_t MessageQueueMessage::getMinimumMessageSize() {
 	return this->HEADER_SIZE;
 }
 
 void MessageQueueMessage::print() {
-	sif::debug << "MessageQueueMessage has size: " << this->messageSize << std::hex
-			<< std::endl;
+	sif::debug << "MessageQueueMessage has size: " << this->messageSize <<
+			std::hex << std::endl;
 	for (uint8_t count = 0; count < this->messageSize; count++) {
 		sif::debug << (uint32_t) this->internalBuffer[count] << ":";
 	}
diff --git a/ipc/MessageQueueMessage.h b/ipc/MessageQueueMessage.h
index 8c4ec947..4d82bd88 100644
--- a/ipc/MessageQueueMessage.h
+++ b/ipc/MessageQueueMessage.h
@@ -5,111 +5,129 @@
 #include <stddef.h>
 
 /**
- *	\brief		This class is the representation and data organizer for interprocess messages.
+ *	@brief		This class is the representation and data organizer
+ *				for interprocess messages.
  *
- *	\details	To facilitate and standardize interprocess communication, this class was created
- *				to handle a lightweight "interprocess message protocol". It adds a header with the
- *				sender's queue id to every sent message and defines the maximum total message size.
- *				Specialized messages, such as device commanding messages, can be created by inheriting
- *				from this class and filling the buffer provided by getData with additional content.
- *				If larger amounts of data must be sent between processes, the data shall be stored in
- *				the IPC Store object and only the storage id is passed in a queue message.
- *				The class is used both to generate and send messages and to receive messages from
- *				other tasks.
- *	\ingroup message_queue
+ *	@details
+ *	To facilitate and standardize interprocess communication, this class was
+ *	created to handle a lightweight "interprocess message protocol".
+ *
+ *	It adds a header with the sender's queue id to every sent message and
+ *	defines the maximum total message size. Specialized messages, such as
+ *	device commanding messages, can be created by inheriting from this class
+ *	and filling the buffer provided by getData with additional content.
+ *
+ *	If larger amounts of data must be sent between processes, the data shall
+ *	be stored in the IPC Store object and only the storage id is passed in a
+ *	queue message.The class is used both to generate and send messages and to
+ *	receive messages from other tasks.
+ *	@ingroup message_queue
  */
 class MessageQueueMessage {
 public:
 	/**
-	 * \brief	This constant defines the maximum size of the data content, excluding the header.
-	 * \details	It may be changed if necessary, but in general should be kept as small as possible.
+	 * @brief	The class is initialized empty with this constructor.
+	 * @details	The messageSize attribute is set to the header's size and the
+	 * 			whole content is set to zero.
+	 */
+	MessageQueueMessage();
+	/**
+	 * @brief	With this constructor the class is initialized with the given content.
+	 * @details
+	 * If the passed message size fits into the buffer, the passed data is
+	 * copied to the internal buffer and the messageSize information is set.
+	 * Otherwise, messageSize is set to the header's size and the whole
+	 * content is set to zero.
+	 * @param data	The data to be put in the message.
+	 * @param size	Size of the data to be copied. Must be smaller than
+	 * 				MAX_MESSAGE_SIZE and larger than MIN_MESSAGE_SIZE.
+	 */
+	MessageQueueMessage(uint8_t* data, size_t size);
+	/**
+	 * @brief	The size information of each message is stored in this attribute.
+	 * @details
+	 * It is public to simplify usage and to allow for passing the size
+	 * address as a pointer. Care must be taken when inheriting from this class,
+	 * as every child class is responsible for managing the size information by
+	 * itself. When using the class to receive a message, the size information
+	 * is updated automatically.
+	 *
+	 * Please note that the minimum size is limited by the size of the header
+	 * while the maximum size is limited by the maximum allowed message size.
+	 */
+	size_t messageSize;
+	/**
+	 * @brief	This constant defines the maximum size of the data content, excluding the header.
+	 * @details	It may be changed if necessary, but in general should be kept as small as possible.
 	 */
 	static const size_t MAX_DATA_SIZE = 24;
 	/**
-	 * \brief	This constants defines the size of the header, which is added to every message.
+	 * @brief	This constants defines the size of the header, which is added to every message.
 	 */
 	static const size_t HEADER_SIZE = sizeof(MessageQueueId_t);
 	/**
-	 * \brief	This constant defines the maximum total size in bytes of a sent message.
-	 * \details	It is the sum of the maximum data and the header size. Be aware that this constant
+	 * @brief	This constant defines the maximum total size in bytes of a sent message.
+	 * @details	It is the sum of the maximum data and the header size. Be aware that this constant
 	 * 			is used to define the buffer sizes for every message queue in the system. So, a change
 	 * 			here may have significant impact on the required resources.
 	 */
 	static const size_t MAX_MESSAGE_SIZE = MAX_DATA_SIZE + HEADER_SIZE;
+	/**
+	 * @brief Defines the minimum size of a message where only the
+	 * 		  header is included
+	 */
+	static const size_t MIN_MESSAGE_SIZE = HEADER_SIZE;
 private:
 	/**
-	 * \brief	This is the internal buffer that contains the actual message data.
+	 * @brief	This is the internal buffer that contains the actual message data.
 	 */
 	uint8_t internalBuffer[MAX_MESSAGE_SIZE];
 public:
 	/**
-	 * \brief	The size information of each message is stored in this attribute.
-	 * \details	It is public to simplify usage and to allow for passing the variable's address as a
-	 * 			pointer. Care must be taken when inheriting from this class, as every child class is
-	 * 			responsible for managing the size information by itself. When using the class to
-	 * 			receive a message, the size information is updated automatically.
-	 */
-	size_t messageSize;
-	/**
-	 * \brief	The class is initialized empty with this constructor.
-	 * \details	The messageSize attribute is set to the header's size and the whole content is set to
-	 * 			zero.
-	 */
-	MessageQueueMessage();
-	/**
-	 * \brief	With this constructor the class is initialized with the given content.
-	 * \details	If the passed message size fits into the buffer, the passed data is copied to the
-	 * 			internal buffer and the messageSize information is set. Otherwise, messageSize
-	 * 			is set to the header's size and the whole content is set to zero.
-	 * \param data	The data to be put in the message.
-	 * \param size	Size of the data to be copied. Must be smaller than MAX_MESSAGE_SIZE.
-	 */
-	MessageQueueMessage(uint8_t* data, uint32_t size);
-	/**
-	 * \brief	As no memory is allocated in this class, the destructor is empty.
+	 * @brief	As no memory is allocated in this class, the destructor is empty.
 	 */
 	virtual ~MessageQueueMessage();
 	/**
-	 * \brief	This method is used to get the complete data of the message.
+	 * @brief	This method is used to get the complete data of the message.
 	 */
 	const uint8_t* getBuffer() const;
 	/**
-	 * \brief	This method is used to get the complete data of the message.
+	 * @brief	This method is used to get the complete data of the message.
 	 */
 	uint8_t* getBuffer();
 	/**
-	 * \brief	This method is used to fetch the data content of the message.
-	 * \details It shall be used by child classes to add data at the right position.
+	 * @brief	This method is used to fetch the data content of the message.
+	 * @details It shall be used by child classes to add data at the right position.
 	 */
 	const uint8_t* getData() const;
 	/**
-	 * \brief	This method is used to fetch the data content of the message.
-	 * \details It shall be used by child classes to add data at the right position.
+	 * @brief	This method is used to fetch the data content of the message.
+	 * @details It shall be used by child classes to add data at the right position.
 	 */
 	uint8_t* getData();
 	/**
-	 * \brief	This method is used to extract the sender's message queue id information from a
+	 * @brief	This method is used to extract the sender's message queue id information from a
 	 * 			received message.
 	 */
 	MessageQueueId_t getSender() const;
 	/**
-	 * \brief	With this method, the whole content and the message size is set to zero.
+	 * @brief	With this method, the whole content and the message size is set to zero.
 	 */
 	void clear();
 	/**
-	 * \brief	This is a debug method that prints the content (till messageSize) to the debug output.
+	 * @brief	This is a debug method that prints the content (till messageSize) to the debug output.
 	 */
 	void print();
 	/**
-	 * \brief	This method is used to set the sender's message queue id information prior to
+	 * @brief	This method is used to set the sender's message queue id information prior to
 	 * 			sending the message.
-	 * \param setId	The message queue id that identifies the sending message queue.
+	 * @param setId	The message queue id that identifies the sending message queue.
 	 */
 	void setSender(MessageQueueId_t setId);
 	/**
-	 * \brief	This helper function is used by the MessageQueue class to check the size of an
+	 * @brief	This helper function is used by the MessageQueue class to check the size of an
 	 * 			incoming message.
-	 * \details	The method must be overwritten by child classes if size checks shall be more strict.
+	 * @details	The method must be overwritten by child classes if size checks shall be more strict.
 	 * @return	The default implementation returns HEADER_SIZE.
 	 */
 	virtual size_t getMinimumMessageSize();
diff --git a/ipc/MutexHelper.h b/ipc/MutexHelper.h
index f76ccec4..671cd5a6 100644
--- a/ipc/MutexHelper.h
+++ b/ipc/MutexHelper.h
@@ -10,11 +10,11 @@ public:
 			internalMutex(mutex) {
 		ReturnValue_t status = mutex->lockMutex(timeoutMs);
 		if(status != HasReturnvaluesIF::RETURN_OK){
-			sif::error << "MutexHelper: Lock of Mutex failed " << status << std::endl;
+			sif::error << "MutexHelper: Lock of Mutex failed " <<
+					status << std::endl;
 		}
 	}
 
-
 	~MutexHelper() {
 		internalMutex->unlockMutex();
 	}
diff --git a/ipc/QueueFactory.h b/ipc/QueueFactory.h
index c385b15d..9a883fd8 100644
--- a/ipc/QueueFactory.h
+++ b/ipc/QueueFactory.h
@@ -2,7 +2,7 @@
 #define FRAMEWORK_IPC_QUEUEFACTORY_H_
 
 #include <framework/ipc/MessageQueueIF.h>
-#include <stdint.h>
+#include <cstdint>
 /**
  * Creates message queues.
  * This class is a "singleton" interface, i.e. it provides an
@@ -18,8 +18,8 @@ public:
 	 */
 	static QueueFactory* instance();
 
-	MessageQueueIF* createMessageQueue(uint32_t message_depth = 3,
-			uint32_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE);
+	MessageQueueIF* createMessageQueue(size_t messageDepth = 3,
+			size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
 
 	void deleteMessageQueue(MessageQueueIF* queue);
 private:
diff --git a/memory/FileSystemMessage.cpp b/memory/FileSystemMessage.cpp
new file mode 100644
index 00000000..efbc8009
--- /dev/null
+++ b/memory/FileSystemMessage.cpp
@@ -0,0 +1,28 @@
+/*
+ * FileSystemMessage.cpp
+ *
+ *  Created on: 19.01.2020
+ *      Author: Jakob Meier
+ */
+
+#include "FileSystemMessage.h"
+#include <framework/objectmanager/ObjectManagerIF.h>
+
+ReturnValue_t FileSystemMessage::setWriteToFileCommand(CommandMessage* message,
+		MessageQueueId_t replyQueueId, store_address_t storageID) {
+	message->setCommand(WRITE_TO_FILE);
+	message->setParameter(replyQueueId);
+	message->setParameter2(storageID.raw);
+	return HasReturnvaluesIF::RETURN_OK;
+}
+
+store_address_t FileSystemMessage::getStoreID(const CommandMessage* message) {
+	store_address_t temp;
+	temp.raw = message->getParameter2();
+	return temp;
+}
+
+MessageQueueId_t FileSystemMessage::getReplyQueueId(const CommandMessage* message){
+	return message->getParameter();
+}
+
diff --git a/memory/FileSystemMessage.h b/memory/FileSystemMessage.h
new file mode 100644
index 00000000..b6fd7309
--- /dev/null
+++ b/memory/FileSystemMessage.h
@@ -0,0 +1,30 @@
+/*
+ * FileSystemMessage.h
+ *
+ *  Created on: 19.01.2020
+ *      Author: Jakob Meier
+ */
+
+#ifndef FRAMEWORK_MEMORY_FILESYSTEMMESSAGE_H_
+#define FRAMEWORK_MEMORY_FILESYSTEMMESSAGE_H_
+
+#include <framework/ipc/CommandMessage.h>
+#include <framework/storagemanager/StorageManagerIF.h>
+#include <framework/objectmanager/SystemObject.h>
+
+class FileSystemMessage {
+private:
+	FileSystemMessage(); //A private ctor inhibits instantiation
+public:
+	static const uint8_t MESSAGE_ID = MESSAGE_TYPE::FILE_SYSTEM_MESSAGE;
+	static const Command_t CREATE_FILE = MAKE_COMMAND_ID( 0x01 );
+	static const Command_t DELETE_FILE = MAKE_COMMAND_ID( 0x02 );
+	static const Command_t WRITE_TO_FILE = MAKE_COMMAND_ID( 0x80 );
+
+	static ReturnValue_t setWriteToFileCommand(CommandMessage* message, MessageQueueId_t replyToQueue, store_address_t storageID );
+	static store_address_t getStoreID( const CommandMessage* message );
+	static MessageQueueId_t getReplyQueueId(const CommandMessage* message);
+
+};
+
+#endif /* FRAMEWORK_MEMORY_FILESYSTEMMESSAGE_H_ */
diff --git a/memory/HasFileSystemIF.h b/memory/HasFileSystemIF.h
new file mode 100644
index 00000000..8b66f88b
--- /dev/null
+++ b/memory/HasFileSystemIF.h
@@ -0,0 +1,36 @@
+/*
+ * HasFileSystemIF.h
+ *
+ *  Created on: 19.01.2020
+ *      Author: Jakob Meier
+ */
+
+#ifndef FRAMEWORK_MEMORY_HASFILESYSTEMIF_H_
+#define FRAMEWORK_MEMORY_HASFILESYSTEMIF_H_
+
+#include <framework/returnvalues/HasReturnvaluesIF.h>
+
+class HasFileSystemIF {
+public:
+
+	virtual ~HasFileSystemIF() {}
+	/**
+	 * Function to get the MessageQueueId_t of the implementing object
+	 * @return MessageQueueId_t of the object
+	 */
+	virtual MessageQueueId_t getCommandQueue() const = 0;
+	/**
+	 * Function to write to a file
+	 * @param dirname Directory of the file
+	 * @param filename The filename of the file
+	 * @param data The data to write to the file
+	 * @param size The size of the data to write
+	 * @param packetNumber Counts the number of packets. For large files the write procedure must be split in multiple calls to writeToFile
+	 */
+	virtual ReturnValue_t writeToFile(const char* dirname, char* filename, const uint8_t* data, uint32_t size, uint16_t packetNumber) = 0;
+	virtual ReturnValue_t createFile(const char* dirname, const char* filename, const uint8_t* data, uint32_t size) = 0;
+	virtual ReturnValue_t deleteFile(const char* dirname, const char* filename) = 0;
+};
+
+
+#endif /* FRAMEWORK_MEMORY_HASFILESYSTEMIF_H_ */
diff --git a/memory/MemoryMessage.cpp b/memory/MemoryMessage.cpp
index c1116e08..f978ed4e 100644
--- a/memory/MemoryMessage.cpp
+++ b/memory/MemoryMessage.cpp
@@ -31,12 +31,11 @@ ReturnValue_t MemoryMessage::setMemoryDumpReply(CommandMessage* message, store_a
 	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) {
diff --git a/memory/MemoryMessage.h b/memory/MemoryMessage.h
index 124fad08..bcc262e9 100644
--- a/memory/MemoryMessage.h
+++ b/memory/MemoryMessage.h
@@ -24,7 +24,7 @@ public:
 	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 void 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 );
diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h
index 3a23e339..3575fc16 100644
--- a/objectmanager/ObjectManagerIF.h
+++ b/objectmanager/ObjectManagerIF.h
@@ -9,7 +9,6 @@
 #define OBJECTMANAGERIF_H_
 
 #include <framework/objectmanager/frameworkObjects.h>
-#include <config/objects/systemObjectList.h>
 #include <framework/objectmanager/SystemObjectIF.h>
 #include <framework/returnvalues/HasReturnvaluesIF.h>
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
diff --git a/osal/FreeRTOS/FixedTimeslotTask.cpp b/osal/FreeRTOS/FixedTimeslotTask.cpp
index d1c3fe3a..18ec1092 100644
--- a/osal/FreeRTOS/FixedTimeslotTask.cpp
+++ b/osal/FreeRTOS/FixedTimeslotTask.cpp
@@ -67,8 +67,8 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId,
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
-	sif::error << "Component " << std::hex << componentId
-		  << " not found, not adding it to pst" << std::endl;
+	sif::error << "Component " << std::hex << componentId <<
+			" not found, not adding it to pst" << std::endl;
 	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index e7de799c..38f425a4 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -2,12 +2,14 @@
 
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 
-// TODO I guess we should have a way of checking if we are in an ISR and then use the "fromISR" versions of all calls
-// As a first step towards this, introduces system context variable which needs to be switched manually
+// TODO I guess we should have a way of checking if we are in an ISR and then
+// use the "fromISR" versions of all calls
+// As a first step towards this, introduces system context variable which needs
+// to be switched manually
 // Haven't found function to find system context.
-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) :
 defaultDestination(0),lastPartner(0), callContext(CallContext::task)  {
-	handle = xQueueCreate(message_depth, max_message_size);
+	handle = xQueueCreate(messageDepth, maxMessageSize);
 	if (handle == NULL) {
 		sif::error << "MessageQueue creation failed" << std::endl;
 	}
@@ -120,16 +122,16 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     message->setSender(sentFrom);
     BaseType_t result;
     if(callContext == CallContext::task) {
-        result = xQueueSendToBack(reinterpret_cast<void*>(sendTo),
-                reinterpret_cast<const void*>(message->getBuffer()), 0);
+        result = xQueueSendToBack(reinterpret_cast<QueueHandle_t>(sendTo),
+                static_cast<const void*>(message->getBuffer()), 0);
     }
     else {
         // If the call context is from an interrupt,
         // request a context switch if a higher priority task
         // was blocked by the interrupt.
         BaseType_t xHigherPriorityTaskWoken = pdFALSE;
-        result = xQueueSendFromISR(reinterpret_cast<void*>(sendTo),
-                reinterpret_cast<const void*>(message->getBuffer()),
+        result = xQueueSendFromISR(reinterpret_cast<QueueHandle_t>(sendTo),
+                static_cast<const void*>(message->getBuffer()),
                 &xHigherPriorityTaskWoken);
         if(xHigherPriorityTaskWoken == pdTRUE) {
             TaskManagement::requestContextSwitch(callContext);
diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h
index 27076869..ce6ed46b 100644
--- a/osal/FreeRTOS/MessageQueue.h
+++ b/osal/FreeRTOS/MessageQueue.h
@@ -6,8 +6,11 @@
 #include <framework/ipc/MessageQueueMessage.h>
 #include <framework/osal/FreeRTOS/TaskManagement.h>
 
-#include <FreeRTOS.h>
-#include "queue.h"
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/queue.h>
+}
+
 
 
 //TODO this class assumes that MessageQueueId_t is the same size as void* (the FreeRTOS handle type), compiler will catch this but it might be nice to have something checking or even an always working solution
diff --git a/osal/FreeRTOS/Mutex.h b/osal/FreeRTOS/Mutex.h
index 604511b5..ef93fd34 100644
--- a/osal/FreeRTOS/Mutex.h
+++ b/osal/FreeRTOS/Mutex.h
@@ -3,9 +3,11 @@
 
 #include <framework/ipc/MutexIF.h>
 
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/semphr.h>
+}
 
-#include <FreeRTOS.h>
-#include "semphr.h"
 
 /**
  * @brief OS component to implement MUTual EXclusion
diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp
index 3075be79..a8e40561 100644
--- a/osal/FreeRTOS/PeriodicTask.cpp
+++ b/osal/FreeRTOS/PeriodicTask.cpp
@@ -1,6 +1,7 @@
+#include "PeriodicTask.h"
+
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 #include <framework/tasks/ExecutableObjectIF.h>
-#include "PeriodicTask.h"
 
 PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority,
 		TaskStackSize setStack, TaskPeriod setPeriod,
@@ -31,7 +32,7 @@ void PeriodicTask::taskEntryPoint(void* argument) {
 	// if it is not set and we get here, the scheduler was started before #startTask() was called and we need to suspend
 	// if it is set, the scheduler was not running before #startTask() was called and we can continue
 
-	if (!originalTask->started) {
+	if (not originalTask->started) {
 		vTaskSuspend(NULL);
 	}
 
@@ -70,8 +71,19 @@ void PeriodicTask::taskFunctionality() {
 				it != objectList.end(); ++it) {
 			(*it)->performOperation();
 		}
-		//TODO deadline missed check
+
+		/* If all operations are finished and the difference of the
+		 * current time minus the last wake time is larger than the
+		 * wait period, a deadline was missed. */
+		if(xTaskGetTickCount() - xLastWakeTime >= xPeriod) {
+			sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) <<
+					" missed deadline!\n" << std::flush;
+			if(deadlineMissedFunc != nullptr) {
+				this->deadlineMissedFunc();
+			}
+		}
 		vTaskDelayUntil(&xLastWakeTime, xPeriod);
+
 	}
 }
 
diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h
index 02ab7148..4ece20b3 100644
--- a/osal/FreeRTOS/PeriodicTask.h
+++ b/osal/FreeRTOS/PeriodicTask.h
@@ -5,8 +5,10 @@
 #include <framework/tasks/PeriodicTaskIF.h>
 #include <framework/tasks/Typedef.h>
 
-#include <FreeRTOS.h>
-#include "task.h"
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
 
 #include <vector>
 
diff --git a/osal/FreeRTOS/QueueFactory.cpp b/osal/FreeRTOS/QueueFactory.cpp
index eaf245d3..a4ee5cf3 100644
--- a/osal/FreeRTOS/QueueFactory.cpp
+++ b/osal/FreeRTOS/QueueFactory.cpp
@@ -3,16 +3,18 @@
 #include <framework/osal/FreeRTOS/MessageQueue.h>
 
 
-QueueFactory* QueueFactory::factoryInstance = NULL;
+QueueFactory* QueueFactory::factoryInstance = nullptr;
 
 
 ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
-			MessageQueueMessage* message, MessageQueueId_t sentFrom,bool ignoreFault) {
-	return MessageQueue::sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
+			MessageQueueMessage* message, MessageQueueId_t sentFrom,
+			bool ignoreFault) {
+	return MessageQueue::sendMessageFromMessageQueue(sendTo,message,
+			sentFrom,ignoreFault);
 }
 
 QueueFactory* QueueFactory::instance() {
-	if (factoryInstance == NULL) {
+	if (factoryInstance == nullptr) {
 		factoryInstance = new QueueFactory;
 	}
 	return factoryInstance;
@@ -24,9 +26,9 @@ QueueFactory::QueueFactory() {
 QueueFactory::~QueueFactory() {
 }
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t message_depth,
-		uint32_t max_message_size) {
-	return new MessageQueue(message_depth, max_message_size);
+MessageQueueIF* QueueFactory::createMessageQueue(size_t messageDepth,
+		size_t maxMessageSize) {
+	return new MessageQueue(messageDepth, maxMessageSize);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) {
diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h
index da0cce2a..eaaf97e2 100644
--- a/osal/FreeRTOS/TaskManagement.h
+++ b/osal/FreeRTOS/TaskManagement.h
@@ -4,8 +4,8 @@
 #include <framework/returnvalues/HasReturnvaluesIF.h>
 
 extern "C" {
-#include "FreeRTOS.h"
-#include "task.h"
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
 }
 #include <cstdint>
 
diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index edabe946..96d9fb4d 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -7,15 +7,15 @@
 #include <framework/osal/linux/MessageQueue.h>
 
 
-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) :
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) :
 		id(0), lastPartner(0), defaultDestination(NO_QUEUE) {
 	//debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl;
 	mq_attr attributes;
 	this->id = 0;
 	//Set attributes
 	attributes.mq_curmsgs = 0;
-	attributes.mq_maxmsg = message_depth;
-	attributes.mq_msgsize = max_message_size;
+	attributes.mq_maxmsg = messageDepth;
+	attributes.mq_msgsize = maxMessageSize;
 	attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open
 	//Set the name of the queue
 	sprintf(name, "/Q%u\n", queueCounter++);
@@ -265,7 +265,11 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
 			           << strerror(errno) << " in mq_send" << std::endl;
 			/*NO BREAK*/
 		case EMSGSIZE:
-			//The msg_len is greater than the msgsize associated with the specified queue.
+			// The msg_len is greater than the msgsize associated with
+			//the specified queue.
+			sif::error << "MessageQueue::sendMessage: Size error [" <<
+					strerror(errno) << "] in mq_send" << std::endl;
+			/*NO BREAK*/
 		default:
 			return HasReturnvaluesIF::RETURN_FAILED;
 		}
diff --git a/osal/linux/MessageQueue.h b/osal/linux/MessageQueue.h
index b8285dc5..fdc8dab1 100644
--- a/osal/linux/MessageQueue.h
+++ b/osal/linux/MessageQueue.h
@@ -5,20 +5,23 @@
 #include <framework/ipc/MessageQueueIF.h>
 #include <framework/ipc/MessageQueueMessage.h>
 /**
- *	@brief		This class manages sending and receiving of message queue messages.
+ * @brief		This class manages sending and receiving of message queue messages.
  *
- *	@details	Message queues are used to pass asynchronous messages between processes.
- *				They work like post boxes, where all incoming messages are stored in FIFO
- *				order. This class creates a new receiving queue and provides methods to fetch
- *				received messages. Being a child of MessageQueueSender, this class also provides
- *				methods to send a message to a user-defined or a default destination. In addition
- *				it also provides a reply method to answer to the queue it received its last message
- *				from.
- *				The MessageQueue should be used as "post box" for a single owning object. So all
- *				message queue communication is "n-to-one".
- *				For creating the queue, as well as sending and receiving messages, the class makes
- *				use of the operating system calls provided.
- *	\ingroup message_queue
+ * @details
+ * Message queues are used to pass asynchronous messages between processes.
+ * They work like post boxes, where all incoming messages are stored in FIFO
+ * order. This class creates a new receiving queue and provides methods to fetch
+ * received messages. Being a child of MessageQueueSender, this class also
+ * provides methods to send a message to a user-defined or a default destination.
+ * In addition it also provides a reply method to answer to the queue it
+ * received its last message from.
+ *
+ * The MessageQueue should be used as "post box" for a single owning object.
+ * So all message queue communication is "n-to-one".
+ *
+ * The creation of message queues, as well as sending and receiving messages,
+ * makes use of the operating system calls provided.
+ *	@ingroup message_queue
  */
 class MessageQueue : public MessageQueueIF {
 	friend class MessageQueueSenderIF;
@@ -35,7 +38,8 @@ public:
 	 * @param max_message_size	With this parameter, the maximum message size can be adjusted.
 	 * 							This should be left default.
 	 */
-	MessageQueue( size_t message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE );
+	MessageQueue(size_t messageDepth = 3,
+			size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE );
 	/**
 	 * @brief	The destructor deletes the formerly created message queue.
 	 * @details	This is accomplished by using the delete call provided by the operating system.
diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp
index b754c3f4..28a0b946 100644
--- a/osal/linux/PeriodicPosixTask.cpp
+++ b/osal/linux/PeriodicPosixTask.cpp
@@ -3,9 +3,10 @@
 #include <errno.h>
 #include <framework/osal/linux/PeriodicPosixTask.h>
 
-PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()):PosixThread(name_,priority_,stackSize_),objectList(),started(false),periodMs(period_),deadlineMissedFunc(
-		deadlineMissedFunc_) {
-
+PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_,
+		size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()):
+		PosixThread(name_,priority_,stackSize_),objectList(),started(false),
+		periodMs(period_),deadlineMissedFunc(deadlineMissedFunc_) {
 }
 
 PeriodicPosixTask::~PeriodicPosixTask() {
diff --git a/osal/linux/QueueFactory.cpp b/osal/linux/QueueFactory.cpp
index fc4c9026..2bacb815 100644
--- a/osal/linux/QueueFactory.cpp
+++ b/osal/linux/QueueFactory.cpp
@@ -5,16 +5,18 @@
 #include <framework/serviceinterface/ServiceInterfaceStream.h>
 #include <cstring>
 
-QueueFactory* QueueFactory::factoryInstance = NULL;
+QueueFactory* QueueFactory::factoryInstance = nullptr;
 
 
 ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
-			MessageQueueMessage* message, MessageQueueId_t sentFrom,bool ignoreFault) {
-	return MessageQueue::sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault);
+			MessageQueueMessage* message, MessageQueueId_t sentFrom,
+			bool ignoreFault) {
+	return MessageQueue::sendMessageFromMessageQueue(sendTo,message,
+			sentFrom,ignoreFault);
 }
 
 QueueFactory* QueueFactory::instance() {
-	if (factoryInstance == NULL) {
+	if (factoryInstance == nullptr) {
 		factoryInstance = new QueueFactory;
 	}
 	return factoryInstance;
@@ -26,9 +28,9 @@ QueueFactory::QueueFactory() {
 QueueFactory::~QueueFactory() {
 }
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t message_depth,
-		uint32_t max_message_size) {
-	return new MessageQueue(message_depth, max_message_size);
+MessageQueueIF* QueueFactory::createMessageQueue(size_t messageDepth,
+		size_t maxMessageSize) {
+	return new MessageQueue(messageDepth, maxMessageSize);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) {
diff --git a/storagemanager/LocalPool.h b/storagemanager/LocalPool.h
index a6334458..6892b881 100644
--- a/storagemanager/LocalPool.h
+++ b/storagemanager/LocalPool.h
@@ -6,7 +6,8 @@
 #include <framework/storagemanager/StorageManagerIF.h>
 #include <framework/objectmanager/ObjectManagerIF.h>
 #include <framework/internalError/InternalErrorReporterIF.h>
-#include <string.h>
+#include <framework/storagemanager/StorageAccessor.h>
+#include <cstring>
 
 /**
  * @brief	The LocalPool class provides an intermediate data storage with
@@ -67,10 +68,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;
diff --git a/storagemanager/LocalPool.tpp b/storagemanager/LocalPool.tpp
index c46e1a8d..59d06ab8 100644
--- a/storagemanager/LocalPool.tpp
+++ b/storagemanager/LocalPool.tpp
@@ -121,8 +121,8 @@ 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,
+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) {
 	ReturnValue_t status = reserveSpace(size, storageId, ignoreFault);
 	if (status == RETURN_OK) {
@@ -144,6 +144,26 @@ 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) {
@@ -153,6 +173,26 @@ inline ReturnValue_t LocalPool<NUMBER_OF_POOLS>::getData(
 	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) {
diff --git a/storagemanager/PoolManager.h b/storagemanager/PoolManager.h
index 6e6c7613..87670a82 100644
--- a/storagemanager/PoolManager.h
+++ b/storagemanager/PoolManager.h
@@ -1,9 +1,9 @@
 #ifndef POOLMANAGER_H_
 #define POOLMANAGER_H_
 
-
 #include <framework/storagemanager/LocalPool.h>
 #include <framework/ipc/MutexHelper.h>
+#include <framework/storagemanager/StorageAccessor.h>
 
 /**
  * @brief	The PoolManager class provides an intermediate data storage with
@@ -11,23 +11,21 @@
  * @details	Uses local pool calls but is thread safe by protecting the call
  * 			with a lock.
  */
-
-template <uint8_t NUMBER_OF_POOLS = 5>
+template <uint8_t NUMBER_OF_POOLS>
 class PoolManager : public LocalPool<NUMBER_OF_POOLS> {
 public:
-	PoolManager( object_id_t setObjectId, 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.
-	 */
+	PoolManager(object_id_t setObjectId,
+			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.
 	virtual ~PoolManager();
 
+	//! @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;
 protected:
 	ReturnValue_t reserveSpace(const uint32_t size, store_address_t* address,
 			bool ignoreFault) override;
diff --git a/storagemanager/PoolManager.tpp b/storagemanager/PoolManager.tpp
index ed340b91..7025d795 100644
--- a/storagemanager/PoolManager.tpp
+++ b/storagemanager/PoolManager.tpp
@@ -1,3 +1,6 @@
+#ifndef POOLMANAGER_TPP_
+#define POOLMANAGER_TPP_
+
 template<uint8_t NUMBER_OF_POOLS>
 inline PoolManager<NUMBER_OF_POOLS>::PoolManager(object_id_t setObjectId,
 		const uint16_t element_sizes[NUMBER_OF_POOLS],
@@ -40,11 +43,5 @@ inline ReturnValue_t PoolManager<NUMBER_OF_POOLS>::deleteData(uint8_t* buffer,
 	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::NO_TIMEOUT);
-	ReturnValue_t status = LocalPool<NUMBER_OF_POOLS>::modifyData(packet_id,
-				packet_ptr, size);
-	return status;
-}
+#endif
+
diff --git a/storagemanager/StorageAccessor.cpp b/storagemanager/StorageAccessor.cpp
new file mode 100644
index 00000000..cc292d6c
--- /dev/null
+++ b/storagemanager/StorageAccessor.cpp
@@ -0,0 +1,154 @@
+#include <framework/storagemanager/StorageAccessor.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) {
+		sif::debug << "deleting store data" << std::endl;
+		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) {
+		sif::warning << "StorageAccessor: Not initialized!" << std::endl;
+		return;
+	}
+	sif::info << "StorageAccessor: Printing data: [";
+	for(uint16_t iPool = 0; iPool < size_; iPool++) {
+		sif::info << std::hex << (int)constDataPointer[iPool];
+		if(iPool < size_ - 1){
+			sif::info << " , ";
+		}
+	}
+	sif::info << " ] " << std::endl;
+}
+
+void ConstStorageAccessor::assignStore(StorageManagerIF* store) {
+	internalState = AccessState::ASSIGNED;
+	this->store = store;
+}
+
+
+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;
+}
+
+
diff --git a/storagemanager/StorageAccessor.h b/storagemanager/StorageAccessor.h
new file mode 100644
index 00000000..8949642c
--- /dev/null
+++ b/storagemanager/StorageAccessor.h
@@ -0,0 +1,152 @@
+/**
+ * @brief 	Helper classes to facilitate safe access to storages which is also
+ * 			conforming to RAII principles
+ * @details	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.
+ */
+#ifndef TEST_PROTOTYPES_STORAGEACCESSOR_H_
+#define TEST_PROTOTYPES_STORAGEACCESSOR_H_
+
+#include <framework/ipc/MutexHelper.h>
+#include <framework/storagemanager/StorageManagerIF.h>
+#include <memory>
+
+/**
+ * @brief   Accessor class which can be returned by pool managers
+ *          or passed and set by pool managers to have safe access to the pool
+ *          resources.
+ */
+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= (ConstStorageAccessor&) = delete;
+	ConstStorageAccessor (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*);
+};
+
+
+/**
+ * @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 free prematurely.
+	 * Refer to: 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 /* TEST_PROTOTYPES_STORAGEACCESSOR_H_ */
diff --git a/storagemanager/StorageManagerIF.h b/storagemanager/StorageManagerIF.h
index d1f9cde5..559c4b9a 100644
--- a/storagemanager/StorageManagerIF.h
+++ b/storagemanager/StorageManagerIF.h
@@ -3,7 +3,14 @@
 
 #include <framework/events/Event.h>
 #include <framework/returnvalues/HasReturnvaluesIF.h>
-#include <stddef.h>
+#include <cstddef>
+#include <utility>
+
+class StorageAccessor;
+class ConstStorageAccessor;
+
+using AccessorPair = std::pair<ReturnValue_t, StorageAccessor>;
+using ConstAccessorPair = std::pair<ReturnValue_t, ConstStorageAccessor>;
 
 /**
  * This union defines the type that identifies where a data packet is
@@ -48,6 +55,10 @@ union store_address_t {
 	 * Alternative access to the raw value.
 	 */
 	uint32_t raw;
+
+	bool operator==(const store_address_t& other) const {
+		return raw == other.raw;
+	}
 };
 
 /**
@@ -117,6 +128,27 @@ 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.
@@ -130,8 +162,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;
@@ -150,12 +204,12 @@ 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!
 	 */
 	virtual void clearStore() = 0;
-
 };
 
 #endif /* STORAGEMANAGERIF_H_ */
diff --git a/tmtcpacket/SpacePacketBase.cpp b/tmtcpacket/SpacePacketBase.cpp
index 13c062d8..b9ae0684 100644
--- a/tmtcpacket/SpacePacketBase.cpp
+++ b/tmtcpacket/SpacePacketBase.cpp
@@ -14,7 +14,8 @@ uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
 	return (this->data->header.packet_id_h & 0b11100000) >> 5;
 }
 
-void SpacePacketBase::initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
+void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
+		bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
 	//reset header to zero:
 	memset(data,0, sizeof(this->data->header) );
 	//Set TC/TM bit.
@@ -81,7 +82,7 @@ void SpacePacketBase::setPacketDataLength( uint16_t new_length) {
 	this->data->header.packet_length_l = ( new_length & 0x00FF );
 }
 
-uint32_t SpacePacketBase::getFullSize() {
+size_t SpacePacketBase::getFullSize() {
 	//+1 is done because size in packet data length field is: size of data field -1
 	return this->getPacketDataLength() + sizeof(this->data->header) + 1;
 }
diff --git a/tmtcpacket/SpacePacketBase.h b/tmtcpacket/SpacePacketBase.h
index cc68f714..57a5df9c 100644
--- a/tmtcpacket/SpacePacketBase.h
+++ b/tmtcpacket/SpacePacketBase.h
@@ -2,9 +2,10 @@
 #define SPACEPACKETBASE_H_
 
 #include <framework/tmtcpacket/ccsds_header.h>
+#include <cstddef>
 
 /**
- * \defgroup tmtcpackets Space Packets
+ * @defgroup tmtcpackets Space Packets
  * This is the group, where all classes associated with Telecommand and
  * Telemetry packets belong to.
  * The class hierarchy resembles the dependency between the different standards
@@ -167,7 +168,7 @@ public:
 	 * This method returns the full raw packet size.
 	 * @return	The full size of the packet in bytes.
 	 */
-	uint32_t getFullSize();
+	size_t getFullSize();
 
 	uint32_t getApidAndSequenceCount() const;
 
diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp
index d7f6636d..0972ffff 100644
--- a/tmtcpacket/pus/TcPacketBase.cpp
+++ b/tmtcpacket/pus/TcPacketBase.cpp
@@ -28,7 +28,7 @@ const uint8_t* TcPacketBase::getApplicationData() const {
 	return &tcData->data;
 }
 
-size_t TcPacketBase::getApplicationDataSize() {
+uint16_t TcPacketBase::getApplicationDataSize() {
 	return getPacketDataLength() - sizeof(tcData->data_field) - CRC_SIZE + 1;
 }
 
@@ -46,9 +46,16 @@ void TcPacketBase::setErrorControl() {
 	(&tcData->data)[size + 1] = (crc) & 0X00FF; 		// CRCL
 }
 
-void TcPacketBase::setData(const uint8_t* p_Data) {
-	SpacePacketBase::setData(p_Data);
-	tcData = (TcPacketPointer*) p_Data;
+void TcPacketBase::setData(const uint8_t* pData) {
+	SpacePacketBase::setData(pData);
+	tcData = (TcPacketPointer*) pData;
+}
+
+void TcPacketBase::setApplicationData(const uint8_t * pData, uint16_t dataLen) {
+	setData(pData);
+	// packet data length is actual size of data field minus 1
+	SpacePacketBase::setPacketDataLength(dataLen +
+			sizeof(PUSTcDataFieldHeader) + TcPacketBase::CRC_SIZE - 1);
 }
 
 uint8_t TcPacketBase::getSecondaryHeaderFlag() {
@@ -72,7 +79,7 @@ void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
 		uint8_t ack, uint8_t service, uint8_t subservice) {
 	initSpacePacketHeader(true, true, apid, sequenceCount);
 	memset(&tcData->data_field, 0, sizeof(tcData->data_field));
-	setPacketDataLength(sizeof(tcData->data_field) + CRC_SIZE);
+	setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1);
 	//Data Field Header:
 	//Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000
 	tcData->data_field.version_type_ack = 0b00010000;
@@ -80,3 +87,8 @@ void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount,
 	tcData->data_field.service_type = service;
 	tcData->data_field.service_subtype = subservice;
 }
+
+size_t TcPacketBase::calculateFullPacketLength(size_t appDataLen) {
+	return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) +
+			appDataLen + TcPacketBase::CRC_SIZE;
+}
diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h
index d84d6c45..136aaa0b 100644
--- a/tmtcpacket/pus/TcPacketBase.h
+++ b/tmtcpacket/pus/TcPacketBase.h
@@ -2,6 +2,7 @@
 #define TCPACKETBASE_H_
 
 #include <framework/tmtcpacket/SpacePacketBase.h>
+#include <cstddef>
 
 /**
  * This struct defines a byte-wise structured PUS TC Data Field Header.
@@ -99,7 +100,8 @@ public:
 	 * @param service	PUS Service
 	 * @param subservice PUS Subservice
 	 */
-	void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack, uint8_t service, uint8_t subservice);
+	void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack,
+			uint8_t service, uint8_t subservice);
 	/**
 	 * This command returns the CCSDS Secondary Header Flag.
 	 * It shall always be zero for PUS Packets. This is the
@@ -151,7 +153,7 @@ public:
 	 * @return	The size of the PUS Application Data (without Error Control
 	 * 		field)
 	 */
-	size_t getApplicationDataSize();
+	uint16_t getApplicationDataSize();
 	/**
 	 * This getter returns the Error Control Field of the packet.
 	 *
@@ -175,12 +177,24 @@ public:
 	 *
 	 * @param p_data	A pointer to another PUS Telecommand Packet.
 	 */
-	void setData( const uint8_t* p_data );
+	void setData( const uint8_t* pData );
+	/**
+	 * Set application data and corresponding length field.
+	 * @param pData
+	 * @param dataLen
+	 */
+	void setApplicationData(const uint8_t * pData, uint16_t dataLen);
 	/**
 	 * This is a debugging helper method that prints the whole packet content
 	 * to the screen.
 	 */
 	void print();
+	/**
+	 * Calculate full packet length from application data length.
+	 * @param appDataLen
+	 * @return
+	 */
+	static size_t calculateFullPacketLength(size_t appDataLen);
 };
 
 
diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h
index 6b76bdab..259ed1a3 100644
--- a/tmtcservices/CommandingServiceBase.h
+++ b/tmtcservices/CommandingServiceBase.h
@@ -212,6 +212,8 @@ protected:
 	 */
 	PeriodicTaskIF* executingTask;
 
+	// todo: why do these functions not have returnvalues? the caller should be
+	// able to check whether the send operations actually work.
 	/**
 	 * Send TM data from pointer to data. If a header is supplied it is added before data
 	 * @param subservice Number of subservice