61 lines
2.2 KiB
C++
61 lines
2.2 KiB
C++
#include "CfdpTmFunnel.h"
|
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
|
#include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
|
|
#include "fsfw/tmtcservices/TmTcMessage.h"
|
|
|
|
CfdpTmFunnel::CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid,
|
|
const AcceptsTelemetryIF& downlinkDestination, StorageManagerIF& tmStore)
|
|
: SystemObject(objectId), cfdpInCcsdsApid(cfdpInCcsdsApid), tmStore(tmStore) {
|
|
msgQueue = QueueFactory::instance()->createMessageQueue(5);
|
|
}
|
|
|
|
const char* CfdpTmFunnel::getName() const { return "CFDP TM Funnel"; }
|
|
|
|
MessageQueueId_t CfdpTmFunnel::getReportReceptionQueue(uint8_t virtualChannel) const {
|
|
return msgQueue->getId();
|
|
}
|
|
|
|
ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) {
|
|
TmTcMessage currentMessage;
|
|
ReturnValue_t status = msgQueue->receiveMessage(¤tMessage);
|
|
while (status == returnvalue::OK) {
|
|
status = handlePacket(currentMessage);
|
|
if (status != returnvalue::OK) {
|
|
sif::warning << "TmFunnel packet handling failed" << std::endl;
|
|
break;
|
|
}
|
|
status = msgQueue->receiveMessage(¤tMessage);
|
|
}
|
|
|
|
if (status == MessageQueueIF::EMPTY) {
|
|
return returnvalue::OK;
|
|
}
|
|
return status;
|
|
}
|
|
|
|
ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; }
|
|
|
|
ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) {
|
|
uint8_t* packetData = nullptr;
|
|
size_t size = 0;
|
|
ReturnValue_t result = tmStore.modifyData(msg.getStorageId(), &packetData, &size);
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
auto spacePacket =
|
|
SpacePacketCreator(ccsds::PacketType::TM, false, cfdpInCcsdsApid,
|
|
ccsds::SequenceFlags::UNSEGMENTED, sourceSequenceCount++, 0);
|
|
sourceSequenceCount = sourceSequenceCount & ccsds::LIMIT_SEQUENCE_COUNT;
|
|
spacePacket.setCcsdsLenFromTotalDataFieldLen(size);
|
|
// TODO: Get new slot from TM store, serialize space packet header first, and then the CFDP packet
|
|
result = msgQueue->sendToDefault(&msg);
|
|
if (result != returnvalue::OK) {
|
|
tmStore.deleteData(msg.getStorageId());
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "CfdpTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl;
|
|
#endif
|
|
}
|
|
return result;
|
|
}
|