83 lines
2.7 KiB
C++
83 lines
2.7 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, const AcceptsTelemetryIF &downlinkDestination,
|
|
TimeReaderIF &timeReader, uint32_t messageDepth)
|
|
: SystemObject(objectId), timeReader(timeReader) {
|
|
tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth,
|
|
MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
tmQueue->setDefaultDestination(downlinkDestination.getReportReceptionQueue());
|
|
}
|
|
|
|
PusTmFunnel::~PusTmFunnel() = default;
|
|
|
|
MessageQueueId_t PusTmFunnel::getReportReceptionQueue(uint8_t virtualChannel) const {
|
|
return tmQueue->getId();
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::performOperation(uint8_t) {
|
|
TmTcMessage currentMessage;
|
|
ReturnValue_t status = tmQueue->receiveMessage(¤tMessage);
|
|
while (status == returnvalue::OK) {
|
|
status = handlePacket(¤tMessage);
|
|
if (status != returnvalue::OK) {
|
|
sif::warning << "TmFunnel packet handling failed" << std::endl;
|
|
break;
|
|
}
|
|
status = tmQueue->receiveMessage(¤tMessage);
|
|
}
|
|
|
|
if (status == MessageQueueIF::EMPTY) {
|
|
return returnvalue::OK;
|
|
} else {
|
|
return status;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage *message) {
|
|
uint8_t *packetData = nullptr;
|
|
size_t size = 0;
|
|
ReturnValue_t result = tmPool->modifyData(message->getStorageId(), &packetData, &size);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
PusTmZeroCopyWriter packet(timeReader, packetData, size);
|
|
result = packet.parseDataWithoutCrcCheck();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
packet.setSequenceCount(sourceSequenceCount++);
|
|
sourceSequenceCount = sourceSequenceCount % ccsds::LIMIT_SEQUENCE_COUNT;
|
|
packet.updateErrorControl();
|
|
|
|
result = tmQueue->sendToDefault(message);
|
|
if (result != returnvalue::OK) {
|
|
tmPool->deleteData(message->getStorageId());
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
|
|
#endif
|
|
return result;
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t PusTmFunnel::initialize() {
|
|
tmPool = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
|
|
if (tmPool == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "TmFunnel::initialize: TM store not set." << std::endl;
|
|
sif::error << "Make sure the tm store is set up properly and implements "
|
|
"StorageManagerIF"
|
|
<< std::endl;
|
|
#endif
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
}
|
|
return SystemObject::initialize();
|
|
}
|
|
|
|
const char *PusTmFunnel::getName() const { return "PUS TM Funnel"; }
|