CCSDS unpacker implementation

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;
+}

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