eive-obsw/mission/tmtc/PusTmFunnel.cpp

84 lines
2.8 KiB
C++
Raw Normal View History

2022-10-21 11:51:44 +02:00
#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,
2022-10-21 11:51:44 +02:00
uint32_t messageDepth)
: TmFunnelBase(objectId, tmStore, messageDepth), timeReader(timeReader) {}
2022-10-21 11:51:44 +02:00
PusTmFunnel::~PusTmFunnel() = default;
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
TmTcMessage currentMessage;
ReturnValue_t status = tmQueue->receiveMessage(&currentMessage);
while (status == returnvalue::OK) {
status = handlePacket(currentMessage);
if (status != returnvalue::OK) {
sif::warning << "TmFunnel packet handling failed" << std::endl;
break;
}
status = tmQueue->receiveMessage(&currentMessage);
}
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);
2022-10-21 11:51:44 +02:00
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++) {
2023-02-07 17:18:13 +01:00
const auto &dest = destinations[idx];
2022-11-02 10:55:40 +01:00
if (destinations.size() > 1) {
2022-11-03 15:29:54 +01:00
if (idx < destinations.size() - 1) {
2022-11-02 10:55:40 +01:00
// 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);
2022-11-03 15:29:54 +01:00
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
}
2022-11-02 10:55:40 +01:00
} else {
message.setStorageId(origStoreId);
}
}
2023-02-07 17:18:13 +01:00
result = tmQueue->sendMessage(dest.queueId, &message);
if (result != returnvalue::OK) {
2022-10-21 11:51:44 +02:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2023-02-07 17:18:13 +01:00
sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler " << dest.name
<< " failed" << std::endl;
2022-10-21 11:51:44 +02:00
#endif
tmStore.deleteData(message.getStorageId());
}
2022-10-21 11:51:44 +02:00
}
return result;
}
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }