2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/pus/Service1TelecommandVerification.h"
|
2020-07-10 14:16:55 +02:00
|
|
|
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/pus/servicepackets/Service1Packets.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/tmtcservices/PusVerificationReport.h"
|
2022-07-21 13:48:58 +02:00
|
|
|
#include "fsfw/tmtcservices/tmHelpers.h"
|
2020-07-10 14:16:55 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
Service1TelecommandVerification::Service1TelecommandVerification(object_id_t objectId,
|
|
|
|
uint16_t apid, uint8_t serviceId,
|
|
|
|
object_id_t targetDestination,
|
2022-07-27 17:48:59 +02:00
|
|
|
uint16_t messageQueueDepth,
|
2022-09-05 14:44:35 +02:00
|
|
|
TimeWriterIF* timeStamper)
|
2022-02-02 10:29:30 +01:00
|
|
|
: SystemObject(objectId),
|
|
|
|
apid(apid),
|
|
|
|
serviceId(serviceId),
|
2022-07-20 22:21:15 +02:00
|
|
|
targetDestination(targetDestination),
|
2022-07-26 11:09:57 +02:00
|
|
|
storeHelper(apid),
|
|
|
|
tmHelper(serviceId, storeHelper, sendHelper) {
|
2022-02-02 10:29:30 +01:00
|
|
|
tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
2021-04-09 09:14:42 +02:00
|
|
|
Service1TelecommandVerification::~Service1TelecommandVerification() {
|
2022-02-02 10:29:30 +01:00
|
|
|
QueueFactory::instance()->deleteMessageQueue(tmQueue);
|
2021-04-09 09:14:42 +02:00
|
|
|
}
|
2020-07-10 14:16:55 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t Service1TelecommandVerification::getVerificationQueue() {
|
|
|
|
return tmQueue->getId();
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t Service1TelecommandVerification::performOperation(uint8_t operationCode) {
|
|
|
|
PusVerificationMessage message;
|
|
|
|
ReturnValue_t status = tmQueue->receiveMessage(&message);
|
2022-08-16 01:08:26 +02:00
|
|
|
while (status == returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
status = sendVerificationReport(&message);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (status != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return status;
|
|
|
|
}
|
|
|
|
status = tmQueue->receiveMessage(&message);
|
|
|
|
}
|
|
|
|
if (status == MessageQueueIF::EMPTY) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
return status;
|
|
|
|
}
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service1TelecommandVerification::sendVerificationReport(
|
2022-02-02 10:29:30 +01:00
|
|
|
PusVerificationMessage* message) {
|
|
|
|
ReturnValue_t result;
|
2022-07-27 20:10:15 +02:00
|
|
|
uint8_t reportId = message->getReportId();
|
2022-07-27 21:06:23 +02:00
|
|
|
if (reportId == 0 or reportId > 8) {
|
2022-07-27 20:10:15 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-07-27 21:06:23 +02:00
|
|
|
sif::error << "Service1TelecommandVerification::sendVerificationReport: Invalid report ID "
|
|
|
|
<< static_cast<int>(reportId) << " detected" << std::endl;
|
2022-07-27 20:10:15 +02:00
|
|
|
#else
|
2022-07-27 21:06:23 +02:00
|
|
|
sif::printError(
|
|
|
|
"Service1TelecommandVerification::sendVerificationReport: Invalid report ID "
|
|
|
|
"%d detected\n",
|
|
|
|
reportId);
|
2022-07-27 20:10:15 +02:00
|
|
|
#endif
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-07-27 20:10:15 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
if (message->getReportId() % 2 == 0) {
|
|
|
|
result = generateFailureReport(message);
|
|
|
|
} else {
|
|
|
|
result = generateSuccessReport(message);
|
|
|
|
}
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "Service1TelecommandVerification::sendVerificationReport: "
|
|
|
|
"Sending verification packet failed !"
|
|
|
|
<< std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service1TelecommandVerification::generateFailureReport(
|
2022-02-02 10:29:30 +01:00
|
|
|
PusVerificationMessage* message) {
|
|
|
|
FailureReport report(message->getReportId(), message->getTcPacketId(),
|
|
|
|
message->getTcSequenceControl(), message->getStep(), message->getErrorCode(),
|
|
|
|
message->getParameter1(), message->getParameter2());
|
2022-07-27 17:00:43 +02:00
|
|
|
ReturnValue_t result =
|
|
|
|
storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
result = storeHelper.setSourceDataSerializable(report);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
return result;
|
|
|
|
}
|
2022-07-26 11:09:57 +02:00
|
|
|
return tmHelper.storeAndSendTmPacket();
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
|
2022-02-02 10:29:30 +01:00
|
|
|
PusVerificationMessage* message) {
|
|
|
|
SuccessReport report(message->getReportId(), message->getTcPacketId(),
|
|
|
|
message->getTcSequenceControl(), message->getStep());
|
2022-07-27 17:00:43 +02:00
|
|
|
ReturnValue_t result =
|
|
|
|
storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
return result;
|
|
|
|
}
|
|
|
|
result = storeHelper.setSourceDataSerializable(report);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
return result;
|
|
|
|
}
|
2022-07-26 11:09:57 +02:00
|
|
|
return tmHelper.storeAndSendTmPacket();
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t Service1TelecommandVerification::initialize() {
|
2022-02-02 10:29:30 +01:00
|
|
|
// Get target object for TC verification messages
|
2022-07-20 22:21:15 +02:00
|
|
|
auto* funnel = ObjectManager::instance()->get<AcceptsTelemetryIF>(targetDestination);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (funnel == 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 << "Service1TelecommandVerification::initialize: Specified"
|
|
|
|
" TM funnel invalid. Make sure it is set up and implements"
|
|
|
|
" AcceptsTelemetryIF."
|
|
|
|
<< 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-27 17:00:43 +02:00
|
|
|
if (tmQueue == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
|
2022-07-20 22:21:15 +02:00
|
|
|
if (tmStore == nullptr) {
|
|
|
|
tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
|
|
|
|
if (tmStore == nullptr) {
|
|
|
|
return ObjectManager::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-07-25 11:24:13 +02:00
|
|
|
storeHelper.setTmStore(*tmStore);
|
2022-07-20 22:21:15 +02:00
|
|
|
}
|
2022-07-27 18:03:17 +02:00
|
|
|
if (timeStamper == nullptr) {
|
2022-09-05 14:44:35 +02:00
|
|
|
timeStamper = ObjectManager::instance()->get<TimeWriterIF>(objects::TIME_STAMPER);
|
2022-07-27 18:03:17 +02:00
|
|
|
if (timeStamper == nullptr) {
|
2022-07-27 17:48:59 +02:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
|
|
|
} else {
|
|
|
|
}
|
|
|
|
storeHelper.setTimeStamper(*timeStamper);
|
2022-07-27 17:00:43 +02:00
|
|
|
|
|
|
|
sendHelper.setMsgQueue(*tmQueue);
|
|
|
|
if (errReporter == nullptr) {
|
|
|
|
errReporter =
|
|
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
|
|
if (errReporter != nullptr) {
|
|
|
|
sendHelper.setInternalErrorReporter(*errReporter);
|
|
|
|
}
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
return SystemObject::initialize();
|
2020-07-10 14:16:55 +02:00
|
|
|
}
|