diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp
index 1f514cf3..887df392 100644
--- a/osal/FreeRTOS/MessageQueue.cpp
+++ b/osal/FreeRTOS/MessageQueue.cpp
@@ -38,7 +38,9 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) {
 ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message,
 		MessageQueueId_t* receivedFrom) {
 	ReturnValue_t status = this->receiveMessage(message);
-	*receivedFrom = this->lastPartner;
+	if(status == HasReturnvaluesIF::RETURN_OK) {
+		*receivedFrom = this->lastPartner;
+	}
 	return status;
 }
 
diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp
index 0f1aa7f0..7b413eab 100644
--- a/tcdistribution/PUSDistributor.cpp
+++ b/tcdistribution/PUSDistributor.cpp
@@ -48,6 +48,7 @@ ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
 	ReturnValue_t returnValue = RETURN_OK;
 	bool errorCode = true;
 	uint16_t serviceId = service->getIdentifier();
+	//info << "Service ID: " << (int)serviceId << std::endl;
 	MessageQueueId_t queue = service->getRequestQueue();
 	errorCode = this->queueMap.insert(
 			std::pair<uint32_t, MessageQueueId_t>(serviceId, queue)).second;