2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tcdistribution/PUSDistributor.h"
|
2020-10-01 13:23:06 +02:00
|
|
|
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/tcdistribution/CCSDSDistributorIF.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcservices/PusVerificationReport.h"
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-02-02 10:29:30 +01: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,
|
2022-02-02 10:29:30 +01:00
|
|
|
object_id_t setPacketSource)
|
|
|
|
: TcDistributor(setObjectId),
|
|
|
|
checker(setApid),
|
|
|
|
verifyChannel(),
|
2022-08-15 20:28:16 +02:00
|
|
|
tcStatus(returnvalue::FAILED),
|
2022-02-02 10:29:30 +01:00
|
|
|
packetSource(setPacketSource) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-05-20 11:20:31 +02:00
|
|
|
PUSDistributor::~PUSDistributor() = default;
|
2016-06-15 23:48:41 +02:00
|
|
|
|
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());
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
|
|
|
|
<< storeId.packetIndex << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-05-20 11:20:31 +02:00
|
|
|
auto queueMapIt = this->queueMap.end();
|
2022-02-02 10:29:30 +01:00
|
|
|
if (this->currentPacket == nullptr) {
|
|
|
|
return queueMapIt;
|
2020-10-03 14:19:42 +02:00
|
|
|
}
|
2021-12-03 15:37:49 +01:00
|
|
|
this->currentPacket->setStoreAddress(this->currentMessage.getStorageId(), currentPacket);
|
2020-10-03 14:19:42 +02:00
|
|
|
if (currentPacket->getWholeData() != nullptr) {
|
2022-02-02 10:29:30 +01:00
|
|
|
tcStatus = checker.checkPacket(currentPacket);
|
2022-08-15 20:28:16 +02:00
|
|
|
if (tcStatus != returnvalue::OK) {
|
2021-04-07 12:03:28 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2022-02-02 10:29:30 +01:00
|
|
|
const char* keyword = "unnamed error";
|
|
|
|
if (tcStatus == TcPacketCheckPUS::INCORRECT_CHECKSUM) {
|
|
|
|
keyword = "checksum";
|
|
|
|
} else if (tcStatus == TcPacketCheckPUS::INCORRECT_PRIMARY_HEADER) {
|
|
|
|
keyword = "incorrect primary header";
|
|
|
|
} else if (tcStatus == TcPacketCheckPUS::ILLEGAL_APID) {
|
|
|
|
keyword = "illegal APID";
|
|
|
|
} else if (tcStatus == TcPacketCheckPUS::INCORRECT_SECONDARY_HEADER) {
|
|
|
|
keyword = "incorrect secondary header";
|
|
|
|
} else if (tcStatus == TcPacketCheckPUS::INCOMPLETE_PACKET) {
|
|
|
|
keyword = "incomplete packet";
|
|
|
|
}
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword
|
|
|
|
<< " error" << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-05-24 10:40:25 +02:00
|
|
|
sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n",
|
|
|
|
keyword);
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2020-10-01 13:23:06 +02:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01: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
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2020-10-01 13:23:06 +02:00
|
|
|
if (queueMapIt == this->queueMap.end()) {
|
2022-02-02 10:29:30 +01:00
|
|
|
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
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
|
2020-10-01 13:23:06 +02:00
|
|
|
#endif
|
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-08-15 20:28:16 +02:00
|
|
|
if (tcStatus != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return this->queueMap.end();
|
|
|
|
} else {
|
|
|
|
return queueMapIt;
|
2020-10-01 13:23:06 +02:00
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
|
2022-02-02 10:29:30 +01:00
|
|
|
uint16_t serviceId = service->getIdentifier();
|
2021-04-07 12:03:28 +02:00
|
|
|
#if PUS_DISTRIBUTOR_DEBUGGING == 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t queue = service->getRequestQueue();
|
|
|
|
auto returnPair = queueMap.emplace(serviceId, queue);
|
|
|
|
if (not returnPair.second) {
|
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
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "PUSDistributor::registerService: Service ID already"
|
|
|
|
" exists in map"
|
|
|
|
<< std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return SERVICE_ID_ALREADY_EXISTS;
|
|
|
|
}
|
2022-08-15 20:28:16 +02:00
|
|
|
return returnvalue::OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t PUSDistributor::getRequestQueue() { return tcQueue->getId(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
|
|
|
ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
|
2022-08-15 20:28:16 +02:00
|
|
|
if (queueStatus != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
tcStatus = queueStatus;
|
|
|
|
}
|
2022-08-15 20:28:16 +02:00
|
|
|
if (tcStatus != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
this->verifyChannel.sendFailureReport(tc_verification::ACCEPTANCE_FAILURE, currentPacket,
|
|
|
|
tcStatus);
|
|
|
|
// A failed packet is deleted immediately after reporting,
|
|
|
|
// otherwise it will block memory.
|
|
|
|
currentPacket->deletePacket();
|
2022-08-15 20:28:16 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
|
|
|
this->verifyChannel.sendSuccessReport(tc_verification::ACCEPTANCE_SUCCESS, currentPacket);
|
2022-08-15 20:28:16 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
uint16_t PUSDistributor::getIdentifier() { return checker.getApid(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
|
|
|
ReturnValue_t PUSDistributor::initialize() {
|
2022-02-02 10:29:30 +01:00
|
|
|
currentPacket = new TcPacketStoredPus();
|
|
|
|
if (currentPacket == nullptr) {
|
|
|
|
// Should not happen, memory allocation failed!
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2020-10-20 15:04:28 +02:00
|
|
|
|
2022-05-24 10:40:25 +02:00
|
|
|
auto* ccsdsDistributor = ObjectManager::instance()->get<CCSDSDistributorIF>(packetSource);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (ccsdsDistributor == 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 << "PUSDistributor::initialize: Packet source invalid" << std::endl;
|
|
|
|
sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("PUSDistributor::initialize: Packet source invalid\n");
|
|
|
|
sif::printError("Make sure it exists and implements CCSDSDistributorIF\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-15 20:28:16 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return ccsdsDistributor->registerApplication(this);
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|