122 lines
3.9 KiB
C++
122 lines
3.9 KiB
C++
#include "PusTmFunnel.h"
|
|
|
|
#include <limits>
|
|
|
|
#include "eive/definitions.h"
|
|
#include "eive/objects.h"
|
|
#include "fsfw/ipc/CommandMessage.h"
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/objectmanager.h"
|
|
#include "fsfw/pus/Service5EventReporting.h"
|
|
#include "fsfw/tmstorage/TmStoreMessage.h"
|
|
#include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h"
|
|
#include "tmtc/pusIds.h"
|
|
|
|
PusTmFunnel::PusTmFunnel(TmFunnelBase::FunnelCfg cfg, StorageManagerIF &ramToFileStore,
|
|
TimeReaderIF &timeReader)
|
|
: TmFunnelBase(cfg), ramToFileStore(ramToFileStore), timeReader(timeReader) {}
|
|
|
|
PusTmFunnel::~PusTmFunnel() = default;
|
|
|
|
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
|
|
CommandMessage cmdMessage;
|
|
ReturnValue_t result;
|
|
TmTcMessage currentMessage;
|
|
unsigned int count = 0;
|
|
if (saveSequenceCount) {
|
|
result = saveSequenceCountToFile();
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusTmFunnel: Storing sequence count to file has failed" << std::endl;
|
|
}
|
|
saveSequenceCount = false;
|
|
}
|
|
result = tmQueue->receiveMessage(¤tMessage);
|
|
while (result == returnvalue::OK) {
|
|
result = handleTmPacket(currentMessage);
|
|
if (result != returnvalue::OK) {
|
|
sif::warning << "TmFunnel packet handling failed" << std::endl;
|
|
break;
|
|
}
|
|
count++;
|
|
if (count == 1000) {
|
|
sif::error << "PusTmFunnel: Possible message storm detected" << std::endl;
|
|
break;
|
|
}
|
|
result = tmQueue->receiveMessage(¤tMessage);
|
|
}
|
|
|
|
if (result == MessageQueueIF::EMPTY) {
|
|
return returnvalue::OK;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::handleTmPacket(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++);
|
|
// NOTE: This only works because the limit value bit width is below 16 bits!
|
|
sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
|
|
|
|
// Message type counter handling.
|
|
uint8_t service = packet.getService();
|
|
bool insertionFailed = false;
|
|
auto mapIter = msgCounterMap.find(service);
|
|
if (mapIter == msgCounterMap.end()) {
|
|
auto iterPair = msgCounterMap.emplace(service, 0);
|
|
if (iterPair.second) {
|
|
mapIter = iterPair.first;
|
|
} else {
|
|
// Should really never never happen but you never know..
|
|
insertionFailed = true;
|
|
}
|
|
}
|
|
if (not insertionFailed) {
|
|
packet.setMessageCount(mapIter->second);
|
|
// Sane overflow handling.
|
|
if (mapIter->second == std::numeric_limits<uint16_t>::max()) {
|
|
mapIter->second = 0;
|
|
} else {
|
|
mapIter->second++;
|
|
}
|
|
}
|
|
|
|
// Re-calculate CRC after changing the fields. This operation HAS to come last!
|
|
packet.updateErrorControl();
|
|
|
|
// 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);
|
|
TmTcMessage msg(storageId);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusLiveDemux::handlePacket: Store too full to create data copy" << std::endl;
|
|
} else {
|
|
tmQueue->sendMessage(destination, &msg);
|
|
}
|
|
}
|
|
|
|
// Send to registered live targets.
|
|
return demultiplexLivePackets(origStoreId, packetData, size);
|
|
}
|
|
|
|
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }
|
|
|
|
void PusTmFunnel::addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest) {
|
|
persistentTmMap.addRouting(filter, dest);
|
|
}
|