428 lines
14 KiB
C++
428 lines
14 KiB
C++
#include "../tcdistribution/PUSDistributorIF.h"
|
|
#include "AcceptsTelemetryIF.h"
|
|
#include "../objectmanager/ObjectManagerIF.h"
|
|
|
|
#include "CommandingServiceBase.h"
|
|
#include "TmTcMessage.h"
|
|
#include "../ipc/QueueFactory.h"
|
|
#include "../tmtcpacket/pus/TcPacketStored.h"
|
|
#include "../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 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);
|
|
QueueFactory::instance()->deleteMessageQueue(requestQueue);
|
|
}
|
|
|
|
|
|
ReturnValue_t CommandingServiceBase::performOperation(uint8_t opCode) {
|
|
handleCommandQueue();
|
|
handleRequestQueue();
|
|
checkTimeout();
|
|
doPeriodicOperation();
|
|
return RETURN_OK;
|
|
}
|
|
|
|
|
|
uint16_t CommandingServiceBase::getIdentifier() {
|
|
return service;
|
|
}
|
|
|
|
|
|
MessageQueueId_t CommandingServiceBase::getRequestQueue() {
|
|
return requestQueue->getId();
|
|
}
|
|
|
|
|
|
ReturnValue_t CommandingServiceBase::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
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 == nullptr or distributor == nullptr) {
|
|
sif::error << "CommandingServiceBase::intialize: Packet source or "
|
|
"packet destination invalid!" << std::endl;
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
distributor->registerService(this);
|
|
requestQueue->setDefaultDestination(
|
|
packetForwarding->getReportReceptionQueue());
|
|
|
|
IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
|
|
TCStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
|
|
|
|
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;
|
|
|
|
}
|
|
|
|
void CommandingServiceBase::handleCommandQueue() {
|
|
CommandMessage reply;
|
|
ReturnValue_t result = RETURN_FAILED;
|
|
for (result = commandQueue->receiveMessage(&reply); result == RETURN_OK;
|
|
result = commandQueue->receiveMessage(&reply)) {
|
|
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->second.command, &iter->second.state,
|
|
&nextCommand, iter->second.objectId, &isStep);
|
|
|
|
/* If the child implementation does not implement special handling for
|
|
* rejected replies (RETURN_FAILED or INVALID_REPLY 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 or result == INVALID_REPLY)) {
|
|
result = reply->getReplyRejectedReason();
|
|
failureParameter1 = iter->second.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->second.tcInfo.ackFlags,
|
|
iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
|
|
result, ++iter->second.step, failureParameter1,
|
|
failureParameter2);
|
|
} else {
|
|
verificationReporter.sendFailureReport(
|
|
TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
|
|
iter->second.tcInfo.tcPacketId, iter->second.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->second.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->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
|
iter->second.tcInfo.tcSequenceControl, ++iter->second.step);
|
|
}
|
|
else {
|
|
verificationReporter.sendSuccessReport(
|
|
TC_VERIFY::COMPLETION_SUCCESS,
|
|
iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
|
iter->second.tcInfo.tcSequenceControl, 0);
|
|
checkAndExecuteFifo(iter);
|
|
}
|
|
}
|
|
else {
|
|
if (isStep) {
|
|
nextCommand->clearCommandMessage();
|
|
verificationReporter.sendFailureReport(
|
|
TC_VERIFY::PROGRESS_FAILURE, iter->second.tcInfo.ackFlags,
|
|
iter->second.tcInfo.tcPacketId,
|
|
iter->second.tcInfo.tcSequenceControl, sendResult,
|
|
++iter->second.step, failureParameter1, failureParameter2);
|
|
} else {
|
|
nextCommand->clearCommandMessage();
|
|
verificationReporter.sendFailureReport(
|
|
TC_VERIFY::COMPLETION_FAILURE,
|
|
iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
|
iter->second.tcInfo.tcSequenceControl, sendResult, 0,
|
|
failureParameter1, failureParameter2);
|
|
}
|
|
failureParameter1 = 0;
|
|
failureParameter2 = 0;
|
|
checkAndExecuteFifo(iter);
|
|
}
|
|
}
|
|
|
|
void CommandingServiceBase::handleRequestQueue() {
|
|
TmTcMessage message;
|
|
ReturnValue_t result;
|
|
store_address_t address;
|
|
TcPacketStored packet;
|
|
MessageQueueId_t queue;
|
|
object_id_t objectId;
|
|
for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
|
|
result = requestQueue->receiveMessage(&message)) {
|
|
address = message.getStorageId();
|
|
packet.setStoreAddress(address);
|
|
|
|
if ((packet.getSubService() == 0)
|
|
or (isValidSubservice(packet.getSubService()) != RETURN_OK)) {
|
|
rejectPacket(TC_VERIFY::START_FAILURE, &packet, INVALID_SUBSERVICE);
|
|
continue;
|
|
}
|
|
result = getMessageQueueAndObject(packet.getSubService(),
|
|
packet.getApplicationData(), packet.getApplicationDataSize(),
|
|
&queue, &objectId);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
rejectPacket(TC_VERIFY::START_FAILURE, &packet, result);
|
|
continue;
|
|
}
|
|
|
|
//Is a command already active for the target object?
|
|
CommandMapIter iter;
|
|
iter = commandMap.find(queue);
|
|
|
|
if (iter != commandMap.end()) {
|
|
result = iter->second.fifo.insert(address);
|
|
if (result != RETURN_OK) {
|
|
rejectPacket(TC_VERIFY::START_FAILURE, &packet, OBJECT_BUSY);
|
|
}
|
|
} else {
|
|
CommandInfo newInfo; //Info will be set by startExecution if neccessary
|
|
newInfo.objectId = objectId;
|
|
result = commandMap.insert(queue, newInfo, &iter);
|
|
if (result != RETURN_OK) {
|
|
rejectPacket(TC_VERIFY::START_FAILURE, &packet, BUSY);
|
|
} else {
|
|
startExecution(&packet, iter);
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
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(
|
|
requestQueue->getDefaultDestination(), requestQueue->getId());
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
this->tmPacketCounter++;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
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,
|
|
CommandMapIter iter) {
|
|
ReturnValue_t result = RETURN_OK;
|
|
CommandMessage command;
|
|
iter->second.subservice = storedPacket->getSubService();
|
|
result = prepareCommand(&command, iter->second.subservice,
|
|
storedPacket->getApplicationData(),
|
|
storedPacket->getApplicationDataSize(), &iter->second.state,
|
|
iter->second.objectId);
|
|
|
|
ReturnValue_t sendResult = RETURN_OK;
|
|
switch (result) {
|
|
case RETURN_OK:
|
|
if (command.getCommand() != CommandMessage::CMD_NONE) {
|
|
sendResult = commandQueue->sendMessage(iter.value->first,
|
|
&command);
|
|
}
|
|
if (sendResult == RETURN_OK) {
|
|
Clock::getUptime(&iter->second.uptimeOfStart);
|
|
iter->second.step = 0;
|
|
iter->second.subservice = storedPacket->getSubService();
|
|
iter->second.command = command.getCommand();
|
|
iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
|
|
iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
|
|
iter->second.tcInfo.tcSequenceControl =
|
|
storedPacket->getPacketSequenceControl();
|
|
acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket);
|
|
} else {
|
|
command.clearCommandMessage();
|
|
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
|
|
checkAndExecuteFifo(iter);
|
|
}
|
|
break;
|
|
case EXECUTION_COMPLETE:
|
|
if (command.getCommand() != CommandMessage::CMD_NONE) {
|
|
//Fire-and-forget command.
|
|
sendResult = commandQueue->sendMessage(iter.value->first,
|
|
&command);
|
|
}
|
|
if (sendResult == RETURN_OK) {
|
|
verificationReporter.sendSuccessReport(TC_VERIFY::START_SUCCESS,
|
|
storedPacket);
|
|
acceptPacket(TC_VERIFY::COMPLETION_SUCCESS, storedPacket);
|
|
checkAndExecuteFifo(iter);
|
|
} else {
|
|
command.clearCommandMessage();
|
|
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, sendResult);
|
|
checkAndExecuteFifo(iter);
|
|
}
|
|
break;
|
|
default:
|
|
rejectPacket(TC_VERIFY::START_FAILURE, storedPacket, result);
|
|
checkAndExecuteFifo(iter);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
void CommandingServiceBase::rejectPacket(uint8_t report_id,
|
|
TcPacketStored* packet, ReturnValue_t error_code) {
|
|
verificationReporter.sendFailureReport(report_id, packet, error_code);
|
|
packet->deletePacket();
|
|
}
|
|
|
|
|
|
void CommandingServiceBase::acceptPacket(uint8_t reportId,
|
|
TcPacketStored* packet) {
|
|
verificationReporter.sendSuccessReport(reportId, packet);
|
|
packet->deletePacket();
|
|
}
|
|
|
|
|
|
void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
|
|
store_address_t address;
|
|
if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
|
|
commandMap.erase(&iter);
|
|
} else {
|
|
TcPacketStored newPacket(address);
|
|
startExecution(&newPacket, iter);
|
|
}
|
|
}
|
|
|
|
|
|
void CommandingServiceBase::handleUnrequestedReply(CommandMessage* reply) {
|
|
reply->clearCommandMessage();
|
|
}
|
|
|
|
|
|
inline void CommandingServiceBase::doPeriodicOperation() {
|
|
}
|
|
|
|
MessageQueueId_t CommandingServiceBase::getCommandQueue() {
|
|
return commandQueue->getId();
|
|
}
|
|
|
|
void CommandingServiceBase::checkTimeout() {
|
|
uint32_t uptime;
|
|
Clock::getUptime(&uptime);
|
|
CommandMapIter iter;
|
|
for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
|
|
if ((iter->second.uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
|
|
verificationReporter.sendFailureReport(
|
|
TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
|
|
iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
|
|
TIMEOUT);
|
|
checkAndExecuteFifo(iter);
|
|
}
|
|
}
|
|
}
|
|
|
|
void CommandingServiceBase::setTaskIF(PeriodicTaskIF* task_) {
|
|
executingTask = task_;
|
|
}
|