fsfw/tcdistribution/CCSDSDistributor.cpp

#include "CCSDSDistributor.h"

#include "../objectmanager/ObjectManager.h"
#include "../serviceinterface/ServiceInterface.h"
#include "../tmtcpacket/SpacePacketBase.h"

#define CCSDS_DISTRIBUTOR_DEBUGGING     0

CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid,
        object_id_t setObjectId):
        TcDistributor(setObjectId), defaultApid( setDefaultApid ) {
}

CCSDSDistributor::~CCSDSDistributor() {}

TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
#if CCSDS_DISTRIBUTOR_DEBUGGING == 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
    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
    const uint8_t* packet = nullptr;
    size_t size = 0;
    ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(),
            &packet, &size );
    if(result != HasReturnvaluesIF::RETURN_OK) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "CCSDSDistributor::selectDestination: Getting data from"
                " store failed!" << std::endl;
#else
        sif::printError("CCSDSDistributor::selectDestination: Getting data from"
                " store failed!\n");
#endif
#endif
    }
    SpacePacketBase currentPacket(packet);

#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
    TcMqMapIter position = this->queueMap.find(currentPacket.getAPID());
    if ( position != this->queueMap.end() ) {
        return position;
    } else {
        //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;
    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;
    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::instance()->get<StorageManagerIF>( objects::TC_STORE );
    if (this->tcStore == nullptr) {
#if FSFW_VERBOSE_LEVEL >= 1
#if FSFW_CPP_OSTREAM_ENABLED == 1
        sif::error << "CCSDSDistributor::initialize: Could not initialize"
                " TC store!" << std::endl;
#else
        sif::printError("CCSDSDistributor::initialize: Could not initialize"
                " TC store!\n");
#endif
#endif
        status = RETURN_FAILED;
    }
    return status;
}

ReturnValue_t CCSDSDistributor::callbackAfterSending(
        ReturnValue_t queueStatus) {
    if (queueStatus != RETURN_OK) {
        tcStore->deleteData(currentMessage.getStorageId());
    }
    return RETURN_OK;
}