fsfw/tcdistribution/PUSDistributor.cpp

141 lines
4.8 KiB
C++
Raw Normal View History

2020-08-13 20:53:35 +02:00
#include "CCSDSDistributorIF.h"
#include "PUSDistributor.h"
2020-10-01 13:23:06 +02:00
2021-04-07 12:03:28 +02:00
#include "../serviceinterface/ServiceInterface.h"
2020-08-13 20:53:35 +02:00
#include "../tmtcpacket/pus/TcPacketStored.h"
#include "../tmtcservices/PusVerificationReport.h"
2021-04-07 13:44:03 +02:00
#define PUS_DISTRIBUTOR_DEBUGGING 0
2021-04-07 12:03:28 +02:00
2020-10-01 13:23:06 +02:00
PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
2021-04-07 12:03:28 +02:00
object_id_t setPacketSource) :
TcDistributor(setObjectId), checker(setApid), verifyChannel(),
tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
2020-10-01 13:23:06 +02:00
PUSDistributor::~PUSDistributor() {}
2020-10-01 13:23:06 +02:00
PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
2021-04-07 12:03:28 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
store_address_t storeId = this->currentMessage.getStorageId());
sif:: debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", " <<
storeId.packetIndex << std::endl;
#endif
2020-10-01 13:23:06 +02:00
TcMqMapIter queueMapIt = this->queueMap.end();
2020-10-03 14:19:42 +02:00
if(this->currentPacket == nullptr) {
return queueMapIt;
}
2020-10-01 13:23:06 +02:00
this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
2020-10-03 14:19:42 +02:00
if (currentPacket->getWholeData() != nullptr) {
2020-10-01 13:23:06 +02:00
tcStatus = checker.checkPacket(currentPacket);
if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
2021-04-07 12:03:28 +02:00
#if FSFW_VERBOSE_LEVEL >= 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-07 12:03:28 +02:00
sif::debug << "PUSDistributor::handlePacket: Packet format invalid, code " <<
static_cast<int>(tcStatus) << std::endl;
#else
sif::printDebug("PUSDistributor::handlePacket: Packet format invalid, code %d\n",
static_cast<int>(tcStatus));
#endif
2020-10-01 13:23:06 +02:00
#endif
2021-04-07 12:03:28 +02:00
}
2020-10-01 13:23:06 +02:00
uint32_t queue_id = currentPacket->getService();
queueMapIt = this->queueMap.find(queue_id);
}
else {
tcStatus = PACKET_LOST;
}
2020-10-01 13:23:06 +02:00
if (queueMapIt == this->queueMap.end()) {
tcStatus = DESTINATION_NOT_FOUND;
2021-04-07 12:03:28 +02:00
#if FSFW_VERBOSE_LEVEL >= 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-07 12:03:28 +02:00
sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
#else
sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
2020-10-01 13:23:06 +02:00
#endif
}
2020-10-01 13:23:06 +02:00
if (tcStatus != RETURN_OK) {
return this->queueMap.end();
}
else {
return queueMapIt;
}
}
ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
2021-04-07 12:03:28 +02:00
uint16_t serviceId = service->getIdentifier();
#if PUS_DISTRIBUTOR_DEBUGGING == 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-07 12:03:28 +02:00
sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
#else
sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
#endif
#endif
2021-04-07 12:03:28 +02:00
MessageQueueId_t queue = service->getRequestQueue();
auto returnPair = queueMap.emplace(serviceId, queue);
if (not returnPair.second) {
#if FSFW_VERBOSE_LEVEL >= 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-07 12:03:28 +02:00
sif::error << "PUSDistributor::registerService: Service ID already"
" exists in map" << std::endl;
#else
sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
#endif
2021-04-07 12:03:28 +02:00
#endif
return SERVICE_ID_ALREADY_EXISTS;
}
return HasReturnvaluesIF::RETURN_OK;
}
MessageQueueId_t PUSDistributor::getRequestQueue() {
2021-04-07 12:03:28 +02:00
return tcQueue->getId();
}
ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
2021-04-07 12:03:28 +02:00
if (queueStatus != RETURN_OK) {
tcStatus = queueStatus;
}
if (tcStatus != RETURN_OK) {
this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE,
currentPacket, tcStatus);
// A failed packet is deleted immediately after reporting,
// otherwise it will block memory.
currentPacket->deletePacket();
return RETURN_FAILED;
} else {
this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS,
currentPacket);
return RETURN_OK;
}
}
uint16_t PUSDistributor::getIdentifier() {
2021-04-07 12:03:28 +02:00
return checker.getApid();
}
ReturnValue_t PUSDistributor::initialize() {
2020-10-20 15:04:28 +02:00
currentPacket = new TcPacketStored();
if(currentPacket == nullptr) {
// Should not happen, memory allocation failed!
return ObjectManagerIF::CHILD_INIT_FAILED;
}
2021-04-07 12:03:28 +02:00
CCSDSDistributorIF* ccsdsDistributor =
objectManager->get<CCSDSDistributorIF>(packetSource);
if (ccsdsDistributor == nullptr) {
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-07 12:03:28 +02:00
sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
#else
sif::printError("PUSDistributor::initialize: Packet source invalid\n");
sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
#endif
2021-04-07 12:03:28 +02:00
return RETURN_FAILED;
}
return ccsdsDistributor->registerApplication(this);
}