diff --git a/src/fsfw/tcdistribution/CcsdsUnpacker.cpp b/src/fsfw/tcdistribution/CcsdsUnpacker.cpp index 2f8e9f275..226b857ba 100644 --- a/src/fsfw/tcdistribution/CcsdsUnpacker.cpp +++ b/src/fsfw/tcdistribution/CcsdsUnpacker.cpp @@ -1,7 +1,60 @@ #include "CcsdsUnpacker.h" -CcsdsUnpacker::CcsdsUnpacker() {} +#include "fsfw/tmtcservices/TmTcMessage.h" + +CcsdsUnpacker::CcsdsUnpacker(MessageQueueIF& msgQueue, AcceptsTelecommandsIF& receiver, + StorageManagerIF& sourceStore) + : sourceStore(sourceStore), msgQueue(msgQueue), receiver(receiver) { + msgQueue.setDefaultDestination(receiver.getRequestQueue()); +} + +ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) { + TmTcMessage msg; + ReturnValue_t result; + for (result = msgQueue.receiveMessage(&msg); result == HasReturnvaluesIF::RETURN_OK; + result = msgQueue.receiveMessage(&msg)) { + auto resultPair = sourceStore.getData(msg.getStorageId()); + if(resultPair.first != HasReturnvaluesIF::RETURN_OK) { + continue; + } + if(resultPair.second.size() < 6) { + // TODO: This is a config error. Does it make sense to forward the message? + result = msgQueue.sendToDefault(&msg); + if(result != HasReturnvaluesIF::RETURN_OK) { + + } + continue; + } + StorageManagerIF* tgtStore; + if(targetStore != nullptr) { + tgtStore = targetStore; + } else { + tgtStore = &sourceStore; + } + store_address_t newId; + uint8_t* ptr; + result = tgtStore->getFreeElement(&newId, resultPair.second.size(), &ptr); + if(result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Implement error handling + } + std::memcpy(ptr, resultPair.second.data() + 6, resultPair.second.size() - 6); + result = sourceStore.deleteData(msg.getStorageId()); + if(result != HasReturnvaluesIF::RETURN_OK) { + // TODO: Implement error handling (though this really should not happen) + } + TmTcMessage newMsg(newId); + result = msgQueue.sendToDefault(&newMsg); + if(result != HasReturnvaluesIF::RETURN_OK) { + + } + } + return result; +} -ReturnValue_t CcsdsUnpacker::performOperation(uint8_t operationCode) { return 0; } uint32_t CcsdsUnpacker::getIdentifier() { return 0; } -MessageQueueId_t CcsdsUnpacker::getRequestQueue() { return 0; } + +MessageQueueId_t CcsdsUnpacker::getRequestQueue() { return msgQueue.getId(); } + +void CcsdsUnpacker::setDifferentTargetStore(StorageManagerIF& otherTargetStore) { + targetStore = &otherTargetStore; +} diff --git a/src/fsfw/tcdistribution/CcsdsUnpacker.h b/src/fsfw/tcdistribution/CcsdsUnpacker.h index a134de33e..c4eb7da46 100644 --- a/src/fsfw/tcdistribution/CcsdsUnpacker.h +++ b/src/fsfw/tcdistribution/CcsdsUnpacker.h @@ -1,16 +1,23 @@ #ifndef FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H #define FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H +#include "fsfw/storagemanager/StorageManagerIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" class CcsdsUnpacker : public ExecutableObjectIF, public AcceptsTelecommandsIF { public: - CcsdsUnpacker(); + CcsdsUnpacker(MessageQueueIF& msgQueue, AcceptsTelecommandsIF& receiver, StorageManagerIF& sourceStore); + + void setDifferentTargetStore(StorageManagerIF& otherTargetStore); ReturnValue_t performOperation(uint8_t operationCode) override; uint32_t getIdentifier() override; MessageQueueId_t getRequestQueue() override; private: + StorageManagerIF& sourceStore; + StorageManagerIF* targetStore = nullptr; + MessageQueueIF& msgQueue; + AcceptsTelecommandsIF& receiver; }; #endif // FSFW_TCDISTRIBUTION_CCSDSUNPACKER_H