2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcservices/CommandingServiceBase.h"
|
2020-12-14 21:30:39 +01:00
|
|
|
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
|
|
#include "fsfw/tcdistribution/PUSDistributorIF.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcpacket/pus/tc.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
|
|
|
|
#include "fsfw/tmtcservices/TmTcMessage.h"
|
2022-07-21 13:48:58 +02:00
|
|
|
#include "fsfw/tmtcservices/tcHelpers.h"
|
|
|
|
#include "fsfw/tmtcservices/tmHelpers.h"
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2020-07-16 11:47:11 +02:00
|
|
|
object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
|
|
|
|
object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t apid,
|
|
|
|
uint8_t service, uint8_t numberOfParallelCommands,
|
2022-07-26 13:59:09 +02:00
|
|
|
uint16_t commandTimeoutSeconds, size_t queueDepth,
|
|
|
|
VerificationReporterIF* verificationReporter)
|
2022-02-02 10:29:30 +01:00
|
|
|
: SystemObject(setObjectId),
|
|
|
|
apid(apid),
|
|
|
|
service(service),
|
|
|
|
timeoutSeconds(commandTimeoutSeconds),
|
2022-07-25 10:50:52 +02:00
|
|
|
tmStoreHelper(apid),
|
2022-07-26 11:09:57 +02:00
|
|
|
tmHelper(service, tmStoreHelper, tmSendHelper),
|
2022-07-26 13:59:09 +02:00
|
|
|
verificationReporter(verificationReporter),
|
2022-02-02 10:29:30 +01:00
|
|
|
commandMap(numberOfParallelCommands) {
|
|
|
|
commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
|
|
|
|
requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
void CommandingServiceBase::setPacketSource(object_id_t packetSource_) {
|
|
|
|
packetSource = packetSource_;
|
2020-07-16 11:47:11 +02:00
|
|
|
}
|
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
void CommandingServiceBase::setPacketDestination(object_id_t packetDestination_) {
|
|
|
|
packetDestination = packetDestination_;
|
2020-07-16 11:47:11 +02:00
|
|
|
}
|
|
|
|
|
2019-08-28 14:50:24 +02:00
|
|
|
CommandingServiceBase::~CommandingServiceBase() {
|
2022-02-02 10:29:30 +01:00
|
|
|
QueueFactory::instance()->deleteMessageQueue(commandQueue);
|
|
|
|
QueueFactory::instance()->deleteMessageQueue(requestQueue);
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t CommandingServiceBase::performOperation(uint8_t opCode) {
|
2022-02-02 10:29:30 +01:00
|
|
|
handleCommandQueue();
|
|
|
|
handleRequestQueue();
|
|
|
|
checkTimeout();
|
|
|
|
doPeriodicOperation();
|
|
|
|
return RETURN_OK;
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
uint16_t CommandingServiceBase::getIdentifier() { return service; }
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t CommandingServiceBase::getRequestQueue() { return requestQueue->getId(); }
|
2019-08-28 14:50:24 +02:00
|
|
|
|
|
|
|
ReturnValue_t CommandingServiceBase::initialize() {
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
return result;
|
|
|
|
}
|
2021-12-03 15:37:49 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (packetDestination == objects::NO_OBJECT) {
|
|
|
|
packetDestination = defaultPacketDestination;
|
|
|
|
}
|
2022-07-20 22:21:15 +02:00
|
|
|
auto* packetForwarding = ObjectManager::instance()->get<AcceptsTelemetryIF>(packetDestination);
|
2021-12-03 15:37:49 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (packetSource == objects::NO_OBJECT) {
|
|
|
|
packetSource = defaultPacketSource;
|
|
|
|
}
|
2022-07-20 22:21:15 +02:00
|
|
|
auto* distributor = ObjectManager::instance()->get<PUSDistributorIF>(packetSource);
|
2021-12-03 15:37:49 +01:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (packetForwarding == nullptr or distributor == nullptr) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "CommandingServiceBase::intialize: Packet source or "
|
|
|
|
"packet destination invalid!"
|
|
|
|
<< std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
distributor->registerService(this);
|
|
|
|
requestQueue->setDefaultDestination(packetForwarding->getReportReceptionQueue());
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
|
|
tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
if (ipcStore == nullptr or tcStore == nullptr) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "CommandingServiceBase::intialize: IPC store or TC store "
|
|
|
|
"not initialized yet!"
|
|
|
|
<< std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-07-20 22:21:15 +02:00
|
|
|
if (tmStoreHelper.getTmStore() == nullptr) {
|
|
|
|
auto* tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
|
|
|
|
if (tmStore == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-07-25 11:24:13 +02:00
|
|
|
tmStoreHelper.setTmStore(*tmStore);
|
2022-07-20 22:21:15 +02:00
|
|
|
}
|
2022-07-27 11:41:06 +02:00
|
|
|
// Generally, all TM packets will pass through a layer where the sequence count is set.
|
|
|
|
// This avoids duplicate calculation of the CRC16
|
|
|
|
tmStoreHelper.disableCrcCalculation();
|
2022-07-27 17:00:43 +02:00
|
|
|
if (tmTimeStamper == nullptr) {
|
2022-07-27 17:48:59 +02:00
|
|
|
tmTimeStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
|
|
|
|
if (tmTimeStamper == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
|
|
|
}
|
2022-07-27 17:48:59 +02:00
|
|
|
tmStoreHelper.setTimeStamper(*tmTimeStamper);
|
2022-07-27 11:41:06 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
if (errReporter == nullptr) {
|
|
|
|
errReporter =
|
|
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
|
|
if (errReporter != nullptr) {
|
2022-07-26 18:46:28 +02:00
|
|
|
tmSendHelper.setInternalErrorReporter(*errReporter);
|
2022-07-20 22:21:15 +02:00
|
|
|
}
|
2022-07-26 18:46:28 +02:00
|
|
|
} else {
|
|
|
|
tmSendHelper.setInternalErrorReporter(*errReporter);
|
2022-07-20 22:21:15 +02:00
|
|
|
}
|
2022-07-27 19:56:11 +02:00
|
|
|
tmSendHelper.setMsgQueue(*requestQueue);
|
2022-07-26 18:46:28 +02:00
|
|
|
|
2022-07-26 13:59:09 +02:00
|
|
|
if (verificationReporter == nullptr) {
|
|
|
|
verificationReporter =
|
|
|
|
ObjectManager::instance()->get<VerificationReporterIF>(objects::TC_VERIFICATOR);
|
|
|
|
if (verificationReporter == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
return RETURN_OK;
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void CommandingServiceBase::handleCommandQueue() {
|
2022-02-02 10:29:30 +01:00
|
|
|
CommandMessage reply;
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result;
|
2022-02-02 10:29:30 +01:00
|
|
|
while (true) {
|
|
|
|
result = commandQueue->receiveMessage(&reply);
|
|
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
handleCommandMessage(&reply);
|
|
|
|
continue;
|
|
|
|
} else if (result == MessageQueueIF::EMPTY) {
|
|
|
|
break;
|
|
|
|
} else {
|
2021-02-08 13:42:44 +01:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::warning << "CommandingServiceBase::handleCommandQueue: Receiving message failed"
|
|
|
|
"with code"
|
|
|
|
<< result << std::endl;
|
2021-02-08 13:42:44 +01:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printWarning(
|
|
|
|
"CommandingServiceBase::handleCommandQueue: Receiving message "
|
|
|
|
"failed with code %d\n",
|
|
|
|
result);
|
2021-02-08 13:42:44 +01:00
|
|
|
#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
|
|
|
|
#endif /* FSFW_VERBOSE_LEVEL >= 1 */
|
2022-02-02 10:29:30 +01:00
|
|
|
break;
|
2021-12-03 15:37:49 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2020-06-10 22:53:24 +02:00
|
|
|
}
|
|
|
|
|
2020-06-24 16:01:17 +02:00
|
|
|
void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
|
2022-02-02 10:29:30 +01:00
|
|
|
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) {
|
2021-12-03 15:37:49 +01:00
|
|
|
case EXECUTION_COMPLETE:
|
|
|
|
case RETURN_OK:
|
|
|
|
case NO_STEP_MESSAGE:
|
2022-02-02 10:29:30 +01:00
|
|
|
// handle result of reply handler implemented by developer.
|
|
|
|
handleReplyHandlerResult(result, iter, &nextCommand, reply, isStep);
|
|
|
|
break;
|
2021-12-03 15:37:49 +01:00
|
|
|
case INVALID_REPLY:
|
2022-02-02 10:29:30 +01:00
|
|
|
// might be just an unrequested reply at a bad moment
|
|
|
|
handleUnrequestedReply(reply);
|
|
|
|
break;
|
2021-12-03 15:37:49 +01:00
|
|
|
default:
|
2022-02-02 10:29:30 +01:00
|
|
|
if (isStep) {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationFailureWithFullInfo(tcverif::PROGRESS_FAILURE, iter->second.tcInfo,
|
|
|
|
result, true);
|
|
|
|
failParams.step = ++iter->second.step;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
|
|
|
|
result, true);
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-07-26 13:59:09 +02:00
|
|
|
verificationReporter->sendFailureReport(failParams);
|
2022-02-02 10:29:30 +01:00
|
|
|
failureParameter1 = 0;
|
|
|
|
failureParameter2 = 0;
|
|
|
|
checkAndExecuteFifo(iter);
|
|
|
|
break;
|
|
|
|
}
|
2020-06-10 22:53:24 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
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) {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationSuccessWithFullInfo(tcverif::PROGRESS_SUCCESS, iter->second.tcInfo);
|
|
|
|
successParams.step = ++iter->second.step;
|
|
|
|
verificationReporter->sendSuccessReport(successParams);
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationSuccessWithFullInfo(tcverif::COMPLETION_SUCCESS, iter->second.tcInfo);
|
|
|
|
verificationReporter->sendSuccessReport(successParams);
|
2022-02-02 10:29:30 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
2021-12-03 15:37:49 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
if (isStep) {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationFailureWithFullInfo(tcverif::PROGRESS_FAILURE, iter->second.tcInfo, result,
|
|
|
|
true);
|
|
|
|
failParams.step = ++iter->second.step;
|
2022-02-02 10:29:30 +01:00
|
|
|
nextCommand->clearCommandMessage();
|
2022-07-26 13:59:09 +02:00
|
|
|
verificationReporter->sendFailureReport(failParams);
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
|
|
|
|
result, true);
|
2022-02-02 10:29:30 +01:00
|
|
|
nextCommand->clearCommandMessage();
|
2022-07-26 13:59:09 +02:00
|
|
|
verificationReporter->sendFailureReport(failParams);
|
2021-12-03 15:37:49 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
failureParameter1 = 0;
|
|
|
|
failureParameter2 = 0;
|
|
|
|
checkAndExecuteFifo(iter);
|
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void CommandingServiceBase::handleRequestQueue() {
|
2022-02-02 10:29:30 +01:00
|
|
|
TmTcMessage message;
|
|
|
|
ReturnValue_t result;
|
|
|
|
store_address_t address;
|
|
|
|
MessageQueueId_t queue;
|
|
|
|
object_id_t objectId;
|
|
|
|
for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
|
|
|
|
result = requestQueue->receiveMessage(&message)) {
|
2022-07-27 19:40:54 +02:00
|
|
|
address = message.getStorageId();
|
|
|
|
result = setUpTcReader(address);
|
2022-07-20 22:21:15 +02:00
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
2022-07-29 10:23:59 +02:00
|
|
|
rejectPacketInvalidTc(result, address);
|
2022-07-21 11:34:11 +02:00
|
|
|
continue;
|
|
|
|
}
|
2022-07-20 22:21:15 +02:00
|
|
|
if ((tcReader.getSubService() == 0) or
|
|
|
|
(isValidSubservice(tcReader.getSubService()) != RETURN_OK)) {
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, address, INVALID_SUBSERVICE);
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
|
|
|
}
|
2021-12-03 15:37:49 +01:00
|
|
|
|
2022-07-21 11:34:11 +02:00
|
|
|
result = getMessageQueueAndObject(tcReader.getSubService(), tcReader.getUserData(),
|
|
|
|
tcReader.getUserDataLen(), &queue, &objectId);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, address, result);
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
2021-12-03 15:37:49 +01:00
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
// 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) {
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, address, OBJECT_BUSY);
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
} else {
|
|
|
|
CommandInfo newInfo; // Info will be set by startExecution if neccessary
|
|
|
|
newInfo.objectId = objectId;
|
|
|
|
result = commandMap.insert(queue, newInfo, &iter);
|
|
|
|
if (result != RETURN_OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, address, BUSY);
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2022-07-21 13:48:58 +02:00
|
|
|
startExecution(address, iter);
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-07-20 22:21:15 +02:00
|
|
|
ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, const uint8_t* sourceData,
|
|
|
|
size_t sourceDataLen) {
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result = tmHelper.prepareTmPacket(subservice, sourceData, sourceDataLen);
|
2022-07-28 13:00:53 +02:00
|
|
|
if (result != result::OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return tmHelper.storeAndSendTmPacket();
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, object_id_t objectId,
|
|
|
|
const uint8_t* data, size_t dataLen) {
|
2022-07-26 13:59:09 +02:00
|
|
|
telemetry::DataWithObjectIdPrefix dataWithObjId(objectId, data, dataLen);
|
|
|
|
ReturnValue_t result = tmHelper.prepareTmPacket(subservice, dataWithObjId);
|
2022-07-28 13:00:53 +02:00
|
|
|
if (result != result::OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return tmHelper.storeAndSendTmPacket();
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-07-25 11:26:45 +02:00
|
|
|
ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, SerializeIF& sourceData) {
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result = tmHelper.prepareTmPacket(subservice, sourceData);
|
2022-07-28 13:00:53 +02:00
|
|
|
if (result != result::OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
return tmHelper.storeAndSendTmPacket();
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-07-26 13:59:09 +02:00
|
|
|
void CommandingServiceBase::startExecution(store_address_t storeId, CommandMapIter& iter) {
|
2022-02-02 10:29:30 +01:00
|
|
|
CommandMessage command;
|
2022-07-21 13:48:58 +02:00
|
|
|
iter->second.subservice = tcReader.getSubService();
|
2022-07-26 13:59:09 +02:00
|
|
|
ReturnValue_t result =
|
|
|
|
prepareCommand(&command, iter->second.subservice, tcReader.getUserData(),
|
|
|
|
tcReader.getUserDataLen(), &iter->second.state, iter->second.objectId);
|
2022-02-02 10:29:30 +01:00
|
|
|
|
|
|
|
ReturnValue_t sendResult = RETURN_OK;
|
|
|
|
switch (result) {
|
2021-12-03 15:37:49 +01:00
|
|
|
case RETURN_OK:
|
2022-02-02 10:29:30 +01:00
|
|
|
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;
|
2022-07-21 13:48:58 +02:00
|
|
|
iter->second.subservice = tcReader.getSubService();
|
2022-02-02 10:29:30 +01:00
|
|
|
iter->second.command = command.getCommand();
|
2022-07-21 13:48:58 +02:00
|
|
|
iter->second.tcInfo.ackFlags = tcReader.getAcknowledgeFlags();
|
|
|
|
iter->second.tcInfo.tcPacketId = tcReader.getPacketIdRaw();
|
|
|
|
iter->second.tcInfo.tcSequenceControl = tcReader.getPacketSeqCtrlRaw();
|
2022-07-26 13:59:09 +02:00
|
|
|
acceptPacket(tcverif::START_SUCCESS, storeId);
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
command.clearCommandMessage();
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, storeId, sendResult);
|
2022-02-02 10:29:30 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
|
|
|
}
|
|
|
|
break;
|
2021-12-03 15:37:49 +01:00
|
|
|
case EXECUTION_COMPLETE:
|
2022-02-02 10:29:30 +01:00
|
|
|
if (command.getCommand() != CommandMessage::CMD_NONE) {
|
|
|
|
// Fire-and-forget command.
|
|
|
|
sendResult = commandQueue->sendMessage(iter.value->first, &command);
|
|
|
|
}
|
|
|
|
if (sendResult == RETURN_OK) {
|
2022-07-26 13:59:09 +02:00
|
|
|
verificationReporter->sendSuccessReport(
|
|
|
|
VerifSuccessParams(tcverif::START_SUCCESS, tcReader));
|
|
|
|
acceptPacket(tcverif::COMPLETION_SUCCESS, storeId);
|
2021-12-03 15:37:49 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
command.clearCommandMessage();
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, storeId, sendResult);
|
2022-02-02 10:29:30 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
default:
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, storeId, result);
|
2022-02-02 10:29:30 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
|
|
|
break;
|
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-07-29 10:23:59 +02:00
|
|
|
ReturnValue_t CommandingServiceBase::rejectPacketInvalidTc(ReturnValue_t errorCode,
|
|
|
|
store_address_t tcStoreId) {
|
|
|
|
failureParameter1 = INVALID_TC;
|
|
|
|
prepareVerificationFailureWithNoTcInfo(tcverif::START_FAILURE, errorCode, true);
|
|
|
|
if (tcStoreId != store_address_t::invalid()) {
|
|
|
|
tcStore->deleteData(tcStoreId);
|
|
|
|
}
|
|
|
|
return verificationReporter->sendFailureReport(failParams);
|
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t CommandingServiceBase::rejectPacket(uint8_t reportId, store_address_t tcStoreId,
|
|
|
|
ReturnValue_t errorCode) {
|
|
|
|
ReturnValue_t result =
|
|
|
|
verificationReporter->sendFailureReport(VerifFailureParams(reportId, tcReader, errorCode));
|
2022-07-20 22:21:15 +02:00
|
|
|
tcStore->deleteData(tcStoreId);
|
2022-07-29 10:23:59 +02:00
|
|
|
return result;
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-07-29 10:23:59 +02:00
|
|
|
ReturnValue_t CommandingServiceBase::acceptPacket(uint8_t reportId, store_address_t tcStoreId) {
|
|
|
|
ReturnValue_t result =
|
|
|
|
verificationReporter->sendSuccessReport(VerifSuccessParams(reportId, tcReader));
|
2022-07-20 22:21:15 +02:00
|
|
|
tcStore->deleteData(tcStoreId);
|
2022-07-29 10:23:59 +02:00
|
|
|
return result;
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2020-09-22 14:00:18 +02:00
|
|
|
void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
|
2022-02-02 10:29:30 +01:00
|
|
|
store_address_t address;
|
|
|
|
if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
|
|
|
|
commandMap.erase(&iter);
|
|
|
|
} else {
|
2022-07-21 13:48:58 +02:00
|
|
|
ReturnValue_t result = setUpTcReader(address);
|
2022-07-20 22:21:15 +02:00
|
|
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
2022-07-21 13:48:58 +02:00
|
|
|
startExecution(address, iter);
|
2022-07-20 22:21:15 +02:00
|
|
|
} else {
|
|
|
|
// TODO: Warning?
|
2022-07-26 13:59:09 +02:00
|
|
|
rejectPacket(tcverif::START_FAILURE, address, result);
|
2022-07-20 22:21:15 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2020-06-24 16:01:17 +02:00
|
|
|
void CommandingServiceBase::handleUnrequestedReply(CommandMessage* reply) {
|
2022-02-02 10:29:30 +01:00
|
|
|
reply->clearCommandMessage();
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
inline void CommandingServiceBase::doPeriodicOperation() {}
|
2019-08-28 14:50:24 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t CommandingServiceBase::getCommandQueue() { return commandQueue->getId(); }
|
2019-08-28 14:50:24 +02:00
|
|
|
|
|
|
|
void CommandingServiceBase::checkTimeout() {
|
2022-02-02 10:29:30 +01:00
|
|
|
uint32_t uptime;
|
|
|
|
Clock::getUptime(&uptime);
|
|
|
|
CommandMapIter iter;
|
|
|
|
for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
|
|
|
|
if ((iter->second.uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
|
2022-07-26 13:59:09 +02:00
|
|
|
prepareVerificationFailureWithFullInfo(tcverif::COMPLETION_FAILURE, iter->second.tcInfo,
|
|
|
|
TIMEOUT, false);
|
|
|
|
verificationReporter->sendFailureReport(failParams);
|
2022-02-02 10:29:30 +01:00
|
|
|
checkAndExecuteFifo(iter);
|
2021-12-03 15:37:49 +01:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2019-08-28 14:50:24 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void CommandingServiceBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
|
2022-07-20 22:21:15 +02:00
|
|
|
|
2022-07-25 11:24:13 +02:00
|
|
|
void CommandingServiceBase::setCustomTmStore(StorageManagerIF& store) {
|
2022-07-20 22:21:15 +02:00
|
|
|
tmStoreHelper.setTmStore(store);
|
|
|
|
}
|
2022-07-29 10:23:59 +02:00
|
|
|
|
2022-07-21 13:48:58 +02:00
|
|
|
ReturnValue_t CommandingServiceBase::setUpTcReader(store_address_t storeId) {
|
2022-07-26 17:41:10 +02:00
|
|
|
return tc::prepareTcReader(*tcStore, storeId, tcReader);
|
2022-07-21 13:48:58 +02:00
|
|
|
}
|
2022-07-26 13:59:09 +02:00
|
|
|
|
2022-07-29 10:23:59 +02:00
|
|
|
void CommandingServiceBase::prepareVerificationFailureWithNoTcInfo(uint8_t reportId,
|
|
|
|
ReturnValue_t errorCode,
|
|
|
|
bool setCachedFailParams) {
|
|
|
|
failParams.resetTcFields();
|
|
|
|
failParams.resetFailParams();
|
|
|
|
failParams.reportId = reportId;
|
|
|
|
failParams.errorCode = errorCode;
|
|
|
|
if (setCachedFailParams) {
|
|
|
|
failParams.errorParam1 = failureParameter1;
|
|
|
|
failParams.errorParam2 = failureParameter2;
|
|
|
|
}
|
|
|
|
}
|
2022-07-26 13:59:09 +02:00
|
|
|
void CommandingServiceBase::prepareVerificationFailureWithFullInfo(uint8_t reportId,
|
|
|
|
CommandInfo::TcInfo& tcInfo,
|
|
|
|
ReturnValue_t errorCode,
|
|
|
|
bool setCachedFailParams) {
|
|
|
|
failParams.reportId = reportId;
|
|
|
|
failParams.tcPacketId = tcInfo.tcPacketId;
|
|
|
|
failParams.tcPsc = tcInfo.tcSequenceControl;
|
|
|
|
failParams.ackFlags = tcInfo.ackFlags;
|
2022-07-29 10:23:59 +02:00
|
|
|
failParams.resetFailParams();
|
2022-07-26 13:59:09 +02:00
|
|
|
failParams.errorCode = errorCode;
|
|
|
|
if (setCachedFailParams) {
|
|
|
|
failParams.errorParam1 = failureParameter1;
|
|
|
|
failParams.errorParam2 = failureParameter2;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void CommandingServiceBase::prepareVerificationSuccessWithFullInfo(
|
|
|
|
uint8_t reportId, CommandingServiceBase::CommandInfo::TcInfo& tcInfo) {
|
2022-07-27 20:10:15 +02:00
|
|
|
successParams.reportId = reportId;
|
|
|
|
successParams.tcPacketId = tcInfo.tcPacketId;
|
|
|
|
successParams.tcPsc = tcInfo.tcSequenceControl;
|
|
|
|
successParams.ackFlags = tcInfo.ackFlags;
|
2022-07-26 13:59:09 +02:00
|
|
|
}
|