diff --git a/CHANGELOG b/CHANGELOG
index 298ef0d4..fc46ee02 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -81,4 +81,7 @@ now
   For the fsfw, this can be done by checking the processor define FSFW_CPP_OSTREAM_ENABLED from FSFWConfig.h. 
   For mission code, developers need to replace sif:: calls by the printf counterparts, but only if the CPP stream are excluded. 
   If this is not the case, everything should work as usual.
-- 
\ No newline at end of file
+
+### PUS Parameter Service 20
+
+Added PUS parameter service 20 (only custom subservices available).
\ No newline at end of file
diff --git a/action/CMakeLists.txt b/action/CMakeLists.txt
index a62d4044..f9ac451d 100644
--- a/action/CMakeLists.txt
+++ b/action/CMakeLists.txt
@@ -1,7 +1,7 @@
 target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		ActionHelper.cpp
-		ActionMessage.cpp
-		CommandActionHelper.cpp
-		SimpleActionHelper.cpp
+    PRIVATE 
+        ActionHelper.cpp
+        ActionMessage.cpp
+        CommandActionHelper.cpp
+        SimpleActionHelper.cpp
 )
\ No newline at end of file
diff --git a/container/CMakeLists.txt b/container/CMakeLists.txt
index 904cde55..13eced1d 100644
--- a/container/CMakeLists.txt
+++ b/container/CMakeLists.txt
@@ -1,5 +1,5 @@
 target_sources(${LIB_FSFW_NAME} 
-	PRIVATE 
-		SharedRingBuffer.cpp
-		SimpleRingBuffer.cpp
+    PRIVATE 
+        SharedRingBuffer.cpp
+        SimpleRingBuffer.cpp
 )
\ No newline at end of file
diff --git a/controller/CMakeLists.txt b/controller/CMakeLists.txt
index b7624d98..550acfcd 100644
--- a/controller/CMakeLists.txt
+++ b/controller/CMakeLists.txt
@@ -1,4 +1,4 @@
-target_sources(${LIB_FSFW_NAME} 
-    PRIVATE 
-        ControllerBase.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    ControllerBase.cpp
+    ExtendedControllerBase.cpp
 )
\ No newline at end of file
diff --git a/controller/ExtendedControllerBase.cpp b/controller/ExtendedControllerBase.cpp
index 397c8c10..cc93efa8 100644
--- a/controller/ExtendedControllerBase.cpp
+++ b/controller/ExtendedControllerBase.cpp
@@ -8,6 +8,9 @@ ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId,
         actionHelper(this, commandQueue) {
 }
 
