131 lines
4.4 KiB
C++
131 lines
4.4 KiB
C++
#include "PusTmFunnel.h"
|
|
|
|
#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, TimeReaderIF &timeReader,
|
|
SdCardMountedIF &sdcMan)
|
|
: TmFunnelBase(cfg), timeReader(timeReader), sdcMan(sdcMan) {}
|
|
|
|
PusTmFunnel::~PusTmFunnel() = default;
|
|
|
|
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
|
|
CommandMessage cmdMessage;
|
|
ReturnValue_t result;
|
|
/*
|
|
try {
|
|
result = okStore.handleCommandQueue(ipcStore, *this);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusTmFunnel::performOperation: Issue handling OK store command" << std::endl;
|
|
}
|
|
result = notOkStore.handleCommandQueue(ipcStore, *this);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusTmFunnel::performOperation: Issue handling NOT OK store command"
|
|
<< std::endl;
|
|
}
|
|
result = hkStore.handleCommandQueue(ipcStore, *this);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusTmFunnel::performOperation: Issue handling HK store command" << std::endl;
|
|
}
|
|
result = miscStore.handleCommandQueue(ipcStore, *this);
|
|
if (result != returnvalue::OK) {
|
|
sif::error << "PusTmFunnel::performOperation: Issue handling MISC store command" << std::endl;
|
|
}
|
|
} catch (const std::bad_optional_access &e) {
|
|
sif::error << e.what() << "when handling TM store command" << std::endl;
|
|
}
|
|
*/
|
|
|
|
TmTcMessage currentMessage;
|
|
unsigned int count = 0;
|
|
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 == 500) {
|
|
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++);
|
|
sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
|
|
packet.updateErrorControl();
|
|
|
|
// TODO: 1. Send packet to persistent TM store when applicable.
|
|
// 2. Send packet to live TM VC
|
|
// 3. Send packet to TCP/IP destination
|
|
return sendPacketToDestinations(origStoreId, message, packetData, size);
|
|
// timeval currentUptime{};
|
|
// Clock::getUptime(¤tUptime);
|
|
// if (currentUptime.tv_sec - lastTvUpdate.tv_sec >
|
|
// static_cast<signed int>(TV_UPDATE_INTERVAL_SECS)) {
|
|
// Clock::getClock_timeval(¤tTv);
|
|
// lastTvUpdate = currentUptime;
|
|
// }
|
|
|
|
// bool sdcUsable = sdcMan.isSdCardUsable(std::nullopt);
|
|
// initStoresIfPossible(sdcUsable);
|
|
// if (sdcUsable) {
|
|
// miscStore.passPacket(packet);
|
|
// okStore.passPacket(packet);
|
|
// notOkStore.passPacket(packet);
|
|
// hkStore.passPacket(packet);
|
|
// }
|
|
}
|
|
|
|
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }
|
|
|
|
void PusTmFunnel::initStoresIfPossible(bool sdCardUsable) {
|
|
if (not storesInitialized and sdCardUsable and sdcMan.getCurrentMountPrefix() != nullptr) {
|
|
// miscStore.initializeTmStore();
|
|
// okStore.initializeTmStore();
|
|
// hkStore.initializeTmStore();
|
|
// notOkStore.initializeTmStore();
|
|
storesInitialized = true;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::initialize() {
|
|
// initStoresIfPossible(sdcMan.isSdCardUsable(std::nullopt));
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
void PusTmFunnel::addPersistentTmStoreRouting(PusPacketFilter filter, MessageQueueId_t dest) {
|
|
router.addRouting(filter, dest);
|
|
}
|