fsfw/pus/Service1TelecommandVerification.cpp
2021-06-14 10:19:01 +02:00

116 lines
3.9 KiB
C++

#include "Service1TelecommandVerification.h"
#include "servicepackets/Service1Packets.h"
#include "../ipc/QueueFactory.h"
#include "../objectmanager/ObjectManager.h"
#include "../tmtcservices/PusVerificationReport.h"
#include "../tmtcpacket/pus/tm/TmPacketStored.h"
#include "../serviceinterface/ServiceInterfaceStream.h"
#include "../tmtcservices/AcceptsTelemetryIF.h"
Service1TelecommandVerification::Service1TelecommandVerification(
object_id_t objectId, uint16_t apid, uint8_t serviceId,
object_id_t targetDestination, uint16_t messageQueueDepth):
SystemObject(objectId), apid(apid), serviceId(serviceId),
targetDestination(targetDestination) {
tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
}
Service1TelecommandVerification::~Service1TelecommandVerification() {
QueueFactory::instance()->deleteMessageQueue(tmQueue);
}
MessageQueueId_t Service1TelecommandVerification::getVerificationQueue(){
return tmQueue->getId();
}
ReturnValue_t Service1TelecommandVerification::performOperation(
uint8_t operationCode){
PusVerificationMessage message;
ReturnValue_t status = tmQueue->receiveMessage(&message);
while(status == HasReturnvaluesIF::RETURN_OK) {
status = sendVerificationReport(&message);
if(status != HasReturnvaluesIF::RETURN_OK) {
return status;
}
status = tmQueue->receiveMessage(&message);
}
if (status == MessageQueueIF::EMPTY) {
return HasReturnvaluesIF::RETURN_OK;
}
else {
return status;
}
}
ReturnValue_t Service1TelecommandVerification::sendVerificationReport(
PusVerificationMessage* message) {
ReturnValue_t result;
if(message->getReportId() % 2 == 0) {
result = generateFailureReport(message);
} else {
result = generateSuccessReport(message);
}
if(result != HasReturnvaluesIF::RETURN_OK){
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Service1TelecommandVerification::sendVerificationReport: "
"Sending verification packet failed !" << std::endl;
#endif
}
return result;
}
ReturnValue_t Service1TelecommandVerification::generateFailureReport(
PusVerificationMessage *message) {
FailureReport report(
message->getReportId(), message->getTcPacketId(),
message->getTcSequenceControl(), message->getStep(),
message->getErrorCode(), message->getParameter1(),
message->getParameter2());
#if FSFW_USE_PUS_C_TELEMETRY == 0
TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
packetSubCounter++, &report);
#else
TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
packetSubCounter++, &report);
#endif
ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
tmQueue->getId());
return result;
}
ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
PusVerificationMessage *message) {
SuccessReport report(message->getReportId(),message->getTcPacketId(),
message->getTcSequenceControl(),message->getStep());
#if FSFW_USE_PUS_C_TELEMETRY == 0
TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(),
packetSubCounter++, &report);
#else
TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(),
packetSubCounter++, &report);
#endif
ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(),
tmQueue->getId());
return result;
}
ReturnValue_t Service1TelecommandVerification::initialize() {
// Get target object for TC verification messages
AcceptsTelemetryIF* funnel = ObjectManager::instance()->
get<AcceptsTelemetryIF>(targetDestination);
if(funnel == nullptr){
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::error << "Service1TelecommandVerification::initialize: Specified"
" TM funnel invalid. Make sure it is set up and implements"
" AcceptsTelemetryIF." << std::endl;
#endif
return ObjectManagerIF::CHILD_INIT_FAILED;
}
tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
return SystemObject::initialize();
}