+ExtendedControllerBase::~ExtendedControllerBase() {
+}
+
 ReturnValue_t ExtendedControllerBase::executeAction(ActionId_t actionId,
         MessageQueueId_t commandedBy, const uint8_t *data, size_t size) {
     // needs to be overriden and implemented by child class.
diff --git a/controller/ExtendedControllerBase.h b/controller/ExtendedControllerBase.h
index bcb414dd..f069819b 100644
--- a/controller/ExtendedControllerBase.h
+++ b/controller/ExtendedControllerBase.h
@@ -22,13 +22,14 @@ class ExtendedControllerBase: public ControllerBase,
 public:
     ExtendedControllerBase(object_id_t objectId, object_id_t parentId,
             size_t commandQueueDepth = 3);
+    virtual ~ExtendedControllerBase();
 
-    /** SystemObjectIF overrides */
+    /* SystemObjectIF overrides */
     virtual ReturnValue_t initialize() override;
 
     virtual MessageQueueId_t getCommandQueue() const override;
 
-    /** ExecutableObjectIF overrides */
+    /* ExecutableObjectIF overrides */
     virtual ReturnValue_t performOperation(uint8_t opCode) override;
     virtual ReturnValue_t initializeAfterTaskCreation() override;
 
@@ -49,15 +50,15 @@ protected:
      */
     virtual void performControlOperation() = 0;
 
-    /** Handle the four messages mentioned above */
+    /* Handle the four messages mentioned above */
     void handleQueue() override;
 
-    /** HasActionsIF overrides */
+    /* HasActionsIF overrides */
     virtual ReturnValue_t executeAction(ActionId_t actionId,
             MessageQueueId_t commandedBy, const uint8_t* data,
             size_t size) override;
 
-    /** HasLocalDatapoolIF overrides */
+    /* HasLocalDatapoolIF overrides */
     virtual LocalDataPoolManager* getHkManagerHandle() override;
     virtual object_id_t getObjectId() const override;
     virtual ReturnValue_t initializeLocalDataPool(
diff --git a/datapool/PoolDataSetBase.cpp b/datapool/PoolDataSetBase.cpp
index 01069785..2fd51966 100644
--- a/datapool/PoolDataSetBase.cpp
+++ b/datapool/PoolDataSetBase.cpp
@@ -1,5 +1,6 @@
 #include "PoolDataSetBase.h"
 #include "../serviceinterface/ServiceInterfaceStream.h"
+#include <cstring>
 
 PoolDataSetBase::PoolDataSetBase(PoolVariableIF** registeredVariablesArray,
         const size_t maxFillCount):
diff --git a/datapool/PoolDataSetBase.h b/datapool/PoolDataSetBase.h
index 75654146..ab895455 100644
--- a/datapool/PoolDataSetBase.h
+++ b/datapool/PoolDataSetBase.h
@@ -39,8 +39,12 @@ public:
 	 * 			supply a pointer to this dataset to PoolVariable
 	 * 			initializations to register pool variables.
 	 */
-	PoolDataSetBase(PoolVariableIF** registeredVariablesArray,
-	        const size_t maxFillCount);
+	PoolDataSetBase(PoolVariableIF** registeredVariablesArray, const size_t maxFillCount);
+
+	/* Forbidden for now */
+	PoolDataSetBase(const PoolDataSetBase& otherSet) = delete;
+	const PoolDataSetBase& operator=(const PoolDataSetBase& otherSet) = delete;
+
 	virtual~ PoolDataSetBase();
 
 	/**
@@ -63,8 +67,7 @@ public:
 	 * - @c SET_WAS_ALREADY_READ if read() is called twice without calling
 	 *      commit() in between
 	 */
-	virtual ReturnValue_t read(
-			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+	virtual ReturnValue_t read(MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
 			uint32_t lockTimeout = 20) override;
 	/**
 	 * @brief	The commit call initializes writing back the registered variables.
@@ -84,8 +87,7 @@ public:
 	 * 			- @c COMMITING_WITHOUT_READING if set was not read yet and
 	 * 			  contains non write-only variables
 	 */
-	virtual ReturnValue_t commit(
-			MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
+	virtual ReturnValue_t commit(MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING,
 			uint32_t lockTimeout = 20) override;
 
 	/**
diff --git a/datapoollocal/HasLocalDataPoolIF.h b/datapoollocal/HasLocalDataPoolIF.h
index 1d87d47c..05570175 100644
--- a/datapoollocal/HasLocalDataPoolIF.h
+++ b/datapoollocal/HasLocalDataPoolIF.h
@@ -80,7 +80,7 @@ public:
      * @param storeId If a snapshot was requested, data will be located inside
      * the IPC store with this store ID.
      */
-    virtual void handleChangedPoolVariable(lp_id_t poolId,
+    virtual void handleChangedPoolVariable(gp_id_t globPoolId,
             store_address_t storeId = storeId::INVALID_STORE_ADDRESS) {
         return;
     }
diff --git a/datapoollocal/LocalDataPoolManager.cpp b/datapoollocal/LocalDataPoolManager.cpp
index 2c5c4e0e..44769163 100644
--- a/datapoollocal/LocalDataPoolManager.cpp
+++ b/datapoollocal/LocalDataPoolManager.cpp
@@ -102,7 +102,7 @@ ReturnValue_t LocalDataPoolManager::performHkOperation() {
 		switch(receiver.reportingType) {
 		case(ReportingType::PERIODIC): {
 			if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
-				// Periodic packets shall only be generated from datasets.
+				/* Periodic packets shall only be generated from datasets */
 				continue;
 			}
 			performPeriodicHkGeneration(receiver);
@@ -156,19 +156,17 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receive
 	if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
 		LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
 				receiver.dataId.localPoolId);
-		//LocalPoolObjectBase* poolObj = owner->getPoolObjectHandle(receiver.dataId.localPoolId);
 		if(poolObj == nullptr) {
 			printWarningOrError(sif::OutputTypes::OUT_WARNING,
 					"handleNotificationUpdate", POOLOBJECT_NOT_FOUND);
 			return POOLOBJECT_NOT_FOUND;
 		}
 		if(poolObj->hasChanged()) {
-			// prepare and send update notification.
+			/* Prepare and send update notification. */
 			CommandMessage notification;
 			HousekeepingMessage::setUpdateNotificationVariableCommand(&notification,
-					receiver.dataId.localPoolId);
-			ReturnValue_t result = hkQueue->sendMessage(
-					receiver.destinationQueue, &notification);
+			        gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId));
+			ReturnValue_t result = hkQueue->sendMessage(receiver.destinationQueue, &notification);
 			if(result != HasReturnvaluesIF::RETURN_OK) {
 				status = result;
 			}
@@ -185,10 +183,10 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receive
 			return DATASET_NOT_FOUND;
 		}
 		if(dataSet->hasChanged()) {
-			// prepare and send update notification
+			/* Prepare and send update notification */
 			CommandMessage notification;
-			HousekeepingMessage::setUpdateNotificationSetCommand(
-					&notification, receiver.dataId.sid);
+			HousekeepingMessage::setUpdateNotificationSetCommand(&notification,
+			        receiver.dataId.sid);
 			ReturnValue_t result = hkQueue->sendMessage(
 					receiver.destinationQueue, &notification);
 			if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -198,8 +196,7 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receive
 		}
 	}
 	if(toReset != nullptr) {
-		handleChangeResetLogic(receiver.dataType,
-				receiver.dataId, toReset);
+		handleChangeResetLogic(receiver.dataType, receiver.dataId, toReset);
 	}
 	return HasReturnvaluesIF::RETURN_OK;
 }
@@ -207,7 +204,7 @@ ReturnValue_t LocalDataPoolManager::handleNotificationUpdate(HkReceiver& receive
 ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
 		HkReceiver& receiver, ReturnValue_t& status) {
 	MarkChangedIF* toReset = nullptr;
-	// check whether data has changed and send messages in case it has.
+	/* Check whether data has changed and send messages in case it has */
 	if(receiver.dataType == DataType::LOCAL_POOL_VARIABLE) {
 		LocalPoolObjectBase* poolObj = HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
 				receiver.dataId.localPoolId);
@@ -221,14 +218,14 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
 			return HasReturnvaluesIF::RETURN_OK;
 		}
 
-		// prepare and send update snapshot.
+		/* Prepare and send update snapshot */
 		timeval now;
 		Clock::getClock_timeval(&now);
 		CCSDSTime::CDS_short cds;
 		CCSDSTime::convertToCcsds(&cds, &now);
-		HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds),
-				sizeof(cds), HasLocalDpIFManagerAttorney::getPoolObjectHandle(owner,
-						receiver.dataId.localPoolId));
+		HousekeepingSnapshot updatePacket(reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
+		        HasLocalDpIFManagerAttorney::getPoolObjectHandle(
+		                owner,receiver.dataId.localPoolId));
 
 		store_address_t storeId;
 		ReturnValue_t result = addUpdateToStore(updatePacket, storeId);
