Merge pull request 'Updating TC Distributor modules' (#403) from mueller/tmtcservices-update into development
Reviewed-on: fsfw/fsfw#403
This commit is contained in:
commit
69788e1279
@ -1,8 +1,10 @@
|
||||
#include "CCSDSDistributor.h"
|
||||
|
||||
#include "../serviceinterface/ServiceInterfaceStream.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 ) {
|
||||
@ -11,26 +13,36 @@ CCSDSDistributor::CCSDSDistributor(uint16_t 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().pool_index << ", " <<
|
||||
// this->currentMessage.getStorageId().packet_index << std::endl;
|
||||
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
|
||||
// sif:: info << "CCSDSDistributor::selectDestination has packet with APID "
|
||||
// << std::hex << currentPacket.getAPID() << std::dec << std::endl;
|
||||
#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() ) {
|
||||
@ -76,9 +88,14 @@ ReturnValue_t CCSDSDistributor::initialize() {
|
||||
ReturnValue_t status = this->TcDistributor::initialize();
|
||||
this->tcStore = objectManager->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;
|
||||
}
|
||||
|
||||
@ -1,22 +1,24 @@
|
||||
#include "CCSDSDistributorIF.h"
|
||||
#include "PUSDistributor.h"
|
||||
|
||||
#include "../serviceinterface/ServiceInterfaceStream.h"
|
||||
#include "../serviceinterface/ServiceInterface.h"
|
||||
#include "../tmtcpacket/pus/TcPacketStored.h"
|
||||
#include "../tmtcservices/PusVerificationReport.h"
|
||||
|
||||
#define PUS_DISTRIBUTOR_DEBUGGING 0
|
||||
|
||||
PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId,
|
||||
object_id_t setPacketSource) :
|
||||
TcDistributor(setObjectId), checker(setApid), verifyChannel(),
|
||||
tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
|
||||
object_id_t setPacketSource) :
|
||||
TcDistributor(setObjectId), checker(setApid), verifyChannel(),
|
||||
tcStatus(RETURN_FAILED), packetSource(setPacketSource) {}
|
||||
|
||||
PUSDistributor::~PUSDistributor() {}
|
||||
|
||||
PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
// sif:: debug << "PUSDistributor::handlePacket received: "
|
||||
// << this->current_packet_id.store_index << ", "
|
||||
// << this->current_packet_id.packet_index << std::endl;
|
||||
#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
|
||||
TcMqMapIter queueMapIt = this->queueMap.end();
|
||||
if(this->currentPacket == nullptr) {
|
||||
@ -25,15 +27,17 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
|
||||
this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
|
||||
if (currentPacket->getWholeData() != nullptr) {
|
||||
tcStatus = checker.checkPacket(currentPacket);
|
||||
#ifdef DEBUG
|
||||
if(tcStatus != HasReturnvaluesIF::RETURN_OK) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::debug << "PUSDistributor::handlePacket: Packet format "
|
||||
<< "invalid, code "<< static_cast<int>(tcStatus)
|
||||
<< std::endl;
|
||||
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
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
uint32_t queue_id = currentPacket->getService();
|
||||
queueMapIt = this->queueMap.find(queue_id);
|
||||
}
|
||||
@ -43,11 +47,12 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
|
||||
|
||||
if (queueMapIt == this->queueMap.end()) {
|
||||
tcStatus = DESTINATION_NOT_FOUND;
|
||||
#ifdef DEBUG
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::debug << "PUSDistributor::handlePacket: Destination not found, "
|
||||
<< "code "<< static_cast<int>(tcStatus) << std::endl;
|
||||
#endif
|
||||
sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
|
||||
#else
|
||||
sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
|
||||
#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
|
||||
#endif
|
||||
}
|
||||
|
||||
@ -62,46 +67,54 @@ PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() {
|
||||
|
||||
|
||||
ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) {
|
||||
uint16_t serviceId = service->getIdentifier();
|
||||
uint16_t serviceId = service->getIdentifier();
|
||||
#if PUS_DISTRIBUTOR_DEBUGGING == 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
// sif::info << "Service ID: " << (int)serviceId << std::endl;
|
||||
sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
|
||||
#else
|
||||
sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
|
||||
#endif
|
||||
MessageQueueId_t queue = service->getRequestQueue();
|
||||
auto returnPair = queueMap.emplace(serviceId, queue);
|
||||
if (not returnPair.second) {
|
||||
#endif
|
||||
MessageQueueId_t queue = service->getRequestQueue();
|
||||
auto returnPair = queueMap.emplace(serviceId, queue);
|
||||
if (not returnPair.second) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "PUSDistributor::registerService: Service ID already"
|
||||
" exists in map." << std::endl;
|
||||
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
|
||||
return SERVICE_ID_ALREADY_EXISTS;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
#endif
|
||||
return SERVICE_ID_ALREADY_EXISTS;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
MessageQueueId_t PUSDistributor::getRequestQueue() {
|
||||
return tcQueue->getId();
|
||||
return tcQueue->getId();
|
||||
}
|
||||
|
||||
ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
|
||||
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;
|
||||
}
|
||||
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() {
|
||||
return checker.getApid();
|
||||
return checker.getApid();
|
||||
}
|
||||
|
||||
ReturnValue_t PUSDistributor::initialize() {
|
||||
@ -111,15 +124,17 @@ ReturnValue_t PUSDistributor::initialize() {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
CCSDSDistributorIF* ccsdsDistributor =
|
||||
objectManager->get<CCSDSDistributorIF>(packetSource);
|
||||
if (ccsdsDistributor == nullptr) {
|
||||
CCSDSDistributorIF* ccsdsDistributor =
|
||||
objectManager->get<CCSDSDistributorIF>(packetSource);
|
||||
if (ccsdsDistributor == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "PUSDistributor::initialize: Packet source invalid."
|
||||
<< " Make sure it exists and implements CCSDSDistributorIF!"
|
||||
<< std::endl;
|
||||
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
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return ccsdsDistributor->registerApplication(this);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
return ccsdsDistributor->registerApplication(this);
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user