diff --git a/src/fsfw/cfdp/handler/SourceHandler.cpp b/src/fsfw/cfdp/handler/SourceHandler.cpp
index b9d06b92..dcb5d68a 100644
--- a/src/fsfw/cfdp/handler/SourceHandler.cpp
+++ b/src/fsfw/cfdp/handler/SourceHandler.cpp
@@ -11,6 +11,7 @@
 #include "fsfw/globalfunctions/arrayprinter.h"
 #include "fsfw/objectmanager.h"
 #include "fsfw/serviceinterface.h"
+#include "fsfw/tasks/TaskFactory.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
 using namespace returnvalue;
@@ -69,13 +70,10 @@ cfdp::SourceHandler::FsmResult& cfdp::SourceHandler::fsmNacked() {
   if (step == TransactionStep::SENDING_FILE_DATA) {
     bool noFdPdu = false;
     result = prepareAndSendNextFileDataPdu(noFdPdu);
-    if (result != OK) {
-      // TODO: Error handling
-    }
-    if (!noFdPdu) {
+    if (result == OK and !noFdPdu) {
       fsmResult.callStatus = CallStatus::CALL_AGAIN;
-      return fsmResult;
     }
+    return fsmResult;
   }
   if (step == TransactionStep::SENDING_EOF) {
     result = prepareAndSendEofPdu();
@@ -269,16 +267,20 @@ ReturnValue_t cfdp::SourceHandler::prepareAndSendNextFileDataPdu(bool& noFileDat
     addError(result);
     return result;
   }
+  sif::debug << "sending next fd pdu, offset " << offset.value() << "" << std::endl;
   auto fileDataInfo = FileDataInfo(offset, fileBuf.data(), readLen);
   auto fileDataPdu = FileDataCreator(transactionParams.pduConf, fileDataInfo);
   result = sendGenericPdu(fileDataPdu);
   if (result != OK) {
+    sif::debug << "SENDING generic PDU failed" << std::endl;
     return result;
   }
   transactionParams.progress += readLen;
   if (transactionParams.progress >= fileSize) {
     // Advance FSM after all file data PDUs were sent.
     step = TransactionStep::SENDING_EOF;
+    sif::debug << "done" << std::endl;
+    TaskFactory::delayTask(10000);
   }
   return OK;
 }
@@ -322,6 +324,10 @@ ReturnValue_t cfdp::SourceHandler::sendGenericPdu(const SerializeIF& pdu) {
       fsfwParams.tmStore->getFreeElement(&storeId, pdu.getSerializedSize(), &dataPtr);
   if (result != OK) {
     addError(result);
+    fsmResult.callStatus = CallStatus::CALL_AFTER_DELAY;
+    if(result == StorageManagerIF::DATA_STORAGE_FULL) {
+      return TM_STORE_FULL;
+    }
     return result;
   }
   size_t serializedLen = 0;
@@ -333,8 +339,10 @@ ReturnValue_t cfdp::SourceHandler::sendGenericPdu(const SerializeIF& pdu) {
   TmTcMessage tmMsg(storeId);
   result =
       fsfwParams.msgQueue->sendMessage(fsfwParams.packetDest.getReportReceptionQueue(), &tmMsg);
-  if (result == MessageQueueIF::FULL) {
+  if(result != OK) {
     fsmResult.callStatus = CallStatus::CALL_AFTER_DELAY;
+  }
+  if (result == MessageQueueIF::FULL) {
     return TARGET_MSG_QUEUE_FULL;
   } else if (result == OK) {
     fsmResult.packetsSent += 1;
diff --git a/src/fsfw/cfdp/handler/defs.h b/src/fsfw/cfdp/handler/defs.h
index cafc3afc..4356835d 100644
--- a/src/fsfw/cfdp/handler/defs.h
+++ b/src/fsfw/cfdp/handler/defs.h
@@ -70,6 +70,7 @@ static constexpr ReturnValue_t SOURCE_NAME_EMPTY = returnvalue::makeCode(CID, 3)
 static constexpr ReturnValue_t DEST_NAME_EMPTY = returnvalue::makeCode(CID, 4);
 static constexpr ReturnValue_t WRONG_REMOTE_CFG_ENTITY_ID = returnvalue::makeCode(CID, 5);
 static constexpr ReturnValue_t TARGET_MSG_QUEUE_FULL = returnvalue::makeCode(CID, 6);
+static constexpr ReturnValue_t TM_STORE_FULL = returnvalue::makeCode(CID, 7);
 
 }  // namespace cfdp
 #endif  // FSFW_CFDP_HANDLER_DEFS_H