@@ -238,7 +235,7 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
 
 		CommandMessage notification;
 		HousekeepingMessage::setUpdateSnapshotVariableCommand(&notification,
-				receiver.dataId.localPoolId, storeId);
+				gp_id_t(owner->getObjectId(), receiver.dataId.localPoolId), storeId);
 		result = hkQueue->sendMessage(receiver.destinationQueue,
 				&notification);
 		if (result != HasReturnvaluesIF::RETURN_OK) {
@@ -259,7 +256,7 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(
 			return HasReturnvaluesIF::RETURN_OK;
 		}
 
-		// prepare and send update snapshot.
+		/* Prepare and send update snapshot */
 		timeval now;
 		Clock::getClock_timeval(&now);
 		CCSDSTime::CDS_short cds;
@@ -309,7 +306,7 @@ ReturnValue_t LocalDataPoolManager::addUpdateToStore(
 void LocalDataPoolManager::handleChangeResetLogic(
 		DataType type, DataId dataId, MarkChangedIF* toReset) {
 	if(hkUpdateResetList == nullptr) {
-		// config error!
+		/* Config error */
 		return;
 	}
 	HkUpdateResetList& listRef = *hkUpdateResetList;
@@ -326,16 +323,16 @@ void LocalDataPoolManager::handleChangeResetLogic(
 			continue;
 		}
 
-		// only one update recipient, we can reset changes status immediately.
+		/* Only one update recipient, we can reset changes status immediately */
 		if(changeInfo.updateCounter <= 1) {
 			toReset->setChanged(false);
 		}
-		// All recipients have been notified, reset the changed flag.
+		/* All recipients have been notified, reset the changed flag */
 		if(changeInfo.currentUpdateCounter <= 1) {
 			toReset->setChanged(false);
 			changeInfo.currentUpdateCounter = 0;
 		}
-		// Not all recipiens have been notified yet, decrement.
+		/* Not all recipiens have been notified yet, decrement */
 		else {
 			changeInfo.currentUpdateCounter--;
 		}
@@ -371,7 +368,6 @@ ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid,
 	hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
 
 	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
-	//LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
 	if(dataSet != nullptr) {
 		LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enableReporting);
 		LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
@@ -557,15 +553,14 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(
 				dataSet, true);
 	}
 
-	// Notification handling.
+	/* Notification handling */
 	case(HousekeepingMessage::UPDATE_NOTIFICATION_SET): {
 		owner->handleChangedDataset(sid);
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	case(HousekeepingMessage::UPDATE_NOTIFICATION_VARIABLE): {
-		lp_id_t locPoolId = HousekeepingMessage::
-				getUpdateNotificationVariableCommand(message);
-		owner->handleChangedPoolVariable(locPoolId);
+		gp_id_t globPoolId = HousekeepingMessage::getUpdateNotificationVariableCommand(message);
+		owner->handleChangedPoolVariable(globPoolId);
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 	case(HousekeepingMessage::UPDATE_SNAPSHOT_SET): {
@@ -576,9 +571,9 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(
 	}
 	case(HousekeepingMessage::UPDATE_SNAPSHOT_VARIABLE): {
 		store_address_t storeId;
-		lp_id_t localPoolId = HousekeepingMessage::
-				getUpdateSnapshotVariableCommand(message, &storeId);
-		owner->handleChangedPoolVariable(localPoolId, storeId);
+		gp_id_t globPoolId = HousekeepingMessage::getUpdateSnapshotVariableCommand(message,
+		        &storeId);
+		owner->handleChangedPoolVariable(globPoolId, storeId);
 		return HasReturnvaluesIF::RETURN_OK;
 	}
 
@@ -693,7 +688,6 @@ void LocalDataPoolManager::setNonDiagnosticIntervalFactor(
 
 void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
 	sid_t sid = receiver.dataId.sid;
-	//LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
 	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
 	if(dataSet == nullptr) {
 		printWarningOrError(sif::OutputTypes::OUT_WARNING,
@@ -710,24 +704,23 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
 			LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet);
 
 	if(periodicHelper == nullptr) {
-		// Configuration error.
+		/* Configuration error */
 		return;
 	}
 
-	if(periodicHelper->checkOpNecessary()) {
+	if(not periodicHelper->checkOpNecessary()) {
 		return;
 	}
 
 	ReturnValue_t result = generateHousekeepingPacket(
 			sid, dataSet, true);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
-		// configuration error
+		/* Configuration error */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "LocalDataPoolManager::performHkOperation: "
-				<< "HK generation failed." << std::endl;
+		sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." <<
+		        std::endl;
 #else
-		sif::printWarning("LocalDataPoolManager::performHkOperation: "
-				"HK generation failed.\n");
+		sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n");
 #endif
 	}
 }
@@ -735,7 +728,6 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) {
 
 ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid,
 		bool enable, bool isDiagnostics) {
-	//LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
 	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
 	if((LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and not isDiagnostics) or
 			(not LocalPoolDataSetAttorney::isDiagnostics(*dataSet) and isDiagnostics)) {
@@ -753,7 +745,6 @@ ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid,
 
 ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid,
 		float newCollectionInterval, bool isDiagnostics) {
-	//LocalPoolDataSetBase* dataSet = owner->getDataSetHandle(sid);
 	LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
 	bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet);
 	if((targetIsDiagnostics and not isDiagnostics) or
@@ -848,7 +839,6 @@ MutexIF* LocalDataPoolManager::getLocalPoolMutex() {
 
 object_id_t LocalDataPoolManager::getCreatorObjectId() const {
 	return owner->getObjectId();
-	//return owner->getObjectId();
 }
 
 void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
@@ -883,26 +873,30 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
 	}
 
 	if(outputType == sif::OutputTypes::OUT_WARNING) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::warning << "LocalDataPoolManager::" << functionName
-				<< ": Object ID " << std::setw(8) << std::setfill('0')
+				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
 				<< std::hex << owner->getObjectId() << " | " << errorPrint
 				<< std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printWarning("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
 				functionName, owner->getObjectId(), errorPrint);
-#endif
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
 	}
 	else if(outputType == sif::OutputTypes::OUT_ERROR) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
 		sif::error << "LocalDataPoolManager::" << functionName
-				<< ": Object ID " << std::setw(8) << std::setfill('0')
+				<< ": Object ID 0x" << std::setw(8) << std::setfill('0')
 				<< std::hex << owner->getObjectId() << " | " << errorPrint
 				<< std::dec << std::setfill(' ') << std::endl;
 #else
 		sif::printError("LocalDataPoolManager::%s: Object ID 0x%08x | %s\n",
 				functionName, owner->getObjectId(), errorPrint);
-#endif
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
 	}
 }
 
diff --git a/datapoollocal/LocalPoolDataSetBase.cpp b/datapoollocal/LocalPoolDataSetBase.cpp
index 035ba694..d1d95d8a 100644
--- a/datapoollocal/LocalPoolDataSetBase.cpp
+++ b/datapoollocal/LocalPoolDataSetBase.cpp
@@ -51,6 +51,7 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(sid_t sid,
         AccessPoolManagerIF* accessor = HasLocalDpIFUserAttorney::getAccessorHandle(hkOwner);
         if(accessor != nullptr) {
             mutexIfSingleDataCreator = accessor->getLocalPoolMutex();
+            poolManager = accessor->getPoolManagerHandle();
         }
     }
 
@@ -66,9 +67,19 @@ LocalPoolDataSetBase::LocalPoolDataSetBase(
 
 
 LocalPoolDataSetBase::~LocalPoolDataSetBase() {
+    /* We only delete objects which were created in the class constructor */
     if(periodicHelper != nullptr) {
         delete periodicHelper;
     }
+    /* In case set was read but not comitted, we commit all variables with an invalid state */
+    if(state == States::STATE_SET_WAS_READ) {
+        for (uint16_t count = 0; count < fillCount; count++) {
+            if(registeredVariables[count] != nullptr) {
+                registeredVariables[count]->setValid(false);
+                registeredVariables[count]->commit(MutexIF::TimeoutType::WAITING, 20);
+            }
+        }
+    }
 }
 
 ReturnValue_t LocalPoolDataSetBase::lockDataPool(
@@ -83,7 +94,7 @@ ReturnValue_t LocalPoolDataSetBase::lockDataPool(
 ReturnValue_t LocalPoolDataSetBase::serializeWithValidityBuffer(uint8_t **buffer,
         size_t *size, size_t maxSize,
         SerializeIF::Endianness streamEndianness) const {
-    ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
     uint8_t validityMaskSize = std::ceil(static_cast<float>(fillCount)/8.0);
     uint8_t validityMask[validityMaskSize];
     uint8_t validBufferIndex = 0;
diff --git a/datapoollocal/LocalPoolDataSetBase.h b/datapoollocal/LocalPoolDataSetBase.h
index cc41b0eb..ca907431 100644
--- a/datapoollocal/LocalPoolDataSetBase.h
+++ b/datapoollocal/LocalPoolDataSetBase.h
@@ -97,12 +97,18 @@ public:
      * @brief	The destructor automatically manages writing the valid
      * 			information of variables.
      * @details
-     * In case the data set was read out, but not committed(indicated by state),
+     * In case the data set was read out, but not committed (indicated by state),
      * the destructor parses all variables that are still registered to the set.
      * For each, the valid flag in the data pool is set to "invalid".
      */
     ~LocalPoolDataSetBase();
 
+    /* The copy constructor and assingment constructor are forbidden for now.
+    The use-cases are limited and the first step would be to implement them properly for the
+    base class */
+    LocalPoolDataSetBase(const LocalPoolDataSetBase& otherSet) = delete;
+    const LocalPoolDataSetBase& operator=(const LocalPoolDataSetBase& otherSet) = delete;
+
     void setValidityBufferGeneration(bool withValidityBuffer);
 
     sid_t getSid() const;
@@ -153,6 +159,7 @@ public:
     bool hasChanged() const override;
 
     object_id_t getCreatorObjectId();
+
 protected:
     sid_t sid;
     //! This mutex is used if the data is created by one object only.
diff --git a/datapoollocal/LocalPoolObjectBase.h b/datapoollocal/LocalPoolObjectBase.h
index 797cf8b5..3f7fb6dd 100644
--- a/datapoollocal/LocalPoolObjectBase.h
+++ b/datapoollocal/LocalPoolObjectBase.h
@@ -20,12 +20,10 @@ class LocalPoolObjectBase: public PoolVariableIF,
         public HasReturnvaluesIF,
         public MarkChangedIF {
 public:
-    LocalPoolObjectBase(lp_id_t poolId,
-            HasLocalDataPoolIF* hkOwner, DataSetIF* dataSet,
+    LocalPoolObjectBase(lp_id_t poolId, HasLocalDataPoolIF* hkOwner, DataSetIF* dataSet,
             pool_rwm_t setReadWriteMode);
 
-    LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId,
-            DataSetIF* dataSet = nullptr,
+    LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, DataSetIF* dataSet = nullptr,
             pool_rwm_t setReadWriteMode = pool_rwm_t::VAR_READ_WRITE);
 
     void setReadWriteMode(pool_rwm_t newReadWriteMode);
diff --git a/datapoollocal/ProvidesDataPoolSubscriptionIF.h b/datapoollocal/ProvidesDataPoolSubscriptionIF.h
index a0d73b93..ead479cc 100644
--- a/datapoollocal/ProvidesDataPoolSubscriptionIF.h
+++ b/datapoollocal/ProvidesDataPoolSubscriptionIF.h
@@ -38,24 +38,22 @@ public:
             bool isDiagnostics,
             object_id_t packetDestination) = 0;
 
-
     /**
      * @brief   Subscribe for a notification message which will be sent
      *          if a dataset has changed.
      * @details
      * This subscription mechanism will generally be used internally by
      * other software components.
-     * @param setId     Set ID of the set to receive update messages from.
-     * @param destinationObject
-     * @param targetQueueId
-     * @param generateSnapshot If this is set to true, a copy of the current
-     * data with a timestamp will be generated and sent via message.
-     * Otherwise, only an notification message is sent.
+     * @param setId                 Set ID of the set to receive update messages from.
+     * @param destinationObject     Object ID of the receiver.
+     * @param targetQueueId         Receiver queue ID
+     * @param generateSnapshot      If this is set to true, a copy of the current data with a
+     *                              timestamp will be generated and sent via message.
+     *                              Otherwise, only an notification message is sent.
      * @return
      */
     virtual ReturnValue_t subscribeForSetUpdateMessages(const uint32_t setId,
-            object_id_t destinationObject,
-            MessageQueueId_t targetQueueId,
+            object_id_t destinationObject, MessageQueueId_t targetQueueId,
             bool generateSnapshot) = 0;
 
     /**
@@ -64,12 +62,12 @@ public:
      * @details
      * This subscription mechanism will generally be used internally by
      * other software components.
-     * @param localPoolId Pool ID of the pool variable
-     * @param destinationObject
-     * @param targetQueueId
-     * @param generateSnapshot If this is set to true, a copy of the current
-     * data with a timestamp will be generated and sent via message.
-     * Otherwise, only an notification message is sent.
+     * @param localPoolId           Pool ID of the pool variable
+     * @param destinationObject     Object ID of the receiver
+     * @param targetQueueId         Receiver queue ID
+     * @param generateSnapshot      If this is set to true, a copy of the current data with a
+     *                              timestamp will be generated and sent via message. Otherwise,
+     *                              only an notification message is sent.
      * @return
      */
     virtual ReturnValue_t subscribeForVariableUpdateMessages(
diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp
index a1ad9548..35d34bf9 100644
--- a/devicehandlers/DeviceHandlerBase.cpp
+++ b/devicehandlers/DeviceHandlerBase.cpp
@@ -435,8 +435,7 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId,
     }
 }
 
-ReturnValue_t DeviceHandlerBase::insertInCommandMap(
-        DeviceCommandId_t deviceCommand) {
+ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) {
     DeviceCommandInfo info;
     info.expectedReplies = 0;
     info.isExecuting = false;
@@ -701,13 +700,11 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
     ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
     DeviceCommandId_t foundId = DeviceHandlerIF::NO_COMMAND_ID;
     size_t foundLen = 0;
-    // The loop may not execute more often than the number of received bytes
-    // (worst case). This approach avoids infinite loops due to buggy
-    // scanForReply routines.
+    /* The loop may not execute more often than the number of received bytes
+    (worst case). This approach avoids infinite loops due to buggy scanForReply routines. */
     uint32_t remainingLength = receivedDataLen;
     for (uint32_t count = 0; count < receivedDataLen; count++) {
-        result = scanForReply(receivedData, remainingLength, &foundId,
-                &foundLen);
+        result = scanForReply(receivedData, remainingLength, &foundId, &foundLen);
         switch (result) {
         case RETURN_OK:
             handleReply(receivedData, foundId, foundLen);
@@ -790,9 +787,9 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData,
         replyToReply(iter, result);
     }
     else {
-        // Other completion failure messages are created by timeout.
-        // Powering down the device might take some time during which periodic
-        // replies may still come in.
+        /* Other completion failure messages are created by timeout.
+        Powering down the device might take some time during which periodic
+        replies may still come in. */
         if (mode != _MODE_WAIT_OFF) {
             triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId);
         }
diff --git a/housekeeping/HousekeepingMessage.cpp b/housekeeping/HousekeepingMessage.cpp
index 5d732961..d9803ef6 100644
--- a/housekeeping/HousekeepingMessage.cpp
+++ b/housekeeping/HousekeepingMessage.cpp
@@ -125,8 +125,10 @@ sid_t HousekeepingMessage::getSid(const CommandMessage* message) {
 	return sid;
 }
 
-void HousekeepingMessage::setSid(CommandMessage *message, sid_t sid) {
-	std::memcpy(message->getData(), &sid.raw, sizeof(sid.raw));
+gp_id_t HousekeepingMessage::getGpid(const CommandMessage* message) {
+    gp_id_t globalPoolId;
+    std::memcpy(&globalPoolId.raw, message->getData(), sizeof(globalPoolId.raw));
+    return globalPoolId;
 }
 
 void HousekeepingMessage::setHkStuctureReportReply(CommandMessage *reply,
@@ -169,9 +171,9 @@ void HousekeepingMessage::setUpdateNotificationSetCommand(
 }
 
 void HousekeepingMessage::setUpdateNotificationVariableCommand(
-        CommandMessage *command, lp_id_t localPoolId) {
+        CommandMessage *command, gp_id_t globalPoolId) {
     command->setCommand(UPDATE_NOTIFICATION_VARIABLE);
-    command->setParameter(localPoolId);
+    setGpid(command, globalPoolId);
 }
 
 void HousekeepingMessage::setUpdateSnapshotSetCommand(CommandMessage *command,
@@ -182,9 +184,9 @@ void HousekeepingMessage::setUpdateSnapshotSetCommand(CommandMessage *command,
 }
 
 void HousekeepingMessage::setUpdateSnapshotVariableCommand(
-        CommandMessage *command, lp_id_t localPoolId, store_address_t storeId) {
+        CommandMessage *command, gp_id_t globalPoolId, store_address_t storeId) {
     command->setCommand(UPDATE_SNAPSHOT_VARIABLE);
-    command->setParameter(localPoolId);
+    setGpid(command, globalPoolId);
     command->setParameter3(storeId.raw);
 }
 
@@ -193,9 +195,9 @@ sid_t HousekeepingMessage::getUpdateNotificationSetCommand(
     return getSid(command);
 }
 
-lp_id_t HousekeepingMessage::getUpdateNotificationVariableCommand(
+gp_id_t HousekeepingMessage::getUpdateNotificationVariableCommand(
         const CommandMessage *command) {
-    return command->getParameter();
+    return getGpid(command);
 }
 
 sid_t HousekeepingMessage::getUpdateSnapshotSetCommand(
@@ -206,10 +208,18 @@ sid_t HousekeepingMessage::getUpdateSnapshotSetCommand(
     return getSid(command);
 }
 
-lp_id_t HousekeepingMessage::getUpdateSnapshotVariableCommand(
+gp_id_t HousekeepingMessage::getUpdateSnapshotVariableCommand(
         const CommandMessage *command, store_address_t *storeId) {
     if(storeId != nullptr) {
         *storeId = command->getParameter3();
     }
-    return command->getParameter();
+    return getGpid(command);
+}
+
+void HousekeepingMessage::setSid(CommandMessage *message, sid_t sid) {
+    std::memcpy(message->getData(), &sid.raw, sizeof(sid.raw));
+}
+
+void HousekeepingMessage::setGpid(CommandMessage *message, gp_id_t globalPoolId) {
+    std::memcpy(message->getData(), &globalPoolId.raw, sizeof(globalPoolId.raw));
 }
diff --git a/housekeeping/HousekeepingMessage.h b/housekeeping/HousekeepingMessage.h
index 0c7680dc..a38ff73f 100644
--- a/housekeeping/HousekeepingMessage.h
+++ b/housekeeping/HousekeepingMessage.h
@@ -75,6 +75,7 @@ public:
 	//static constexpr Command_t UPDATE_HK_REPORT = MAKE_COMMAND_ID(134);
 
 	static sid_t getSid(const CommandMessage* message);
+	static gp_id_t getGpid(const CommandMessage* message);
 
 	/* Housekeeping Interface Messages */
 
@@ -123,26 +124,27 @@ public:
     static void setUpdateNotificationSetCommand(CommandMessage* command,
             sid_t sid);
     static void setUpdateNotificationVariableCommand(CommandMessage* command,
-            lp_id_t localPoolId);
+            gp_id_t globalPoolId);
 
     static void setUpdateSnapshotSetCommand(CommandMessage* command, sid_t sid,
             store_address_t storeId);
     static void setUpdateSnapshotVariableCommand(CommandMessage* command,
-            lp_id_t localPoolId, store_address_t storeId);
+            gp_id_t globalPoolId, store_address_t storeId);
 
     static sid_t getUpdateNotificationSetCommand(const CommandMessage* command);
-    static lp_id_t getUpdateNotificationVariableCommand(
+    static gp_id_t getUpdateNotificationVariableCommand(
             const CommandMessage* command);
 
     static sid_t getUpdateSnapshotSetCommand(const CommandMessage* command,
             store_address_t* storeId);
-    static lp_id_t getUpdateSnapshotVariableCommand(const CommandMessage* command,
+    static gp_id_t getUpdateSnapshotVariableCommand(const CommandMessage* command,
             store_address_t* storeId);
 
     /** Utility */
 	static void clear(CommandMessage* message);
 private:
 	static void setSid(CommandMessage* message, sid_t sid);
+	static void setGpid(CommandMessage* message, gp_id_t globalPoolId);
 };
 
 
diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp
index 7ffd4758..9fb8b7a3 100644
--- a/osal/host/QueueMapManager.cpp
+++ b/osal/host/QueueMapManager.cpp
@@ -18,17 +18,19 @@ QueueMapManager* QueueMapManager::instance() {
 
 ReturnValue_t QueueMapManager::addMessageQueue(
 		MessageQueueIF* queueToInsert, MessageQueueId_t* id) {
-	// Not thread-safe, but it is assumed all message queues are created
-	// at software initialization now. If this is to be made thread-safe in
-	// the future, it propably would be sufficient to lock the increment
-	// operation here
+	/* Not thread-safe, but it is assumed all message queues are created at software initialization
+	now. If this is to be made thread-safe in the future, it propably would be sufficient to lock
+	the increment operation here. */
 	uint32_t currentId = queueCounter++;
 	auto returnPair = queueMap.emplace(currentId, queueToInsert);
 	if(not returnPair.second) {
-		// this should never happen for the atomic variable.
+		/* This should never happen for the atomic variable. */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "QueueMapManager: This ID is already inside the map!"
-				<< std::endl;
+		sif::error << "QueueMapManager::addMessageQueue This ID is already "
+		        "inside the map!" << std::endl;
+#else
+		sif::printError("QueueMapManager::addMessageQueue This ID is already "
+                "inside the map!\n");
 #endif
 		return HasReturnvaluesIF::RETURN_FAILED;
 	}
@@ -47,8 +49,11 @@ MessageQueueIF* QueueMapManager::getMessageQueue(
 	}
 	else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::warning << "QueueMapManager::getQueueHandle: The ID " <<
-				messageQueueId << " does not exists in the map" << std::endl;
+		sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId <<
+		        " does not exists in the map!" << std::endl;
+#else
+		sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
+		        messageQueueId);
 #endif
 		return nullptr;
 	}
diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h
index d7d5c915..3610ca63 100644
--- a/osal/host/QueueMapManager.h
+++ b/osal/host/QueueMapManager.h
@@ -36,7 +36,7 @@ public:
 private:
 	//! External instantiation is forbidden.
 	QueueMapManager();
-	uint32_t queueCounter = 1;
+	uint32_t queueCounter = 0;
 	MutexIF* mapLock;
 	QueueMap queueMap;
 	static QueueMapManager* mqManagerInstance;
diff --git a/pus/Service3Housekeeping.cpp b/pus/Service3Housekeeping.cpp
index ba7ff6ee..5456764d 100644
--- a/pus/Service3Housekeeping.cpp
+++ b/pus/Service3Housekeeping.cpp
@@ -4,199 +4,199 @@
 
 
 Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid,
-			uint8_t serviceId):
-		CommandingServiceBase(objectId, apid, serviceId,
-		NUM_OF_PARALLEL_COMMANDS, COMMAND_TIMEOUT_SECONDS) {}
+        uint8_t serviceId):
+        CommandingServiceBase(objectId, apid, serviceId,
+                NUM_OF_PARALLEL_COMMANDS, COMMAND_TIMEOUT_SECONDS) {}
 
 Service3Housekeeping::~Service3Housekeeping() {}
 
 ReturnValue_t Service3Housekeeping::isValidSubservice(uint8_t subservice) {
-	switch(static_cast<Subservice>(subservice)) {
-	case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
-	case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
-	case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
-	case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
-	case Subservice::REPORT_HK_REPORT_STRUCTURES:
-	case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES :
-	case Subservice::GENERATE_ONE_PARAMETER_REPORT:
-	case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
-	case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
-	case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
-		return HasReturnvaluesIF::RETURN_OK;
-	// Telemetry or invalid subservice.
-	case Subservice::HK_DEFINITIONS_REPORT:
-	case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
-	case Subservice::HK_REPORT:
-	case Subservice::DIAGNOSTICS_REPORT:
-	default:
-		return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
-	}
+    switch(static_cast<Subservice>(subservice)) {
+    case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
+    case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
+    case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+    case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+    case Subservice::REPORT_HK_REPORT_STRUCTURES:
+    case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES :
+    case Subservice::GENERATE_ONE_PARAMETER_REPORT:
+    case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
+    case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
+    case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
+        return HasReturnvaluesIF::RETURN_OK;
+        // Telemetry or invalid subservice.
+    case Subservice::HK_DEFINITIONS_REPORT:
+    case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
+    case Subservice::HK_REPORT:
+    case Subservice::DIAGNOSTICS_REPORT:
+    default:
+        return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
+    }
 }
 
 ReturnValue_t Service3Housekeeping::getMessageQueueAndObject(uint8_t subservice,
-		const uint8_t *tcData, size_t tcDataLen,
-		MessageQueueId_t *id, object_id_t *objectId) {
-	ReturnValue_t result = checkAndAcquireTargetID(objectId,tcData,tcDataLen);
-	if(result != RETURN_OK) {
-		return result;
-	}
-	return checkInterfaceAndAcquireMessageQueue(id,objectId);
+        const uint8_t *tcData, size_t tcDataLen,
+        MessageQueueId_t *id, object_id_t *objectId) {
+    ReturnValue_t result = checkAndAcquireTargetID(objectId,tcData,tcDataLen);
+    if(result != RETURN_OK) {
+        return result;
+    }
+    return checkInterfaceAndAcquireMessageQueue(id,objectId);
 }
 
 ReturnValue_t Service3Housekeeping::checkAndAcquireTargetID(
-		object_id_t* objectIdToSet, const uint8_t* tcData, size_t tcDataLen) {
-	if(SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen,
-	        SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
-		return CommandingServiceBase::INVALID_TC;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+        object_id_t* objectIdToSet, const uint8_t* tcData, size_t tcDataLen) {
+    if(SerializeAdapter::deSerialize(objectIdToSet, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG) != HasReturnvaluesIF::RETURN_OK) {
+        return CommandingServiceBase::INVALID_TC;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t Service3Housekeeping::checkInterfaceAndAcquireMessageQueue(
-		MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
-	// check HasLocalDataPoolIF property of target
-	HasLocalDataPoolIF* possibleTarget =
-			objectManager->get<HasLocalDataPoolIF>(*objectId);
-	if(possibleTarget == nullptr){
-		return CommandingServiceBase::INVALID_OBJECT;
-	}
-	*messageQueueToSet = possibleTarget->getCommandQueue();
-	return HasReturnvaluesIF::RETURN_OK;
+        MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
+    // check HasLocalDataPoolIF property of target
+    HasLocalDataPoolIF* possibleTarget =
+            objectManager->get<HasLocalDataPoolIF>(*objectId);
+    if(possibleTarget == nullptr){
+        return CommandingServiceBase::INVALID_OBJECT;
+    }
+    *messageQueueToSet = possibleTarget->getCommandQueue();
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 
 ReturnValue_t Service3Housekeeping::prepareCommand(CommandMessage* message,
-		uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
-		uint32_t *state, object_id_t objectId) {
-	switch(static_cast<Subservice>(subservice)) {
-	case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
-		return prepareReportingTogglingCommand(message, objectId, true, false,
-				tcData, tcDataLen);
-	case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
-		return prepareReportingTogglingCommand(message, objectId, false, false,
-				tcData, tcDataLen);
-	case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
-		return prepareReportingTogglingCommand(message, objectId, true, true,
-				tcData, tcDataLen);
-	case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
-		return prepareReportingTogglingCommand(message, objectId, false, true,
-				tcData, tcDataLen);
-	case Subservice::REPORT_HK_REPORT_STRUCTURES:
-		return prepareStructureReportingCommand(message, objectId, false, tcData,
-				tcDataLen);
-	case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES:
-		return prepareStructureReportingCommand(message, objectId, true, tcData,
-				tcDataLen);
-	case Subservice::GENERATE_ONE_PARAMETER_REPORT:
-		return prepareOneShotReportCommand(message, objectId, false,
-				tcData, tcDataLen);
-	case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
-		return prepareOneShotReportCommand(message, objectId, true,
-				tcData, tcDataLen);
-	case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
-		return prepareCollectionIntervalModificationCommand(message, objectId,
-				false, tcData, tcDataLen);
-	case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
-		return prepareCollectionIntervalModificationCommand(message, objectId,
-				true, tcData, tcDataLen);
-	case Subservice::HK_DEFINITIONS_REPORT:
-	case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
-	case Subservice::HK_REPORT:
-	case Subservice::DIAGNOSTICS_REPORT:
-		// Those are telemetry packets.
-		return CommandingServiceBase::INVALID_TC;
-	default:
-		// should never happen, subservice was already checked.
-		return HasReturnvaluesIF::RETURN_FAILED;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+        uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
+        uint32_t *state, object_id_t objectId) {
+    switch(static_cast<Subservice>(subservice)) {
+    case Subservice::ENABLE_PERIODIC_HK_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, true, false,
+                tcData, tcDataLen);
+    case Subservice::DISABLE_PERIODIC_HK_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, false, false,
+                tcData, tcDataLen);
+    case Subservice::ENABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, true, true,
+                tcData, tcDataLen);
+    case Subservice::DISABLE_PERIODIC_DIAGNOSTICS_REPORT_GENERATION:
+        return prepareReportingTogglingCommand(message, objectId, false, true,
+                tcData, tcDataLen);
+    case Subservice::REPORT_HK_REPORT_STRUCTURES:
+        return prepareStructureReportingCommand(message, objectId, false, tcData,
+                tcDataLen);
+    case Subservice::REPORT_DIAGNOSTICS_REPORT_STRUCTURES:
+        return prepareStructureReportingCommand(message, objectId, true, tcData,
+                tcDataLen);
+    case Subservice::GENERATE_ONE_PARAMETER_REPORT:
+        return prepareOneShotReportCommand(message, objectId, false,
+                tcData, tcDataLen);
+    case Subservice::GENERATE_ONE_DIAGNOSTICS_REPORT:
+        return prepareOneShotReportCommand(message, objectId, true,
+                tcData, tcDataLen);
+    case Subservice::MODIFY_PARAMETER_REPORT_COLLECTION_INTERVAL:
+        return prepareCollectionIntervalModificationCommand(message, objectId,
+                false, tcData, tcDataLen);
+    case Subservice::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL:
+        return prepareCollectionIntervalModificationCommand(message, objectId,
+                true, tcData, tcDataLen);
+    case Subservice::HK_DEFINITIONS_REPORT:
+    case Subservice::DIAGNOSTICS_DEFINITION_REPORT:
+    case Subservice::HK_REPORT:
+    case Subservice::DIAGNOSTICS_REPORT:
+        // Those are telemetry packets.
+        return CommandingServiceBase::INVALID_TC;
+    default:
+        // should never happen, subservice was already checked.
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t Service3Housekeeping::prepareReportingTogglingCommand(
-		CommandMessage *command, object_id_t objectId,
-		bool enableReporting, bool isDiagnostics,
-		const uint8_t* tcData, size_t tcDataLen) {
-	if(tcDataLen < sizeof(sid_t)) {
-		// TC data should consist of object ID and set ID.
-		return CommandingServiceBase::INVALID_TC;
-	}
+        CommandMessage *command, object_id_t objectId,
+        bool enableReporting, bool isDiagnostics,
+        const uint8_t* tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
 
-	sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
-	HousekeepingMessage::setToggleReportingCommand(command, targetSid,
-			enableReporting, isDiagnostics);
-	return HasReturnvaluesIF::RETURN_OK;
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setToggleReportingCommand(command, targetSid,
+            enableReporting, isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t Service3Housekeeping::prepareStructureReportingCommand(
-		CommandMessage *command, object_id_t objectId, bool isDiagnostics,
-		const uint8_t* tcData, size_t tcDataLen) {
-	if(tcDataLen < sizeof(sid_t)) {
-		// TC data should consist of object ID and set ID.
-		return CommandingServiceBase::INVALID_TC;
-	}
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t* tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
 
-	sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
-	HousekeepingMessage::setStructureReportingCommand(command, targetSid,
-			isDiagnostics);
-	return HasReturnvaluesIF::RETURN_OK;
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setStructureReportingCommand(command, targetSid,
+            isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t Service3Housekeeping::prepareOneShotReportCommand(
-		CommandMessage *command, object_id_t objectId, bool isDiagnostics,
-		const uint8_t *tcData, size_t tcDataLen) {
-	if(tcDataLen < sizeof(sid_t)) {
-		// TC data should consist of object ID and set ID.
-		return CommandingServiceBase::INVALID_TC;
-	}
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t *tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t)) {
+        // TC data should consist of object ID and set ID.
+        return CommandingServiceBase::INVALID_TC;
+    }
 
-	sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
-	HousekeepingMessage::setOneShotReportCommand(command, targetSid,
-			isDiagnostics);
-	return HasReturnvaluesIF::RETURN_OK;
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    HousekeepingMessage::setOneShotReportCommand(command, targetSid,
+            isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 ReturnValue_t Service3Housekeeping::prepareCollectionIntervalModificationCommand(
-		CommandMessage *command, object_id_t objectId, bool isDiagnostics,
-		const uint8_t *tcData, size_t tcDataLen) {
-	if(tcDataLen < sizeof(sid_t) + sizeof(float)) {
-		// SID plus the size of the new collection intervL.
-		return CommandingServiceBase::INVALID_TC;
-	}
+        CommandMessage *command, object_id_t objectId, bool isDiagnostics,
+        const uint8_t *tcData, size_t tcDataLen) {
+    if(tcDataLen < sizeof(sid_t) + sizeof(float)) {
+        // SID plus the size of the new collection intervL.
+        return CommandingServiceBase::INVALID_TC;
+    }
 
-	sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
-	float newCollectionInterval = 0;
-	SerializeAdapter::deSerialize(&newCollectionInterval, &tcData, &tcDataLen,
-			SerializeIF::Endianness::BIG);
-	HousekeepingMessage::setCollectionIntervalModificationCommand(command,
-			targetSid, newCollectionInterval, isDiagnostics);
-	return HasReturnvaluesIF::RETURN_OK;
+    sid_t targetSid = buildSid(objectId, &tcData, &tcDataLen);
+    float newCollectionInterval = 0;
+    SerializeAdapter::deSerialize(&newCollectionInterval, &tcData, &tcDataLen,
+            SerializeIF::Endianness::BIG);
+    HousekeepingMessage::setCollectionIntervalModificationCommand(command,
+            targetSid, newCollectionInterval, isDiagnostics);
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 
 ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
-		Command_t previousCommand, uint32_t *state,
-		CommandMessage* optionalNextCommand, object_id_t objectId,
-		bool *isStep) {
-	Command_t command = reply->getCommand();
-	switch(command) {
+        Command_t previousCommand, uint32_t *state,
+        CommandMessage* optionalNextCommand, object_id_t objectId,
+        bool *isStep) {
+    Command_t command = reply->getCommand();
+    switch(command) {
 
-	case(HousekeepingMessage::HK_REPORT): {
-		ReturnValue_t result = generateHkReply(reply,
-				static_cast<uint8_t>(Subservice::HK_REPORT));
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
-		return CommandingServiceBase::EXECUTION_COMPLETE;
-	}
+    case(HousekeepingMessage::HK_REPORT): {
+        ReturnValue_t result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::HK_REPORT));
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
 
-	case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
-		ReturnValue_t result = generateHkReply(reply,
-				static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
-		if(result != HasReturnvaluesIF::RETURN_OK) {
-			return result;
-		}
-		return CommandingServiceBase::EXECUTION_COMPLETE;
-	}
+    case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
+        ReturnValue_t result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
+        if(result != HasReturnvaluesIF::RETURN_OK) {
+            return result;
+        }
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
 
     case(HousekeepingMessage::HK_DEFINITIONS_REPORT): {
         return generateHkReply(reply, static_cast<uint8_t>(
@@ -209,93 +209,110 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
         break;
     }
 
-	case(HousekeepingMessage::HK_REQUEST_SUCCESS): {
-		return CommandingServiceBase::EXECUTION_COMPLETE;
-	}
+    case(HousekeepingMessage::HK_REQUEST_SUCCESS): {
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
 
-	case(HousekeepingMessage::HK_REQUEST_FAILURE): {
-		failureParameter1 = objectId;
-		ReturnValue_t error = HasReturnvaluesIF::RETURN_FAILED;
-		HousekeepingMessage::getHkRequestFailureReply(reply,&error);
-		failureParameter2 = error;
-		return CommandingServiceBase::EXECUTION_COMPLETE;
-	}
+    case(HousekeepingMessage::HK_REQUEST_FAILURE): {
+        failureParameter1 = objectId;
+        ReturnValue_t error = HasReturnvaluesIF::RETURN_FAILED;
+        HousekeepingMessage::getHkRequestFailureReply(reply,&error);
+        failureParameter2 = error;
+        return CommandingServiceBase::EXECUTION_COMPLETE;
+    }
 
-	default:
+    default:
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Service3Housekeeping::handleReply: Invalid reply with "
-				<< "reply command " << command << "!" << std::endl;
+        sif::warning << "Service3Housekeeping::handleReply: Invalid reply with "
+        << "reply command " << command << "!" << std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleReply: Invalid reply with "
+                "reply command %hu!\n", command);
 #endif
-		return CommandingServiceBase::INVALID_REPLY;
-	}
-	return HasReturnvaluesIF::RETURN_OK;
+        return CommandingServiceBase::INVALID_REPLY;
+    }
+    return HasReturnvaluesIF::RETURN_OK;
 }
 
 void Service3Housekeeping::handleUnrequestedReply(
-		CommandMessage* reply) {
-	ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
-	Command_t command = reply->getCommand();
+        CommandMessage* reply) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    Command_t command = reply->getCommand();
 
-	switch(command) {
+    switch(command) {
 
-	case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
-		result = generateHkReply(reply,
-				static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
-		break;
-	}
+    case(HousekeepingMessage::DIAGNOSTICS_REPORT): {
+        result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::DIAGNOSTICS_REPORT));
+        break;
+    }
 
-	case(HousekeepingMessage::HK_REPORT): {
-		result = generateHkReply(reply,
-				static_cast<uint8_t>(Subservice::HK_REPORT));
-		break;
-	}
+    case(HousekeepingMessage::HK_REPORT): {
+        result = generateHkReply(reply,
+                static_cast<uint8_t>(Subservice::HK_REPORT));
+        break;
+    }
 
-	default:
+    case(HousekeepingMessage::HK_REQUEST_SUCCESS): {
+        break;
+    }
+
+    case(HousekeepingMessage::HK_REQUEST_FAILURE): {
+        break;
+    }
+
+    default: {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::error << "Service3Housekeeping::handleUnrequestedReply: Invalid "
-		        << "reply with " << "reply command " << command << "!"
-		        << std::endl;
+        sif::warning << "Service3Housekeeping::handleUnrequestedReply: Invalid reply with reply "
+                "command " << command << "!" << std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleUnrequestedReply: Invalid reply with "
+                "reply command %hu!\n", command);
 #endif
-		return;
-	}
+        return;
+    }
+    }
 
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		// Configuration error
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        /* Configuration error */
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-		sif::debug << "Service3Housekeeping::handleUnrequestedReply:"
-				<< "Could not generate reply!" << std::endl;
+        sif::warning << "Service3Housekeeping::handleUnrequestedReply: Could not generate reply!" <<
+                std::endl;
+#else
+        sif::printWarning("Service3Housekeeping::handleUnrequestedReply: "
+                "Could not generate reply!\n");
 #endif
-	}
+    }
 }
 
 MessageQueueId_t Service3Housekeeping::getHkQueue() const {
-	return commandQueue->getId();
+    return commandQueue->getId();
 }
 
 ReturnValue_t Service3Housekeeping::generateHkReply(
-		const CommandMessage* hkMessage, uint8_t subserviceId) {
-	store_address_t storeId;
+        const CommandMessage* hkMessage, uint8_t subserviceId) {
+    store_address_t storeId;
 
-	sid_t sid = HousekeepingMessage::getHkDataReply(hkMessage, &storeId);
-	auto resultPair = IPCStore->getData(storeId);
-	if(resultPair.first != HasReturnvaluesIF::RETURN_OK) {
-		return resultPair.first;
-	}
+    sid_t sid = HousekeepingMessage::getHkDataReply(hkMessage, &storeId);
+    auto resultPair = IPCStore->getData(storeId);
+    if(resultPair.first != HasReturnvaluesIF::RETURN_OK) {
+        return resultPair.first;
+    }
 
-	HkPacket hkPacket(sid, resultPair.second.data(), resultPair.second.size());
-	return sendTmPacket(static_cast<uint8_t>(subserviceId),
-			hkPacket.hkData, hkPacket.hkSize, nullptr, 0);
+    HkPacket hkPacket(sid, resultPair.second.data(), resultPair.second.size());
+    return sendTmPacket(static_cast<uint8_t>(subserviceId),
+            hkPacket.hkData, hkPacket.hkSize, nullptr, 0);
 }
 
 sid_t Service3Housekeeping::buildSid(object_id_t objectId,
-		const uint8_t** tcData, size_t* tcDataLen) {
-	sid_t targetSid;
-	targetSid.objectId = objectId;
-	// skip deserialization of object ID, was already done.
-	*tcData += sizeof(object_id_t);
-	*tcDataLen -= sizeof(object_id_t);
-	// size check is expected to be performed beforehand!
-	SerializeAdapter::deSerialize(&targetSid.ownerSetId, tcData, tcDataLen,
-			SerializeIF::Endianness::BIG);
-	return targetSid;
+        const uint8_t** tcData, size_t* tcDataLen) {
+    sid_t targetSid;
+    targetSid.objectId = objectId;
+    // skip deserialization of object ID, was already done.
+    *tcData += sizeof(object_id_t);
+    *tcDataLen -= sizeof(object_id_t);
+    // size check is expected to be performed beforehand!
+    SerializeAdapter::deSerialize(&targetSid.ownerSetId, tcData, tcDataLen,
+            SerializeIF::Endianness::BIG);
+    return targetSid;
 }
diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp
index 259f2ccb..8b6f7a09 100644
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@@ -7,6 +7,7 @@
 #include "../ipc/QueueFactory.h"
 #include "../tmtcpacket/pus/TcPacketStored.h"
 #include "../tmtcpacket/pus/TmPacketStored.h"
+#include "../serviceinterface/ServiceInterface.h"
 
 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
 object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
@@ -104,9 +105,27 @@ ReturnValue_t CommandingServiceBase::initialize() {
 void CommandingServiceBase::handleCommandQueue() {
 	CommandMessage reply;
 	ReturnValue_t result = RETURN_FAILED;
-	for (result = commandQueue->receiveMessage(&reply); result == RETURN_OK;
-			result = commandQueue->receiveMessage(&reply)) {
-		handleCommandMessage(&reply);
+	while(true) {
+	    result = commandQueue->receiveMessage(&reply);
+	    if (result == HasReturnvaluesIF::RETURN_OK) {
+	        handleCommandMessage(&reply);
+	        continue;
+	    }
+	    else if(result == MessageQueueIF::EMPTY) {
+	        break;
+	    }
+	    else {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+	        sif::warning << "CommandingServiceBase::handleCommandQueue: Receiving message failed"
+	                "with code" << result << std::endl;
+#else
+	        sif::printWarning("CommandingServiceBase::handleCommandQueue: Receiving message "
+	                "failed with code %d\n", result);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+	        break;
+	    }
 	}
 }