114 lines
3.6 KiB
C++
114 lines
3.6 KiB
C++
#include "CCSDSDistributorIF.h"
|
|
#include "PUSDistributor.h"
|
|
|
|
#include "../serviceinterface/ServiceInterfaceStream.h"
|
|
#include "../tmtcpacket/pus/TcPacketStored.h"
|
|
#include "../tmtcservices/PusVerificationReport.h"
|
|
|
|
PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
|
|
object_id_t setPacketSource) :
|
|
TcDistributor(setObjectId), checker(setApid), verifyChannel(),
|
|
tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
|
|
|
|
PUSDistributor::~PUSDistributor() {}
|
|
|
|
PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
|
|
// sif:: debug << "PUSDistributor::handlePacket received: "
|
|
// << this->current_packet_id.store_index << ", "
|
|
// << this->current_packet_id.packet_index << std::endl;
|
|
TcMqMapIter queueMapIt = this->queueMap.end();
|
|
if(this->currentPacket == nullptr) {
|
|
return queueMapIt;
|
|
}
|
|
this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
|
|
if (currentPacket->getWholeData() != nullptr) {
|
|
tcStatus = checker.checkPacket(currentPacket);
|
|
#ifdef DEBUG
|
|
if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
|
|
sif::debug << "PUSDistributor::handlePacket: Packet format "
|
|
<< "invalid, code "<< static_cast<int>(tcStatus)
|
|
<< std::endl;
|
|
}
|
|
#endif
|
|
uint32_t queue_id = currentPacket->getService();
|
|
queueMapIt = this->queueMap.find(queue_id);
|
|
}
|
|
else {
|
|
tcStatus = PACKET_LOST;
|
|
}
|
|
|
|
if (queueMapIt == this->queueMap.end()) {
|
|
tcStatus = DESTINATION_NOT_FOUND;
|
|
#ifdef DEBUG
|
|
sif::debug << "PUSDistributor::handlePacket: Destination not found, "
|
|
<< "code "<< static_cast<int>(tcStatus) << std::endl;
|
|
#endif
|
|
}
|
|
|
|
if (tcStatus != RETURN_OK) {
|
|
return this->queueMap.end();
|
|
}
|
|
else {
|
|
return queueMapIt;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
|
|
uint16_t serviceId = service->getIdentifier();
|
|
// sif::info << "Service ID: " << (int)serviceId << std::endl;
|
|
MessageQueueId_t queue = service->getRequestQueue();
|
|
auto returnPair = queueMap.emplace(serviceId, queue);
|
|
if (not returnPair.second) {
|
|
sif::error << "PUSDistributor::registerService: Service ID already"
|
|
" exists in map." << std::endl;
|
|
return SERVICE_ID_ALREADY_EXISTS;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
MessageQueueId_t PUSDistributor::getRequestQueue() {
|
|
return tcQueue->getId();
|
|
}
|
|
|
|
ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
|
|
if (queueStatus != RETURN_OK) {
|
|
tcStatus = queueStatus;
|
|
}
|
|
if (tcStatus != RETURN_OK) {
|
|
this->verifyChannel.sendFailureReport(TC_VERIFY::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_VERIFY::ACCEPTANCE_SUCCESS,
|
|
currentPacket);
|
|
return RETURN_OK;
|
|
}
|
|
}
|
|
|
|
uint16_t PUSDistributor::getIdentifier() {
|
|
return checker.getApid();
|
|
}
|
|
|
|
ReturnValue_t PUSDistributor::initialize() {
|
|
currentPacket = new TcPacketStored();
|
|
if(currentPacket == nullptr) {
|
|
// Should not happen, memory allocation failed!
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
|
|
CCSDSDistributorIF* ccsdsDistributor =
|
|
objectManager->get<CCSDSDistributorIF>(packetSource);
|
|
if (ccsdsDistributor == nullptr) {
|
|
sif::error << "PUSDistributor::initialize: Packet source invalid."
|
|
<< " Make sure it exists and implements CCSDSDistributorIF!"
|
|
<< std::endl;
|
|
return RETURN_FAILED;
|
|
}
|
|
return ccsdsDistributor->registerApplication(this);
|
|
}
|