action
container
contrib
controller
coordinates
datalinklayer
datapool
devicehandlers
events
fdir
globalfunctions
health
internalError
ipc
memory
modes
monitoring
objectmanager
osal
parameters
power
pus
returnvalues
rmap
serialize
serviceinterface
storagemanager
subsystem
tasks
tcdistribution
thermal
timemanager
tmstorage
tmtcpacket
tmtcservices
AcceptsTelecommandsIF.h
AcceptsTelemetryIF.h
AcceptsVerifyMessageIF.h
CommandingServiceBase.cpp
CommandingServiceBase.h
PusServiceBase.cpp
PusServiceBase.h
PusVerificationReport.cpp
PusVerificationReport.h
ServiceTypes.h
SourceSequenceCounter.h
TmTcBridge.cpp
TmTcBridge.h
TmTcMessage.cpp
TmTcMessage.h
VerificationCodes.h
VerificationReporter.cpp
VerificationReporter.h
unittest
.gitignore
.gitmodules
LICENSE
NOTICE
fsfw.mk
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_;
|
|
}
|