CSB new update init

tmtcservices/CommandingServiceBase.cpp

@@ -1,26 +1,35 @@
-/*
+#include <framework/tcdistribution/PUSDistributorIF.h>
- * CommandingServiceBase.cpp
+#include <framework/tmtcservices/AcceptsTelemetryIF.h>
- *
+#include <framework/objectmanager/ObjectManagerIF.h>
- *  Created on: 28.08.2019
- *      Author: gaisser
- */
 
 #include <framework/tmtcservices/CommandingServiceBase.h>
+#include <framework/tmtcservices/TmTcMessage.h>
+#include <framework/ipc/QueueFactory.h>
+#include <framework/tmtcpacket/pus/TcPacketStored.h>
+#include <framework/tmtcpacket/pus/TmPacketStored.h>
+
+object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
+object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
 
 CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId,
 		uint16_t apid, uint8_t service, uint8_t numberOfParallelCommands,
-		uint16_t commandTimeout_seconds, object_id_t setPacketSource,
+		uint16_t commandTimeoutSeconds, size_t queueDepth) :
-		object_id_t setPacketDestination, size_t queueDepth) :
+		SystemObject(setObjectId), apid(apid), service(service),
-		SystemObject(setObjectId), apid(apid), service(service), timeout_seconds(
+		timeoutSeconds(commandTimeoutSeconds),
-				commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore(
+		commandMap(numberOfParallelCommands) {
-		NULL), commandQueue(NULL), requestQueue(NULL), commandMap(
-				numberOfParallelCommands), failureParameter1(0), failureParameter2(
-				0), packetSource(setPacketSource), packetDestination(
-				setPacketDestination),executingTask(NULL) {
 	commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
 	requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
 }
 
+void CommandingServiceBase::setPacketSource(object_id_t packetSource) {
+	this->packetSource = packetSource;
+}
+
+void CommandingServiceBase::setPacketDestination(
+		object_id_t packetDestination) {
+	this->packetDestination = packetDestination;
+}
+
 
 CommandingServiceBase::~CommandingServiceBase() {
 	QueueFactory::instance()->deleteMessageQueue(commandQueue);
@@ -53,12 +62,22 @@ ReturnValue_t CommandingServiceBase::initialize() {
 		return result;
 	}
 
+	if(packetDestination == objects::NO_OBJECT) {
+	    packetDestination = defaultPacketDestination;
+	}
 	AcceptsTelemetryIF* packetForwarding =
 			objectManager->get<AcceptsTelemetryIF>(packetDestination);
+
+	if(packetSource == objects::NO_OBJECT) {
+	    packetSource = defaultPacketSource;
+	}
 	PUSDistributorIF* distributor = objectManager->get<PUSDistributorIF>(
 			packetSource);
-	if ((packetForwarding == NULL) && (distributor == NULL)) {
+
-		return RETURN_FAILED;
+	if (packetForwarding == nullptr or distributor == nullptr) {
+	    sif::error << "CommandingServiceBase::intialize: Packet source or "
+	            "packet destination invalid!" << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	distributor->registerService(this);
@@ -68,8 +87,10 @@ ReturnValue_t CommandingServiceBase::initialize() {
 	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
 	TCStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
 
-	if ((IPCStore == NULL) || (TCStore == NULL)) {
+	if (IPCStore == nullptr or TCStore == nullptr) {
-		return RETURN_FAILED;
+	    sif::error << "CommandingServiceBase::intialize: IPC store or TC store "
+	                    "not initialized yet!" << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
 	}
 
 	return RETURN_OK;
@@ -77,97 +98,126 @@ ReturnValue_t CommandingServiceBase::initialize() {
 }
 
 void CommandingServiceBase::handleCommandQueue() {
-	CommandMessage reply, nextCommand;
+	CommandMessage reply;
-	ReturnValue_t result, sendResult = RETURN_OK;
+	ReturnValue_t result = RETURN_FAILED;
-	bool isStep = false;
 	for (result = commandQueue->receiveMessage(&reply); result == RETURN_OK;
 			result = commandQueue->receiveMessage(&reply)) {
-		isStep = false;
+		handleCommandMessage(&reply);
-		typename FixedMap<MessageQueueId_t,
-				CommandingServiceBase::CommandInfo>::Iterator iter;
-		if (reply.getSender() == MessageQueueIF::NO_QUEUE) {
-			handleUnrequestedReply(&reply);
-			continue;
-		}
-		if ((iter = commandMap.find(reply.getSender())) == commandMap.end()) {
-			handleUnrequestedReply(&reply);
-			continue;
-		}
-		nextCommand.setCommand(CommandMessage::CMD_NONE);
-		result = handleReply(&reply, iter->command, &iter->state, &nextCommand,
-				iter->objectId, &isStep);
-		switch (result) {
-		case EXECUTION_COMPLETE:
-		case RETURN_OK:
-		case NO_STEP_MESSAGE:
-			iter->command = nextCommand.getCommand();
-			if (nextCommand.getCommand() != CommandMessage::CMD_NONE) {
-				sendResult = commandQueue->sendMessage(reply.getSender(),
-						&nextCommand);
-			}
-			if (sendResult == RETURN_OK) {
-				if (isStep) {
-					if (result != NO_STEP_MESSAGE) {
-						verificationReporter.sendSuccessReport(
-								TC_VERIFY::PROGRESS_SUCCESS,
-								iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
-								iter->tcInfo.tcSequenceControl, ++iter->step);
-					}
-				} else {
-					verificationReporter.sendSuccessReport(
-							TC_VERIFY::COMPLETION_SUCCESS,
-							iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
-							iter->tcInfo.tcSequenceControl, 0);
-					checkAndExecuteFifo(&iter);
-				}
-			} else {
-				if (isStep) {
-					nextCommand.clearCommandMessage();
-					verificationReporter.sendFailureReport(
-							TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
-							iter->tcInfo.tcPacketId,
-							iter->tcInfo.tcSequenceControl, sendResult,
-							++iter->step, failureParameter1, failureParameter2);
-				} else {
-					nextCommand.clearCommandMessage();
-					verificationReporter.sendFailureReport(
-							TC_VERIFY::COMPLETION_FAILURE,
-							iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
-							iter->tcInfo.tcSequenceControl, sendResult, 0,
-							failureParameter1, failureParameter2);
-				}
-				failureParameter1 = 0;
-				failureParameter2 = 0;
-				checkAndExecuteFifo(&iter);
-			}
-			break;
-		case INVALID_REPLY:
-			//might be just an unrequested reply at a bad moment
-			handleUnrequestedReply(&reply);
-			break;
-		default:
-			if (isStep) {
-				verificationReporter.sendFailureReport(
-						TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
-						iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
-						result, ++iter->step, failureParameter1,
-						failureParameter2);
-			} else {
-				verificationReporter.sendFailureReport(
-						TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
-						iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
-						result, 0, failureParameter1, failureParameter2);
-			}
-			failureParameter1 = 0;
-			failureParameter2 = 0;
-			checkAndExecuteFifo(&iter);
-			break;
-		}
-
 	}
 }
 
 
+void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
+	bool isStep = false;
+	CommandMessage nextCommand;
+	CommandMapIter iter = commandMap.find(reply->getSender());
+
+	// handle unrequested reply first
+	if (reply->getSender() == MessageQueueIF::NO_QUEUE or
+			iter == commandMap.end()) {
+		handleUnrequestedReply(reply);
+		return;
+	}
+	nextCommand.setCommand(CommandMessage::CMD_NONE);
+
+
+	// Implemented by child class, specifies what to do with reply.
+	ReturnValue_t result = handleReply(reply, iter->command, &iter->state,
+			&nextCommand, iter->objectId, &isStep);
+
+	/* If the child implementation does not implement special handling for
+	 * rejected replies (RETURN_FAILED is returned), a failure verification
+	 * will be generated with the reason as the return code and the initial
+	 * command as failure parameter 1 */
+	if(reply->getCommand() == CommandMessage::REPLY_REJECTED and
+			result == RETURN_FAILED) {
+	    result = reply->getReplyRejectedReason();
+	    failureParameter1 = iter->command;
+	}
+
+	switch (result) {
+	case EXECUTION_COMPLETE:
+	case RETURN_OK:
+	case NO_STEP_MESSAGE:
+		// handle result of reply handler implemented by developer.
+		handleReplyHandlerResult(result, iter, &nextCommand, reply, isStep);
+		break;
+	case INVALID_REPLY:
+		//might be just an unrequested reply at a bad moment
+		handleUnrequestedReply(reply);
+		break;
+	default:
+		if (isStep) {
+			verificationReporter.sendFailureReport(
+					TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
+					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
+					result, ++iter->step, failureParameter1,
+					failureParameter2);
+		} else {
+			verificationReporter.sendFailureReport(
+					TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
+					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
+					result, 0, failureParameter1, failureParameter2);
+		}
+		failureParameter1 = 0;
+		failureParameter2 = 0;
+		checkAndExecuteFifo(iter);
+		break;
+	}
+
+}
+
+void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
+		CommandMapIter iter, CommandMessage* nextCommand,
+		CommandMessage* reply, bool& isStep) {
+	iter->command = nextCommand->getCommand();
+
+	// In case a new command is to be sent immediately, this is performed here.
+	// If no new command is sent, only analyse reply result by initializing
+	// sendResult as RETURN_OK
+	ReturnValue_t sendResult = RETURN_OK;
+	if (nextCommand->getCommand() != CommandMessage::CMD_NONE) {
+		sendResult = commandQueue->sendMessage(reply->getSender(),
+				nextCommand);
+	}
+
+	if (sendResult == RETURN_OK) {
+		if (isStep and result != NO_STEP_MESSAGE) {
+			verificationReporter.sendSuccessReport(
+					TC_VERIFY::PROGRESS_SUCCESS,
+					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->tcInfo.tcSequenceControl, ++iter->step);
+		}
+		else {
+			verificationReporter.sendSuccessReport(
+					TC_VERIFY::COMPLETION_SUCCESS,
+					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->tcInfo.tcSequenceControl, 0);
+			checkAndExecuteFifo(iter);
+		}
+	}
+	else {
+		if (isStep) {
+			nextCommand->clearCommandMessage();
+			verificationReporter.sendFailureReport(
+					TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
+					iter->tcInfo.tcPacketId,
+					iter->tcInfo.tcSequenceControl, sendResult,
+					++iter->step, failureParameter1, failureParameter2);
+		} else {
+			nextCommand->clearCommandMessage();
+			verificationReporter.sendFailureReport(
+					TC_VERIFY::COMPLETION_FAILURE,
+					iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
+					iter->tcInfo.tcSequenceControl, sendResult, 0,
+					failureParameter1, failureParameter2);
+		}
+		failureParameter1 = 0;
+		failureParameter2 = 0;
+		checkAndExecuteFifo(iter);
+	}
+}
+
 void CommandingServiceBase::handleRequestQueue() {
 	TmTcMessage message;
 	ReturnValue_t result;
@@ -180,8 +230,8 @@ void CommandingServiceBase::handleRequestQueue() {
 		address = message.getStorageId();
 		packet.setStoreAddress(address);
 
-		if ((packet.getSubService() == 0)
+		if (packet.getSubService() == 0
-				|| (isValidSubservice(packet.getSubService()) != RETURN_OK)) {
+				or isValidSubservice(packet.getSubService()) != RETURN_OK) {
 			rejectPacket(TC_VERIFY::START_FAILURE, &packet, INVALID_SUBSERVICE);
 			continue;
 		}
@@ -194,8 +244,7 @@ void CommandingServiceBase::handleRequestQueue() {
 		}
 
 		//Is a command already active for the target object?
-		typename FixedMap<MessageQueueId_t,
+		CommandMapIter iter;
-				CommandingServiceBase::CommandInfo>::Iterator iter;
 		iter = commandMap.find(queue);
 
 		if (iter != commandMap.end()) {
@@ -210,7 +259,7 @@ void CommandingServiceBase::handleRequestQueue() {
 			if (result != RETURN_OK) {
 				rejectPacket(TC_VERIFY::START_FAILURE, &packet, BUSY);
 			} else {
-				startExecution(&packet, &iter);
+				startExecution(&packet, iter);
 			}
 		}
 
@@ -218,9 +267,9 @@ void CommandingServiceBase::handleRequestQueue() {
 }
 
 
-void CommandingServiceBase::sendTmPacket(uint8_t subservice,
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
-		const uint8_t* data, uint32_t dataLen, const uint8_t* headerData,
+		const uint8_t* data, size_t dataLen, const uint8_t* headerData,
-		uint32_t headerSize) {
+		size_t headerSize) {
 	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
 			this->tmPacketCounter, data, dataLen, headerData, headerSize);
 	ReturnValue_t result = tmPacketStored.sendPacket(
@@ -228,79 +277,79 @@ void CommandingServiceBase::sendTmPacket(uint8_t subservice,
 	if (result == HasReturnvaluesIF::RETURN_OK) {
 		this->tmPacketCounter++;
 	}
+	return result;
 }
 
 
-void CommandingServiceBase::sendTmPacket(uint8_t subservice,
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
-		object_id_t objectId, const uint8_t *data, uint32_t dataLen) {
+        object_id_t objectId, const uint8_t *data, size_t dataLen) {
-	uint8_t buffer[sizeof(object_id_t)];
+    uint8_t buffer[sizeof(object_id_t)];
-	uint8_t* pBuffer = buffer;
+    uint8_t* pBuffer = buffer;
-	size_t size = 0;
+    size_t size = 0;
-	SerializeAdapter::serialize(&objectId, &pBuffer, &size,
+    SerializeAdapter::serialize(&objectId, &pBuffer, &size,
-			sizeof(object_id_t), SerializeIF::Endianness::BIG);
+                sizeof(object_id_t), SerializeIF::Endianness::BIG);
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
-			this->tmPacketCounter, data, dataLen, buffer, size);
+            this->tmPacketCounter, data, dataLen, buffer, size);
-	ReturnValue_t result = tmPacketStored.sendPacket(
+    ReturnValue_t result = tmPacketStored.sendPacket(
-			requestQueue->getDefaultDestination(), requestQueue->getId());
+            requestQueue->getDefaultDestination(), requestQueue->getId());
-	if (result == HasReturnvaluesIF::RETURN_OK) {
+    if (result == HasReturnvaluesIF::RETURN_OK) {
-		this->tmPacketCounter++;
+        this->tmPacketCounter++;
-	}
+    }
-
+    return result;
 }
 
 
-void CommandingServiceBase::sendTmPacket(uint8_t subservice,
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
-		SerializeIF* content, SerializeIF* header) {
+        SerializeIF* content, SerializeIF* header) {
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
-			this->tmPacketCounter, content, header);
+            this->tmPacketCounter, content, header);
-	ReturnValue_t result = tmPacketStored.sendPacket(
+    ReturnValue_t result = tmPacketStored.sendPacket(
-			requestQueue->getDefaultDestination(), requestQueue->getId());
+            requestQueue->getDefaultDestination(), requestQueue->getId());
-	if (result == HasReturnvaluesIF::RETURN_OK) {
+    if (result == HasReturnvaluesIF::RETURN_OK) {
-		this->tmPacketCounter++;
+        this->tmPacketCounter++;
-	}
+    }
+    return result;
 }
 
 
-void CommandingServiceBase::startExecution(
+void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
-		TcPacketStored *storedPacket,
+        CommandMapIter iter) {
-		typename FixedMap<MessageQueueId_t,
+    ReturnValue_t result = RETURN_OK;
-				CommandingServiceBase::CommandInfo>::Iterator *iter) {
+    CommandMessage command;
-	ReturnValue_t result, sendResult = RETURN_OK;
+    iter->subservice = storedPacket->getSubService();
-	CommandMessage message;
+    result = prepareCommand(&command, iter->subservice,
-	(*iter)->subservice = storedPacket->getSubService();
+            storedPacket->getApplicationData(),
-	result = prepareCommand(&message, (*iter)->subservice,
+            storedPacket->getApplicationDataSize(), &iter->state,
-			storedPacket->getApplicationData(),
+            iter->objectId);
-			storedPacket->getApplicationDataSize(), &(*iter)->state,
-			(*iter)->objectId);
 
+    ReturnValue_t sendResult = RETURN_OK;
 	switch (result) {
 	case RETURN_OK:
-		if (message.getCommand() != CommandMessage::CMD_NONE) {
+		if (command.getCommand() != CommandMessage::CMD_NONE) {
-			sendResult = commandQueue->sendMessage((*iter).value->first,
+			sendResult = commandQueue->sendMessage(iter.value->first,
-					&message);
+					&command);
 		}
 		if (sendResult == RETURN_OK) {
-			Clock::getUptime(&(*iter)->uptimeOfStart);
+			Clock::getUptime(&iter->uptimeOfStart);
-			(*iter)->step = 0;
+			iter->step = 0;
-//			(*iter)->state = 0;
+			iter->subservice = storedPacket->getSubService();
-			(*iter)->subservice = storedPacket->getSubService();
+			iter->command = command.getCommand();
-			(*iter)->command = message.getCommand();
+			iter->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-			(*iter)->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
+			iter->tcInfo.tcPacketId = storedPacket->getPacketId();
-			(*iter)->tcInfo.tcPacketId = storedPacket->getPacketId();
+			iter->tcInfo.tcSequenceControl =
-			(*iter)->tcInfo.tcSequenceControl =
 					storedPacket->getPacketSequenceControl();
 			acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket);
 		} else {
-			message.clearCommandMessage();
+			command.clearCommandMessage();
 			rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
 			checkAndExecuteFifo(iter);
 		}
 		break;
 	case EXECUTION_COMPLETE:
-		if (message.getCommand() != CommandMessage::CMD_NONE) {
+		if (command.getCommand() != CommandMessage::CMD_NONE) {
 			//Fire-and-forget command.
-			sendResult = commandQueue->sendMessage((*iter).value->first,
+			sendResult = commandQueue->sendMessage(iter.value->first,
-					&message);
+					&command);
 		}
 		if (sendResult == RETURN_OK) {
 			verificationReporter.sendSuccessReport(TC_VERIFY::START_SUCCESS,
@@ -308,7 +357,7 @@ void CommandingServiceBase::startExecution(
 			acceptPacket(TC_VERIFY::COMPLETION_SUCCESS, storedPacket);
 			checkAndExecuteFifo(iter);
 		} else {
-			message.clearCommandMessage();
+			command.clearCommandMessage();
 			rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
 			checkAndExecuteFifo(iter);
 		}
@@ -335,12 +384,10 @@ void CommandingServiceBase::acceptPacket(uint8_t reportId,
 }
 
 
-void CommandingServiceBase::checkAndExecuteFifo(
+void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter iter) {
-		typename FixedMap<MessageQueueId_t,
-				CommandingServiceBase::CommandInfo>::Iterator *iter) {
 	store_address_t address;
-	if ((*iter)->fifo.retrieve(&address) != RETURN_OK) {
+	if (iter->fifo.retrieve(&address) != RETURN_OK) {
-		commandMap.erase(iter);
+		commandMap.erase(&iter);
 	} else {
 		TcPacketStored newPacket(address);
 		startExecution(&newPacket, iter);
@@ -348,8 +395,7 @@ void CommandingServiceBase::checkAndExecuteFifo(
 }
 
 
-void CommandingServiceBase::handleUnrequestedReply(
+void CommandingServiceBase::handleUnrequestedReply(CommandMessage* reply) {
-		CommandMessage* reply) {
 	reply->clearCommandMessage();
 }
 
@@ -364,18 +410,18 @@ MessageQueueId_t CommandingServiceBase::getCommandQueue() {
 void CommandingServiceBase::checkTimeout() {
 	uint32_t uptime;
 	Clock::getUptime(&uptime);
-	typename FixedMap<MessageQueueId_t,
+	CommandMapIter iter;
-			CommandingServiceBase::CommandInfo>::Iterator iter;
 	for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
-		if ((iter->uptimeOfStart + (timeout_seconds * 1000)) < uptime) {
+		if ((iter->uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
 			verificationReporter.sendFailureReport(
 					TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
 					iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
 					TIMEOUT);
-			checkAndExecuteFifo(&iter);
+			checkAndExecuteFifo(iter);
 		}
 	}
 }
 
-
+void CommandingServiceBase::setTaskIF(PeriodicTaskIF* task_) {
-
+    executingTask = task_;
+}

tmtcservices/CommandingServiceBase.h

@@ -1,40 +1,43 @@
-#ifndef COMMANDINGSERVICEBASE_H_
+#ifndef FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
-#define COMMANDINGSERVICEBASE_H_
+#define FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
 
-#include <framework/container/FixedMap.h>
-#include <framework/container/FIFO.h>
-#include <framework/ipc/CommandMessage.h>
-#include <framework/objectmanager/ObjectManagerIF.h>
 #include <framework/objectmanager/SystemObject.h>
-#include <framework/serialize/SerializeAdapter.h>
 #include <framework/storagemanager/StorageManagerIF.h>
 #include <framework/tasks/ExecutableObjectIF.h>
-#include <framework/tcdistribution/PUSDistributorIF.h>
+#include <framework/ipc/MessageQueueIF.h>
-#include <framework/tmtcpacket/pus/TcPacketStored.h>
-#include <framework/tmtcpacket/pus/TmPacketStored.h>
 #include <framework/tmtcservices/AcceptsTelecommandsIF.h>
-#include <framework/tmtcservices/AcceptsTelemetryIF.h>
+
-#include <framework/tmtcservices/TmTcMessage.h>
 #include <framework/tmtcservices/VerificationReporter.h>
-#include <framework/internalError/InternalErrorReporterIF.h>
+#include <framework/ipc/CommandMessage.h>
-#include <framework/ipc/QueueFactory.h>
+#include <framework/container/FixedMap.h>
-#include <framework/timemanager/Clock.h>
+#include <framework/container/FIFO.h>
+#include <framework/serialize/SerializeIF.h>
+
+class TcPacketStored;
+
+namespace Factory{
+void setStaticFrameworkObjectIds();
+}
 
 /**
- * \brief This class is the basis for all PUS Services, which have to relay Telecommands to software bus.
+ * @brief 	This class is the basis for all PUS Services, which have to
+ * 			relay Telecommands to software bus.
  *
- * It manages Telecommand reception and the generation of Verification Reports like PUSServiceBase.
+ * It manages Telecommand reception and the generation of Verification Reports
- * Every class that inherits from this abstract class has to implement four adaption points:
+ * similar to PusServiceBase. This class is used if a telecommand can't be
+ * handled immediately and must be relayed to the internal software bus.
  *   - isValidSubservice
  *   - getMessageQueueAndObject
  *   - prepareCommand
  *   - handleReply
- * \ingroup pus_services
+ * @author gaisser
+ * @ingroup pus_services
  */
 class CommandingServiceBase: public SystemObject,
 		public AcceptsTelecommandsIF,
 		public ExecutableObjectIF,
 		public HasReturnvaluesIF {
+	friend void (Factory::setStaticFrameworkObjectIds)();
 public:
 	static const uint8_t INTERFACE_ID = CLASS_ID::COMMAND_SERVICE_BASE;
 	static const ReturnValue_t EXECUTION_COMPLETE = MAKE_RETURN_CODE(1);
@@ -59,10 +62,24 @@ public:
 	 */
 	CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
 			uint8_t service, uint8_t numberOfParallelCommands,
-			uint16_t commandTimeout_seconds, object_id_t setPacketSource,
+			uint16_t commandTimeoutSeconds, size_t queueDepth = 20);
-			object_id_t setPacketDestination, size_t queueDepth = 20);
 	virtual ~CommandingServiceBase();
 
+	/**
+	 * This setter can be used to set the packet source individually instead
+	 * of using the default static framework ID set in the factory.
+	 * This should be called at object initialization and not during run-time!
+	 * @param packetSource
+	 */
+	void setPacketSource(object_id_t packetSource);
+	/**
+	 * This setter can be used to set the packet destination individually
+	 * instead of using the default static framework ID set in the factory.
+	 * This should be called at object initialization and not during run-time!
+	 * @param packetDestination
+	 */
+	void setPacketDestination(object_id_t packetDestination);
+
 	/***
 	 * This is the periodically called function.
 	 * Handle request queue for external commands.
@@ -91,82 +108,111 @@ public:
 	 */
 	virtual MessageQueueId_t getCommandQueue();
 
-	virtual ReturnValue_t initialize();
+	virtual ReturnValue_t initialize() override;
 
 	/**
 	 * Implementation of ExecutableObjectIF function
 	 *
 	 * Used to setup the reference of the task, that executes this component
-	 * @param task_ Pointer to the taskIF of this task
+	 * @param task Pointer to the taskIF of this task
 	 */
-	virtual void setTaskIF(PeriodicTaskIF* task_){
+	virtual void setTaskIF(PeriodicTaskIF* task);
-		executingTask = task_;
-	};
 
 protected:
 	/**
 	 * Check the target subservice
 	 * @param subservice[in]
-	 * @return -@c RETURN_OK on success
+	 * @return
-     *         -@c INVALID_SUBSERVICE if service is not known
+	 * -@c RETURN_OK Subservice valid, continue message handling
+     * -@c INVALID_SUBSERVICE if service is not known, rejects packet.
 	 */
 	virtual ReturnValue_t isValidSubservice(uint8_t subservice) = 0;
 
 	/**
-	 * Once a TC Request is valid, the existence of the destination and its target interface is checked and retrieved.
+	 * Once a TC Request is valid, the existence of the destination and its
-	 * The target message queue ID can then be acquired by using the target interface.
+	 * target interface is checked and retrieved. The target message queue ID
+	 * can then be acquired by using the target interface.
 	 * @param subservice
 	 * @param tcData Application Data of TC Packet
 	 * @param tcDataLen
 	 * @param id MessageQueue ID is stored here
 	 * @param objectId Object ID is extracted and stored here
-	 * @return - @c RETURN_OK on success
+	 * @return
-	 *         - @c RETURN_FAILED
+	 * - @c RETURN_OK Cotinue message handling
-	 *         - @c CSB or implementation specific return codes
+	 * - @c RETURN_FAILED Reject the packet and generates a start failure
+	 *      verification
 	 */
 	virtual ReturnValue_t getMessageQueueAndObject(uint8_t subservice,
-			const uint8_t *tcData, uint32_t tcDataLen, MessageQueueId_t *id,
+			const uint8_t *tcData, size_t tcDataLen, MessageQueueId_t *id,
 			object_id_t *objectId) = 0;
 
 	/**
-	 * After the Message Queue and Object ID are determined,
+	 * After the Message Queue and Object ID are determined, the command is
-	 * the command is prepared by using an implementation specific CommandMessage type
+	 * prepared by using an implementation specific CommandMessage type
-	 * which is sent to the target object.
+	 * which is sent to the target object. It contains all necessary information
-	 * It contains all necessary information for the device to execute telecommands.
+	 * for the device to execute telecommands.
-	 * @param message[out] message to be sent to the object
+	 * @param message [out] message which can be set and is sent to the object
-	 * @param subservice[in] Subservice of the current communication
+	 * @param subservice Subservice of the current communication
-	 * @param tcData Additional data of the command
+	 * @param tcData Application data of command
-	 * @param tcDataLen Length of the additional data
+	 * @param tcDataLen Application data length
-	 * @param state[out] Setable state of the communication
+	 * @param state [out/in] Setable state of the communication.
+	 * communication
 	 * @param objectId Target object ID
-	 * @return - @c RETURN_OK on success
+	 * @return
-     *         - @c EXECUTION_COMPLETE if exectuin is finished 
+	 * - @c RETURN_OK to generate a verification start message
-     *         - any other return code will be part of (1,4) start failure
+	 * - @c EXECUTION_COMPELTE Fire-and-forget command. Generate a completion
+	 *      verification message.
+	 * - @c Anything else rejects the packets and generates a start failure
+	 *      verification.
 	 */
-	virtual ReturnValue_t prepareCommand(CommandMessage *message,
+	virtual ReturnValue_t prepareCommand(CommandMessage* message,
-			uint8_t subservice, const uint8_t *tcData, uint32_t tcDataLen,
+			uint8_t subservice, const uint8_t *tcData, size_t tcDataLen,
 			uint32_t *state, object_id_t objectId) = 0;
 
 	/**
-	 * This function is responsible for the communication between the Command Service Base
+	 * This function is implemented by child services to specify how replies
-	 * and the respective PUS Commanding Service once the execution has started.
+	 * to a command from another software component are handled.
-	 * The PUS Commanding Service receives replies from the target device and forwards them by calling this function.
+	 * @param reply
-	 * There are different translations of these replies to specify how the Command Service proceeds.
+	 * This is the reply which can be accessed via the command message
-	 * @param reply Command Message which contains information about the command
+	 * interface. The internal message pointer can be passed to different
-	 * @param previousCommand Command_t of last command
+	 * command message implementations (see CommandMessageIF)
-	 * @param state state of the communication
+	 * @param previousCommand
-	 * @param optionalNextCommand[out] An optional next command which can be set in this function
+	 * Command_t of related command
+	 * @param state [out/in]
+	 * Additional parameter which can be used to pass state information.
+	 * State of the communication
+	 * @param optionalNextCommand [out]
+	 * An optional next command which can be set in this function
 	 * @param objectId Source object ID
 	 * @param isStep Flag value to mark steps of command execution
-	 * @return - @c RETURN_OK, @c EXECUTION_COMPLETE or @c NO_STEP_MESSAGE to generate TC verification success
+	 * @return
-	 *         - @c INVALID_REPLY can handle unrequested replies
+	 * - @c RETURN_OK, @c EXECUTION_COMPLETE or @c NO_STEP_MESSAGE to
-	 *         - Anything else triggers a TC verification failure
+	 *   generate TC verification success
+	 * - @c INVALID_REPLY Calls handleUnrequestedReply
+	 * - Anything else triggers a TC verification failure. If RETURN_FAILED
+	 *   is returned and the command ID is CommandMessage::REPLY_REJECTED,
+	 *   a failure verification message with the reason as the error parameter
+	 *   and the initial command as failure parameter 1.
 	 */
-	virtual ReturnValue_t handleReply(const CommandMessage *reply,
+	virtual ReturnValue_t handleReply(const CommandMessage* reply,
 			Command_t previousCommand, uint32_t *state,
-			CommandMessage *optionalNextCommand, object_id_t objectId,
+			CommandMessage* optionalNextCommand, object_id_t objectId,
 			bool *isStep) = 0;
 
+	/**
+	 * This function can be overidden to handle unrequested reply,
+	 * when the reply sender ID is unknown or is not found is the command map.
+	 * The default implementation will clear the command message and all
+	 * its contents.
+	 * @param reply
+	 * Reply which is non-const so the default implementation can clear the
+	 * message.
+	 */
+	virtual void handleUnrequestedReply(CommandMessage* reply);
+
+	virtual void doPeriodicOperation();
+
+
 	struct CommandInfo {
 		struct tcInfo {
 			uint8_t ackFlags;
@@ -182,84 +228,92 @@ protected:
 		FIFO<store_address_t, 3> fifo;
 	};
 
+	using CommandMapIter = FixedMap<MessageQueueId_t,
+	        CommandingServiceBase::CommandInfo>::Iterator;
+
 	const uint16_t apid;
 
 	const uint8_t service;
 
-	const uint16_t timeout_seconds;
+	const uint16_t timeoutSeconds;
 
-	uint8_t tmPacketCounter;
+	uint8_t tmPacketCounter = 0;
 
-	StorageManagerIF *IPCStore;
+	StorageManagerIF *IPCStore = nullptr;
 
-	StorageManagerIF *TCStore;
+	StorageManagerIF *TCStore = nullptr;
 
-	MessageQueueIF* commandQueue;
+	MessageQueueIF* commandQueue = nullptr;
 
-	MessageQueueIF* requestQueue;
+	MessageQueueIF* requestQueue = nullptr;
 
 	VerificationReporter verificationReporter;
 
 	FixedMap<MessageQueueId_t, CommandInfo> commandMap;
 
-	uint32_t failureParameter1; //!< May be set be children to return a more precise failure condition.
+	/* May be set be children to return a more precise failure condition. */
-	uint32_t failureParameter2; //!< May be set be children to return a more precise failure condition.
+	uint32_t failureParameter1 = 0;
+	uint32_t failureParameter2 = 0;
 
-	object_id_t packetSource;
+	static object_id_t defaultPacketSource;
-
+	object_id_t packetSource = objects::NO_OBJECT;
-	object_id_t packetDestination;
+	static object_id_t defaultPacketDestination;
+	object_id_t packetDestination = objects::NO_OBJECT;
 
 	/**
-	 * Pointer to the task which executes this component, is invalid before setTaskIF was called.
+	 * Pointer to the task which executes this component,
+	 * is invalid before setTaskIF was called.
 	 */
-	PeriodicTaskIF* executingTask;
+	PeriodicTaskIF* executingTask = nullptr;
 
 	/**
-	 * Send TM data from pointer to data. If a header is supplied it is added before data
+	 * @brief   Send TM data from pointer to data.
+	 *          If a header is supplied it is added before data
 	 * @param subservice Number of subservice
 	 * @param data Pointer to the data in the Packet
 	 * @param dataLen Lenght of data in the Packet
 	 * @param headerData HeaderData will be placed before data
 	 * @param headerSize Size of HeaderData
 	 */
-	void sendTmPacket(uint8_t subservice, const uint8_t *data, uint32_t dataLen,
+	ReturnValue_t sendTmPacket(uint8_t subservice, const uint8_t *data,
-			const uint8_t* headerData = NULL, uint32_t headerSize = 0);
+	        size_t dataLen, const uint8_t* headerData = nullptr,
+	        size_t headerSize = 0);
 
 	/**
-	 * To send TM packets of objects that still need to be serialized and consist of an object ID with appended data
+	 * @brief   To send TM packets of objects that still need to be serialized
+	 *          and consist of an object ID with appended data.
 	 * @param subservice Number of subservice
 	 * @param objectId ObjectId is placed before data
 	 * @param data Data to append to the packet
 	 * @param dataLen Length of Data
 	 */
-	void sendTmPacket(uint8_t subservice, object_id_t objectId,
+	ReturnValue_t sendTmPacket(uint8_t subservice, object_id_t objectId,
-			const uint8_t *data, uint32_t dataLen);
+			const uint8_t *data, size_t dataLen);
 
 	/**
-	 * To send packets has data which is in form of a SerializeIF or Adapters implementing it
+	 * @brief   To send packets which are contained inside a class implementing
+	 *          SerializeIF.
 	 * @param subservice Number of subservice
 	 * @param content This is a pointer to the serialized packet
 	 * @param header Serialize IF header which will be placed before content
 	 */
-	void sendTmPacket(uint8_t subservice, SerializeIF* content,
+	ReturnValue_t sendTmPacket(uint8_t subservice, SerializeIF* content,
-			SerializeIF* header = NULL);
+			SerializeIF* header = nullptr);
 
-	virtual void handleUnrequestedReply(CommandMessage *reply);
+	void checkAndExecuteFifo(CommandMapIter iter);
-
-	virtual void doPeriodicOperation();
-
-	void checkAndExecuteFifo(
-			typename FixedMap<MessageQueueId_t, CommandInfo>::Iterator *iter);
 
 private:
 	/**
 	 * This method handles internal execution of a command,
-	 * once it has been started by @sa{startExecution()} in the Request Queue handler.
+	 * once it has been started by @sa{startExecution()} in the request
-	 * It handles replies generated by the devices and relayed by the specific service implementation.
+	 * queue handler.
-	 * This means that it determines further course of action depending on the return values specified
+	 * It handles replies generated by the devices and relayed by the specific
-	 * in the service implementation.
+	 * service implementation. This means that it determines further course of
+	 * action depending on the return values specified in the service
+	 * implementation.
 	 * This includes the generation of TC verification messages. Note that
-	 * the static framework object ID @c VerificationReporter::messageReceiver needs to be set.
+	 * the static framework object ID @c VerificationReporter::messageReceiver
+	 * needs to be set.
 	 *   - TM[1,5] Step Successs
 	 *   - TM[1,6] Step Failure
 	 *   - TM[1,7] Completion Success
@@ -268,10 +322,13 @@ private:
 	void handleCommandQueue();
 
 	/**
+	 * @brief       Handler function for request queue
+	 * @details
 	 * Sequence of request queue handling:
 	 * isValidSubservice -> getMessageQueueAndObject -> startExecution
-	 * Generates Start Success Reports TM[1,3] in subfunction @sa{startExecution()}
+	 * Generates a Start Success Reports TM[1,3] in subfunction
-	 * or Start Failure Report TM[1,4] by using the TC Verification Service
+	 * @sa{startExecution()} or  a Start Failure Report TM[1,4] by using the
+	 * TC Verification Service.
 	 */
 	void handleRequestQueue();
 
@@ -280,8 +337,11 @@ private:
 
 	void acceptPacket(uint8_t reportId, TcPacketStored* packet);
 
-	void startExecution(TcPacketStored *storedPacket,
+	void startExecution(TcPacketStored *storedPacket, CommandMapIter iter);
-			typename FixedMap<MessageQueueId_t, CommandInfo>::Iterator *iter);
+
+	void handleCommandMessage(CommandMessage* reply);
+	void handleReplyHandlerResult(ReturnValue_t result, CommandMapIter iter,
+			CommandMessage* nextCommand, CommandMessage* reply, bool& isStep);
 
 	void checkTimeout();
 };