CSB new update init

2020-07-16 11:47:11 +02:00
parent 7a4a2f986a
commit aca0c94c51
2 changed files with 379 additions and 273 deletions
--- a/tmtcservices/CommandingServiceBase.cpp
+++ b/tmtcservices/CommandingServiceBase.cpp
@ -1,26 +1,35 @@
-/*
- * CommandingServiceBase.cpp
- *
- *  Created on: 28.08.2019
- *      Author: gaisser
- */
+#include <framework/tcdistribution/PUSDistributorIF.h>
+#include <framework/tmtcservices/AcceptsTelemetryIF.h>
+#include <framework/objectmanager/ObjectManagerIF.h>

 #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,
-		object_id_t setPacketDestination, size_t queueDepth) :
-		SystemObject(setObjectId), apid(apid), service(service), timeout_seconds(
-				commandTimeout_seconds), tmPacketCounter(0), IPCStore(NULL), TCStore(
-		NULL), commandQueue(NULL), requestQueue(NULL), commandMap(
-				numberOfParallelCommands), failureParameter1(0), failureParameter2(
-				0), packetSource(setPacketSource), packetDestination(
-				setPacketDestination),executingTask(NULL) {
+		uint16_t commandTimeoutSeconds, size_t queueDepth) :
+		SystemObject(setObjectId), apid(apid), service(service),
+		timeoutSeconds(commandTimeoutSeconds),
+		commandMap(numberOfParallelCommands) {
 	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)) {
-		return RETURN_FAILED;
+	if (IPCStore == nullptr or TCStore == nullptr) {
+	    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;
-	ReturnValue_t result, sendResult = RETURN_OK;
-	bool isStep = false;
+	CommandMessage reply;
+	ReturnValue_t result = RETURN_FAILED;
 	for (result = commandQueue->receiveMessage(&reply); result == RETURN_OK;
 			result = commandQueue->receiveMessage(&reply)) {
-		isStep = false;
-		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;
-		}
-
+		handleCommandMessage(&reply);
 	}
 }


+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)
-				|| (isValidSubservice(packet.getSubService()) != RETURN_OK)) {
+		if (packet.getSubService() == 0
+				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,
-				CommandingServiceBase::CommandInfo>::Iterator iter;
+		CommandMapIter 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,
-		const uint8_t* data, uint32_t dataLen, const uint8_t* headerData,
-		uint32_t headerSize) {
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
+		const uint8_t* data, size_t dataLen, const uint8_t* headerData,
+		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,
-		object_id_t objectId, const uint8_t *data, uint32_t dataLen) {
-	uint8_t buffer[sizeof(object_id_t)];
-	uint8_t* pBuffer = buffer;
-	size_t size = 0;
-	SerializeAdapter::serialize(&objectId, &pBuffer, &size,
-			sizeof(object_id_t), SerializeIF::Endianness::BIG);
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
-			this->tmPacketCounter, data, dataLen, buffer, size);
-	ReturnValue_t result = tmPacketStored.sendPacket(
-			requestQueue->getDefaultDestination(), requestQueue->getId());
-	if (result == HasReturnvaluesIF::RETURN_OK) {
-		this->tmPacketCounter++;
-	}
-
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
+        object_id_t objectId, const uint8_t *data, size_t dataLen) {
+    uint8_t buffer[sizeof(object_id_t)];
+    uint8_t* pBuffer = buffer;
+    size_t size = 0;
+    SerializeAdapter::serialize(&objectId, &pBuffer, &size,
+                sizeof(object_id_t), SerializeIF::Endianness::BIG);
+    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, data, dataLen, buffer, size);
+    ReturnValue_t result = tmPacketStored.sendPacket(
+            requestQueue->getDefaultDestination(), requestQueue->getId());
+    if (result == HasReturnvaluesIF::RETURN_OK) {
+        this->tmPacketCounter++;
+    }
+    return result;
 }


