fsfw/tcdistribution/CCSDSDistributor.cpp

112 lines
3.4 KiB
C++
Raw Normal View History

2020-08-13 20:53:35 +02:00
#include "CCSDSDistributor.h"
2020-10-01 13:23:06 +02:00
2021-04-12 12:50:23 +02:00
#include "../serviceinterface/ServiceInterface.h"
2020-08-13 20:53:35 +02:00
#include "../tmtcpacket/SpacePacketBase.h"
2021-04-12 12:50:23 +02:00
#define CCSDS_DISTRIBUTOR_DEBUGGING 0
2020-10-01 13:23:06 +02:00
CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
object_id_t setObjectId):
TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
}
2020-10-01 13:23:06 +02:00
CCSDSDistributor::~CCSDSDistributor() {}
2020-10-01 13:23:06 +02:00
TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
2021-04-12 12:50:23 +02:00
#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2021-04-12 12:50:23 +02:00
sif::debug << "CCSDSDistributor::selectDestination received: " <<
this->currentMessage.getStorageId().poolIndex << ", " <<
this->currentMessage.getStorageId().packetIndex << std::endl;
#else
sif::printDebug("CCSDSDistributor::selectDestination received: %d, %d\n",
currentMessage.getStorageId().poolIndex, currentMessage.getStorageId().packetIndex);
#endif
#endif
2020-10-01 13:23:06 +02:00
const uint8_t* packet = nullptr;
2020-05-04 16:53:04 +02:00
size_t size = 0;
2020-10-01 13:23:06 +02:00
ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
&packet, &size );
if(result != HasReturnvaluesIF::RETURN_OK) {
2021-04-12 12:50:23 +02:00
#if FSFW_VERBOSE_LEVEL >= 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2020-10-01 13:23:06 +02:00
sif::error << "CCSDSDistributor::selectDestination: Getting data from"
" store failed!" << std::endl;
2021-04-12 12:50:23 +02:00
#else
sif::printError("CCSDSDistributor::selectDestination: Getting data from"
" store failed!\n");
#endif
#endif
2020-10-01 13:23:06 +02:00
}
SpacePacketBase currentPacket(packet);
2021-04-12 12:50:23 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex <<
currentPacket.getAPID() << std::dec << std::endl;
#endif
2020-10-01 13:23:06 +02:00
TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
if ( position != this->queueMap.end() ) {
return position;
} else {
2020-10-01 13:23:06 +02:00
//The APID was not found. Forward packet to main SW-APID anyway to
// create acceptance failure report.
return this->queueMap.find( this->defaultApid );
}
}
MessageQueueId_t CCSDSDistributor::getRequestQueue() {
return tcQueue->getId();
}
ReturnValue_t CCSDSDistributor::registerApplication(
AcceptsTelecommandsIF* application) {
ReturnValue_t returnValue = RETURN_OK;
2020-10-01 13:23:06 +02:00
auto insertPair = this->queueMap.emplace(application->getIdentifier(),
application->getRequestQueue());
if(not insertPair.second) {
returnValue = RETURN_FAILED;
}
return returnValue;
}
ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid,
MessageQueueId_t id) {
ReturnValue_t returnValue = RETURN_OK;
2020-10-01 13:23:06 +02:00
auto insertPair = this->queueMap.emplace(apid, id);
if(not insertPair.second) {
returnValue = RETURN_FAILED;
}
return returnValue;
}
uint16_t CCSDSDistributor::getIdentifier() {
return 0;
}
ReturnValue_t CCSDSDistributor::initialize() {
ReturnValue_t status = this->TcDistributor::initialize();
this->tcStore = objectManager->get<StorageManagerIF>( objects::TC_STORE );
2020-10-01 13:23:06 +02:00
if (this->tcStore == nullptr) {
2021-04-12 12:50:23 +02:00
#if FSFW_VERBOSE_LEVEL >= 1
2021-01-03 14:16:52 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2020-10-01 13:23:06 +02:00
sif::error << "CCSDSDistributor::initialize: Could not initialize"
" TC store!" << std::endl;
2021-04-12 12:50:23 +02:00
#else
sif::printError("CCSDSDistributor::initialize: Could not initialize"
" TC store!\n");
#endif
#endif
2020-10-01 13:23:06 +02:00
status = RETURN_FAILED;
}
return status;
}
ReturnValue_t CCSDSDistributor::callbackAfterSending(
ReturnValue_t queueStatus) {
if (queueStatus != RETURN_OK) {
tcStore->deleteData(currentMessage.getStorageId());
}
return RETURN_OK;
}