eive-obsw/mission/tmtc/PusTmFunnel.cpp

87 lines
2.9 KiB
C++
Raw Normal View History

2022-10-21 11:51:44 +02:00
#include "PusTmFunnel.h"
2022-12-13 14:26:20 +01:00
#include "eive/definitions.h"
2022-12-13 14:19:43 +01:00
#include "eive/objects.h"
2022-12-19 13:25:45 +01:00
#include "fsfw/ipc/CommandMessage.h"
2022-10-21 11:51:44 +02:00
#include "fsfw/ipc/QueueFactory.h"
#include "fsfw/objectmanager.h"
2023-02-22 14:27:50 +01:00
#include "fsfw/pus/Service5EventReporting.h"
2022-12-19 13:25:45 +01:00
#include "fsfw/tmstorage/TmStoreMessage.h"
2022-10-21 11:51:44 +02:00
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
2023-02-22 14:27:50 +01:00
#include "tmtc/pusIds.h"
2022-10-21 11:51:44 +02:00
PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
TimeReaderIF &timeReader, SdCardMountedIF &sdcMan)
: TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader), sdcMan(sdcMan) {}
2022-10-21 11:51:44 +02:00
PusTmFunnel::~PusTmFunnel() = default;
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
2022-12-19 13:25:45 +01:00
CommandMessage cmdMessage;
2023-02-22 13:27:16 +01:00
ReturnValue_t result;
2022-10-21 11:51:44 +02:00
TmTcMessage currentMessage;
2023-02-19 17:06:08 +01:00
unsigned int count = 0;
2023-02-21 20:43:16 +01:00
result = tmQueue->receiveMessage(&currentMessage);
while (result == returnvalue::OK) {
result = handleTmPacket(currentMessage);
if (result != returnvalue::OK) {
2022-10-21 11:51:44 +02:00
sif::warning << "TmFunnel packet handling failed" << std::endl;
break;
}
2023-02-19 17:06:08 +01:00
count++;
2023-03-09 20:18:19 +01:00
if (count == 1000) {
2023-02-19 17:06:08 +01:00
sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
break;
}
2023-02-21 20:43:16 +01:00
result = tmQueue->receiveMessage(&currentMessage);
2022-10-21 11:51:44 +02:00
}
2023-02-21 20:43:16 +01:00
if (result == MessageQueueIF::EMPTY) {
2022-10-21 11:51:44 +02:00
return returnvalue::OK;
}
2023-02-21 20:43:16 +01:00
return result;
2022-10-21 11:51:44 +02:00
}
ReturnValue_t PusTmFunnel::handleTmPacket(TmTcMessage &message) {
2022-10-21 11:51:44 +02:00
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();
2023-03-09 11:46:13 +01:00
// Send to persistent TM store if the packet matches some filter.
MessageQueueId_t destination;
if (persistentTmMap.packetMatches(packet, destination)) {
store_address_t storageId;
result = ramToFileStore.addData(&storageId, packetData, size);
2023-03-10 02:05:51 +01:00
TmTcMessage msg(storageId);
2023-03-09 11:46:13 +01:00
if (result != returnvalue::OK) {
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
} else {
tmQueue->sendMessage(destination, &msg);
}
}
2023-03-09 20:18:19 +01:00
// Send to registered live targets.
2023-03-09 11:46:13 +01:00
return demultiplexLivePackets(origStoreId, packetData, size);
2022-10-21 11:51:44 +02:00
}
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }
2022-12-13 14:30:16 +01:00
void PusTmFunnel::addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest) {
2023-03-09 11:46:13 +01:00
persistentTmMap.addRouting(filter, dest);
}