-void CommandingServiceBase::sendTmPacket(uint8_t subservice,
-		SerializeIF* content, SerializeIF* header) {
-	TmPacketStored tmPacketStored(this->apid, this->service, subservice,
-			this->tmPacketCounter, content, header);
-	ReturnValue_t result = tmPacketStored.sendPacket(
-			requestQueue->getDefaultDestination(), requestQueue->getId());
-	if (result == HasReturnvaluesIF::RETURN_OK) {
-		this->tmPacketCounter++;
-	}
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice,
+        SerializeIF* content, SerializeIF* header) {
+    TmPacketStored tmPacketStored(this->apid, this->service, subservice,
+            this->tmPacketCounter, content, header);
+    ReturnValue_t result = tmPacketStored.sendPacket(
+            requestQueue->getDefaultDestination(), requestQueue->getId());
+    if (result == HasReturnvaluesIF::RETURN_OK) {
+        this->tmPacketCounter++;
+    }
+    return result;
 }


-void CommandingServiceBase::startExecution(
-		TcPacketStored *storedPacket,
-		typename FixedMap<MessageQueueId_t,
-				CommandingServiceBase::CommandInfo>::Iterator *iter) {
-	ReturnValue_t result, sendResult = RETURN_OK;
-	CommandMessage message;
-	(*iter)->subservice = storedPacket->getSubService();
-	result = prepareCommand(&message, (*iter)->subservice,
-			storedPacket->getApplicationData(),
-			storedPacket->getApplicationDataSize(), &(*iter)->state,
-			(*iter)->objectId);
+void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
+        CommandMapIter iter) {
+    ReturnValue_t result = RETURN_OK;
+    CommandMessage command;
+    iter->subservice = storedPacket->getSubService();
+    result = prepareCommand(&command, iter->subservice,
+            storedPacket->getApplicationData(),
+            storedPacket->getApplicationDataSize(), &iter->state,
+            iter->objectId);

+    ReturnValue_t sendResult = RETURN_OK;
 	switch (result) {
 	case RETURN_OK:
-		if (message.getCommand() != CommandMessage::CMD_NONE) {
-			sendResult = commandQueue->sendMessage((*iter).value->first,
-					&message);
+		if (command.getCommand() != CommandMessage::CMD_NONE) {
+			sendResult = commandQueue->sendMessage(iter.value->first,
+					&command);
 		}
 		if (sendResult == RETURN_OK) {
-			Clock::getUptime(&(*iter)->uptimeOfStart);
-			(*iter)->step = 0;
-//			(*iter)->state = 0;
-			(*iter)->subservice = storedPacket->getSubService();
-			(*iter)->command = message.getCommand();
-			(*iter)->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-			(*iter)->tcInfo.tcPacketId = storedPacket->getPacketId();
-			(*iter)->tcInfo.tcSequenceControl =
+			Clock::getUptime(&iter->uptimeOfStart);
+			iter->step = 0;
+			iter->subservice = storedPacket->getSubService();
+			iter->command = command.getCommand();
+			iter->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
+			iter->tcInfo.tcPacketId = storedPacket->getPacketId();
+			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,
-					&message);
+			sendResult = commandQueue->sendMessage(iter.value->first,
+					&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(
-		typename FixedMap<MessageQueueId_t,
-				CommandingServiceBase::CommandInfo>::Iterator *iter) {
+void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter iter) {
 	store_address_t address;
-	if ((*iter)->fifo.retrieve(&address) != RETURN_OK) {
-		commandMap.erase(iter);
+	if (iter->fifo.retrieve(&address) != RETURN_OK) {
+		commandMap.erase(&iter);
 	} else {
 		TcPacketStored newPacket(address);
 		startExecution(&newPacket, iter);
@ -348,8 +395,7 @@ void CommandingServiceBase::checkAndExecuteFifo(
 }


-void CommandingServiceBase::handleUnrequestedReply(
-		CommandMessage* reply) {
+void CommandingServiceBase::handleUnrequestedReply(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,
-			CommandingServiceBase::CommandInfo>::Iterator iter;
+	CommandMapIter 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_;
+}