diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp
index e5c61cae..cb5ffd04 100644
--- a/osal/linux/MessageQueue.cpp
+++ b/osal/linux/MessageQueue.cpp
@@ -1,8 +1,8 @@
-#include "../../serviceinterface/ServiceInterfaceStream.h"
 #include "MessageQueue.h"
+#include "../../serviceinterface/ServiceInterfaceStream.h"
+#include "../../objectmanager/ObjectManagerIF.h"
 
 #include <fstream>
-
 #include <fcntl.h>           /* For O_* constants */
 #include <sys/stat.h>        /* For mode constants */
 #include <cstring>
diff --git a/pus/CService200ModeCommanding.cpp b/pus/CService200ModeCommanding.cpp
index 32f7e5a7..c63b47a6 100644
--- a/pus/CService200ModeCommanding.cpp
+++ b/pus/CService200ModeCommanding.cpp
@@ -107,13 +107,23 @@ ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(
 		const CommandMessage *reply, object_id_t objectId) {
 	ModePacket wrongModeReply(objectId, ModeMessage::getMode(reply),
 			ModeMessage::getSubmode(reply));
-	return sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
+	ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
+	if(result == RETURN_OK){
+		// We want to produce an error here in any case because the mode was not correct
+		return RETURN_FAILED;
+	}
+	return result;
 }
 
 ReturnValue_t CService200ModeCommanding::prepareCantReachModeReply(
 		const CommandMessage *reply, object_id_t objectId) {
 	CantReachModePacket cantReachModePacket(objectId,
 	        ModeMessage::getCantReachModeReason(reply));
-	return sendTmPacket(Subservice::REPLY_CANT_REACH_MODE,
+	ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE,
 	        &cantReachModePacket);
+	if(result == RETURN_OK){
+		// We want to produce an error here in any case because the mode was not reached
+		return RETURN_FAILED;
+	}
+	return result;
 }
diff --git a/tmtcservices/PusVerificationReport.cpp b/tmtcservices/PusVerificationReport.cpp
index b047576b..bfad9ec5 100644
--- a/tmtcservices/PusVerificationReport.cpp
+++ b/tmtcservices/PusVerificationReport.cpp
@@ -4,31 +4,6 @@
 PusVerificationMessage::PusVerificationMessage() {
 }
 
-//PusVerificationMessage::PusVerificationMessage(uint8_t set_report_id,
-//		TcPacketBase* current_packet, ReturnValue_t set_error_code,
-//		uint8_t set_step, uint32_t parameter1, uint32_t parameter2) {
-//	uint8_t ackFlags = current_packet->getAcknowledgeFlags();
-//	uint16_t tcPacketId = current_packet->getPacketId();
-//	uint16_t tcSequenceControl = current_packet->getPacketSequenceControl();
-//	uint8_t* data = this->getBuffer();
-//	data[messageSize] = set_report_id;
-//	messageSize += sizeof(set_report_id);
-//	data[messageSize] = ackFlags;
-//	messageSize += sizeof(ackFlags);
-//	memcpy(&data[messageSize], &tcPacketId, sizeof(tcPacketId));
-//	messageSize += sizeof(tcPacketId);
-//	memcpy(&data[messageSize], &tcSequenceControl, sizeof(tcSequenceControl));
-//	messageSize += sizeof(tcSequenceControl);
-//	data[messageSize] = set_step;
-//	messageSize += sizeof(set_step);
-//	memcpy(&data[messageSize], &set_error_code, sizeof(set_error_code));
-//	messageSize += sizeof(set_error_code);
-//	memcpy(&data[messageSize], &parameter1, sizeof(parameter1));
-//	messageSize += sizeof(parameter1);
-//	memcpy(&data[messageSize], &parameter2, sizeof(parameter2));
-//	messageSize += sizeof(parameter2);
-//}
-
 PusVerificationMessage::PusVerificationMessage(uint8_t set_report_id,
 		uint8_t ackFlags, uint16_t tcPacketId, uint16_t tcSequenceControl,
 		ReturnValue_t set_error_code, uint8_t set_step, uint32_t parameter1,
diff --git a/tmtcservices/PusVerificationReport.h b/tmtcservices/PusVerificationReport.h
index 9dce95ac..88954cfd 100644
--- a/tmtcservices/PusVerificationReport.h
+++ b/tmtcservices/PusVerificationReport.h
@@ -7,6 +7,8 @@
 #include "../tmtcpacket/pus/TcPacketBase.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 
+#include <cstdint>
+
 class PusVerificationMessage: public MessageQueueMessage {
 private:
 	struct verifciationMessageContent {