diff --git a/example/core/GenericFactory.cpp b/example/core/GenericFactory.cpp index 9cb2ce2..10f1757 100644 --- a/example/core/GenericFactory.cpp +++ b/example/core/GenericFactory.cpp @@ -64,7 +64,8 @@ void ObjectFactory::produceGenericObjects(PusTmFunnel **pusFunnel, new CcsdsDistributor(common::COMMON_PUS_APID, objects::CCSDS_DISTRIBUTOR, &tcStore); new PusDistributor(common::COMMON_PUS_APID, objects::PUS_DISTRIBUTOR, *ccsdsDistrib); *pusFunnel = new PusTmFunnel(objects::PUS_TM_FUNNEL, tmtcBridge, *stamperAndReader, tmStore); - auto *cfdpFunnel = new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, tmtcBridge, tmStore); + auto *cfdpFunnel = + new CfdpTmFunnel(objects::CFDP_TM_FUNNEL, common::COMMON_CFDP_APID, tmtcBridge, tmStore); new TmFunnel(objects::TM_FUNNEL, **pusFunnel, *cfdpFunnel); #endif /* OBSW_ADD_CORE_COMPONENTS == 1 */ diff --git a/example/utility/CfdpTmFunnel.cpp b/example/utility/CfdpTmFunnel.cpp index df9e303..9568112 100644 --- a/example/utility/CfdpTmFunnel.cpp +++ b/example/utility/CfdpTmFunnel.cpp @@ -1,11 +1,12 @@ #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, const AcceptsTelemetryIF& downlinkDestination, - StorageManagerIF& tmStore) - : SystemObject(objectId), tmStore(tmStore) { +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); } @@ -35,4 +36,25 @@ ReturnValue_t CfdpTmFunnel::performOperation(uint8_t) { ReturnValue_t CfdpTmFunnel::initialize() { return returnvalue::OK; } -ReturnValue_t CfdpTmFunnel::handlePacket(TmTcMessage& msg) { return 0; } +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; +} diff --git a/example/utility/CfdpTmFunnel.h b/example/utility/CfdpTmFunnel.h index 28da00e..615f91d 100644 --- a/example/utility/CfdpTmFunnel.h +++ b/example/utility/CfdpTmFunnel.h @@ -8,8 +8,8 @@ class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject { public: - CfdpTmFunnel(object_id_t objectId, const AcceptsTelemetryIF& downlinkDestination, - StorageManagerIF& tmStore); + CfdpTmFunnel(object_id_t objectId, uint16_t cfdpInCcsdsApid, + const AcceptsTelemetryIF& downlinkDestination, StorageManagerIF& tmStore); [[nodiscard]] const char* getName() const override; [[nodiscard]] MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; @@ -19,6 +19,8 @@ class CfdpTmFunnel : public AcceptsTelemetryIF, public SystemObject { private: ReturnValue_t handlePacket(TmTcMessage& msg); + uint16_t sourceSequenceCount = 0; + uint16_t cfdpInCcsdsApid; MessageQueueIF* msgQueue; StorageManagerIF& tmStore; }; diff --git a/example/utility/PusTmFunnel.cpp b/example/utility/PusTmFunnel.cpp index 78f0de8..da590b7 100644 --- a/example/utility/PusTmFunnel.cpp +++ b/example/utility/PusTmFunnel.cpp @@ -5,7 +5,7 @@ #include "fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h" PusTmFunnel::PusTmFunnel(object_id_t objectId, const AcceptsTelemetryIF &downlinkDestination, - TimeReaderIF &timeReader, StorageManagerIF& tmStore, uint32_t messageDepth) + TimeReaderIF &timeReader, StorageManagerIF &tmStore, uint32_t messageDepth) : SystemObject(objectId), timeReader(timeReader), tmStore(tmStore) { tmQueue = QueueFactory::instance()->createMessageQueue(messageDepth, MessageQueueMessage::MAX_MESSAGE_SIZE); @@ -36,7 +36,7 @@ ReturnValue_t PusTmFunnel::performOperation(uint8_t) { return status; } -ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage& message) { +ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage &message) { uint8_t *packetData = nullptr; size_t size = 0; ReturnValue_t result = tmStore.modifyData(message.getStorageId(), &packetData, &size); @@ -56,11 +56,9 @@ ReturnValue_t PusTmFunnel::handlePacket(TmTcMessage& message) { if (result != returnvalue::OK) { tmStore.deleteData(message.getStorageId()); #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "TmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; + sif::error << "PusTmFunnel::handlePacket: Error sending TM to downlink handler" << std::endl; #endif - return result; } - return result; } diff --git a/example/utility/PusTmFunnel.h b/example/utility/PusTmFunnel.h index ae2e856..ac6fe4e 100644 --- a/example/utility/PusTmFunnel.h +++ b/example/utility/PusTmFunnel.h @@ -20,7 +20,7 @@ class PusTmFunnel : public AcceptsTelemetryIF, public SystemObject { public: explicit PusTmFunnel(object_id_t objectId, const AcceptsTelemetryIF &downlinkDestination, - TimeReaderIF &timeReader, StorageManagerIF& tmStore, + TimeReaderIF &timeReader, StorageManagerIF &tmStore, uint32_t messageDepth = 20); [[nodiscard]] const char *getName() const override; ~PusTmFunnel() override; @@ -31,9 +31,9 @@ class PusTmFunnel : public AcceptsTelemetryIF, public SystemObject { private: uint16_t sourceSequenceCount = 0; TimeReaderIF &timeReader; - StorageManagerIF& tmStore; + StorageManagerIF &tmStore; MessageQueueIF *tmQueue = nullptr; - ReturnValue_t handlePacket(TmTcMessage& message); + ReturnValue_t handlePacket(TmTcMessage &message); }; #endif // FSFW_EXAMPLE_COMMON_PUSTMFUNNEL_H