90 lines
3.0 KiB
C++
90 lines
3.0 KiB
C++
#include "PusTmFunnel.h"
|
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/objectmanager.h"
|
|
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
|
|
|
|
PusTmFunnel::PusTmFunnel(object_id_t objectId, TimeReaderIF &timeReader, StorageManagerIF &tmStore,
|
|
uint32_t messageDepth)
|
|
: TmFunnelBase(objectId, tmStore, messageDepth), timeReader(timeReader) {}
|
|
|
|
PusTmFunnel::~PusTmFunnel() = default;
|
|
|
|
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
|
|
TmTcMessage currentMessage;
|
|
unsigned int count = 0;
|
|
ReturnValue_t status = tmQueue->receiveMessage(¤tMessage);
|
|
while (status == returnvalue::OK) {
|
|
status = handlePacket(currentMessage);
|
|
if (status != returnvalue::OK) {
|
|
sif::warning << "TmFunnel packet handling failed" << std::endl;
|
|
break;
|
|
}
|
|
count++;
|
|
if (count == 500) {
|
|
sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
|
|
break;
|
|
}
|
|
status = tmQueue->receiveMessage(¤tMessage);
|
|
}
|
|
|
|
if (status == MessageQueueIF::EMPTY) {
|
|
return returnvalue::OK;
|
|
}
|
|
return status;
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) {
|
|
uint8_t *packetData = nullptr;
|
|
size_t size = 0;
|
|
store_address_t origStoreId = message.getStorageId();
|
|
ReturnValue_t result = tmStore.modifyData(origStoreId, &packetData, &size);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
PusTmZeroCopyWriter packet(timeReader, packetData, size);
|
|
result = packet.parseDataWithoutCrcCheck();
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "PusTmFunnel::handlePacket: Error parsing received PUS packet" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
packet.setSequenceCount(sourceSequenceCount++);
|
|
sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
|
|
packet.updateErrorControl();
|
|
|
|
for (unsigned int idx = 0; idx < destinations.size(); idx++) {
|
|
const auto &dest = destinations[idx];
|
|
if (destinations.size() > 1) {
|
|
if (idx < destinations.size() - 1) {
|
|
// Create copy of data to ensure each TM recipient has its own copy. That way, we don't need
|
|
// to bother with send order and where the data is deleted.
|
|
store_address_t storeId;
|
|
result = tmStore.addData(&storeId, packetData, size);
|
|
if (result == returnvalue::OK) {
|
|
message.setStorageId(storeId);
|
|
} else {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "PusTmFunnel::handlePacket: Store too full to create data copy"
|
|
<< std::endl;
|
|
#endif
|
|
}
|
|
} else {
|
|
message.setStorageId(origStoreId);
|
|
}
|
|
}
|
|
result = tmQueue->sendMessage(dest.queueId, &message);
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name
|
|
<< " failed" << std::endl;
|
|
#endif
|
|
tmStore.deleteData(message.getStorageId());
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }
|