diff --git a/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h b/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h
index 31067268..c350bb60 100644
--- a/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h
+++ b/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h
@@ -98,10 +98,10 @@ class ProvidesDataPoolSubscriptionIF {
   virtual ReturnValue_t subscribeForRegularUpdatePacket(subdp::RegularHkUpdateParams params) = 0;
   virtual ReturnValue_t subscribeForDiagUpdatePacket(subdp::DiagnosticsHkUpdateParams params) = 0;
 
-//  virtual ReturnValue_t
-//  subscribeForUpdatePacket(sid_t sid, bool reportingEnabled, bool isDiagnostics) {
-//    return subscribeForUpdatePacket(sid, reportingEnabled, isDiagnostics, objects::NO_OBJECT);
-//  }
+  //  virtual ReturnValue_t
+  //  subscribeForUpdatePacket(sid_t sid, bool reportingEnabled, bool isDiagnostics) {
+  //    return subscribeForUpdatePacket(sid, reportingEnabled, isDiagnostics, objects::NO_OBJECT);
+  //  }
 
   [[deprecated(
       "Please use the new API which takes all arguments as one wrapper "
diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h
index 4020c6dc..7d60fb8e 100644
--- a/src/fsfw/osal/host/MessageQueue.h
+++ b/src/fsfw/osal/host/MessageQueue.h
@@ -68,7 +68,7 @@ class MessageQueue : public MessageQueueBase {
    * @details	This is accomplished by using the delete call provided
    * 			by the operating system.
    */
-  virtual ~MessageQueue();
+  ~MessageQueue() override;
 
   // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
   virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
@@ -111,8 +111,6 @@ class MessageQueue : public MessageQueueBase {
   size_t messageDepth = 0;
 
   MutexIF* queueLock;
-
-  MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
 };
 
 #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */
diff --git a/src/fsfw/osal/host/QueueMapManager.cpp b/src/fsfw/osal/host/QueueMapManager.cpp
index 72c70baa..301ed131 100644
--- a/src/fsfw/osal/host/QueueMapManager.cpp
+++ b/src/fsfw/osal/host/QueueMapManager.cpp
@@ -54,7 +54,7 @@ MessageQueueIF* QueueMapManager::getMessageQueue(MessageQueueId_t messageQueueId
   } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId
-                 << " does not exists in the map!" << std::endl;
+                 << " does not exist in the map" << std::endl;
 #else
     sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
                       messageQueueId);
diff --git a/src/fsfw/pus/Service1TelecommandVerification.cpp b/src/fsfw/pus/Service1TelecommandVerification.cpp
index 102ae5a0..6c1c2310 100644
--- a/src/fsfw/pus/Service1TelecommandVerification.cpp
+++ b/src/fsfw/pus/Service1TelecommandVerification.cpp
@@ -68,8 +68,15 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
   FailureReport report(message->getReportId(), message->getTcPacketId(),
                        message->getTcSequenceControl(), message->getStep(), message->getErrorCode(),
                        message->getParameter1(), message->getParameter2());
-  storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
-  storeHelper.setSourceDataSerializable(report);
+  ReturnValue_t result =
+      storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
   return tmHelper.storeAndSendTmPacket();
 }
 
@@ -77,8 +84,15 @@ ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
     PusVerificationMessage* message) {
   SuccessReport report(message->getReportId(), message->getTcPacketId(),
                        message->getTcSequenceControl(), message->getStep());
-  storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
-  storeHelper.setSourceDataSerializable(report);
+  ReturnValue_t result =
+      storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
   return tmHelper.storeAndSendTmPacket();
 }
 
@@ -94,6 +108,9 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
+  if (tmQueue == nullptr) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
   if (tmStore == nullptr) {
     tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
@@ -102,5 +119,14 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
     }
     storeHelper.setTmStore(*tmStore);
   }
+
+  sendHelper.setMsgQueue(*tmQueue);
+  if (errReporter == nullptr) {
+    errReporter =
+        ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+    if (errReporter != nullptr) {
+      sendHelper.setInternalErrorReporter(*errReporter);
+    }
+  }
   return SystemObject::initialize();
 }
diff --git a/src/fsfw/pus/Service1TelecommandVerification.h b/src/fsfw/pus/Service1TelecommandVerification.h
index dcbb5017..2e121bbf 100644
--- a/src/fsfw/pus/Service1TelecommandVerification.h
+++ b/src/fsfw/pus/Service1TelecommandVerification.h
@@ -86,6 +86,7 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,
   TmSendHelper sendHelper;
   TmStoreHelper storeHelper;
   TmStoreAndSendWrapper tmHelper;
+  InternalErrorReporterIF* errReporter = nullptr;
   StorageManagerIF* tmStore = nullptr;
   MessageQueueIF* tmQueue = nullptr;
 
diff --git a/src/fsfw/tcdistribution/CCSDSDistributor.cpp b/src/fsfw/tcdistribution/CCSDSDistributor.cpp
index 9601825c..4dad31aa 100644
--- a/src/fsfw/tcdistribution/CCSDSDistributor.cpp
+++ b/src/fsfw/tcdistribution/CCSDSDistributor.cpp
@@ -4,10 +4,11 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
 
-#define CCSDS_DISTRIBUTOR_DEBUGGING 0
+#define CCSDS_DISTRIBUTOR_DEBUGGING 1
 
-CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId)
-    : TcDistributor(setObjectId), defaultApid(setDefaultApid) {}
+CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId,
+                                   CcsdsPacketCheckIF* packetChecker)
+    : TcDistributor(setObjectId), defaultApid(setDefaultApid), packetChecker(packetChecker) {}
 
 CCSDSDistributor::~CCSDSDistributor() = default;
 
@@ -25,7 +26,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
 #endif
   const uint8_t* packet = nullptr;
   size_t size = 0;
-  ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(), &packet, &size);
+  ReturnValue_t result = tcStore->getData(currentMessage.getStorageId(), &packet, &size);
   if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -41,10 +42,12 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
     return queueMap.end();
   }
   SpacePacketReader currentPacket(packet, size);
-
+  result = packetChecker->checkPacket(currentPacket, size);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+  }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
-  sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex
-            << currentPacket.getAPID() << std::dec << std::endl;
+  sif::info << "CCSDSDistributor::selectDestination has packet with APID 0x" << std::hex
+            << currentPacket.getApid() << std::dec << std::endl;
 #endif
   auto position = this->queueMap.find(currentPacket.getApid());
   if (position != this->queueMap.end()) {
@@ -52,7 +55,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
   } else {
     // The APID was not found. Forward packet to main SW-APID anyway to
     //  create acceptance failure report.
-    return this->queueMap.find(this->defaultApid);
+    return queueMap.find(this->defaultApid);
   }
 }
 
@@ -80,6 +83,9 @@ ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid, MessageQueueI
 uint16_t CCSDSDistributor::getIdentifier() { return 0; }
 
 ReturnValue_t CCSDSDistributor::initialize() {
+  if (packetChecker == nullptr) {
+    packetChecker = new CcsdsPacketChecker(ccsds::PacketType::TC);
+  }
   ReturnValue_t status = this->TcDistributor::initialize();
   this->tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
   if (this->tcStore == nullptr) {
diff --git a/src/fsfw/tcdistribution/CCSDSDistributor.h b/src/fsfw/tcdistribution/CCSDSDistributor.h
index b5cfc5a4..d6e4f0e7 100644
--- a/src/fsfw/tcdistribution/CCSDSDistributor.h
+++ b/src/fsfw/tcdistribution/CCSDSDistributor.h
@@ -1,11 +1,12 @@
 #ifndef FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 #define FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_
 
-#include "../objectmanager/ObjectManagerIF.h"
-#include "../storagemanager/StorageManagerIF.h"
-#include "../tcdistribution/CCSDSDistributorIF.h"
-#include "../tcdistribution/TcDistributor.h"
-#include "../tmtcservices/AcceptsTelecommandsIF.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
+#include "fsfw/storagemanager/StorageManagerIF.h"
+#include "fsfw/tcdistribution/CCSDSDistributorIF.h"
+#include "fsfw/tcdistribution/CcsdsPacketChecker.h"
+#include "fsfw/tcdistribution/TcDistributor.h"
+#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 
 /**
  * @brief 	An instantiation of the CCSDSDistributorIF.
@@ -24,10 +25,11 @@ class CCSDSDistributor : public TcDistributor,
    * 			TcDistributor ctor with a certain object id.
    * @details
    * @c tcStore is set in the @c initialize method.
-   * @param setDefaultApid The default APID, where packets with unknown
+   * @param unknownApid The default APID, where packets with unknown
    * destination are sent to.
    */
-  CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId);
+  CCSDSDistributor(uint16_t unknownApid, object_id_t setObjectId,
+                   CcsdsPacketCheckIF* packetChecker = nullptr);
   /**
    * The destructor is empty.
    */
@@ -63,6 +65,8 @@ class CCSDSDistributor : public TcDistributor,
    * pure Space Packets and there exists no SpacePacketStored class.
    */
   StorageManagerIF* tcStore = nullptr;
+
+  CcsdsPacketCheckIF* packetChecker = nullptr;
 };
 
 #endif /* FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_ */
diff --git a/src/fsfw/tcdistribution/CFDPDistributor.cpp b/src/fsfw/tcdistribution/CFDPDistributor.cpp
index 5aa84d44..0e6f968c 100644
--- a/src/fsfw/tcdistribution/CFDPDistributor.cpp
+++ b/src/fsfw/tcdistribution/CFDPDistributor.cpp
@@ -35,7 +35,7 @@ CFDPDistributor::TcMqMapIter CFDPDistributor::selectDestination() {
   }
   this->currentPacket->setStoreAddress(this->currentMessage.getStorageId());
   if (currentPacket->getFullData() != nullptr) {
-    tcStatus = checker.checkPacket(currentPacket, currentPacket->getFullPacketLen());
+    tcStatus = checker.checkPacket(*currentPacket, currentPacket->getFullPacketLen());
     if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
diff --git a/src/fsfw/tcdistribution/CMakeLists.txt b/src/fsfw/tcdistribution/CMakeLists.txt
index 9cc3d170..532ba2ca 100644
--- a/src/fsfw/tcdistribution/CMakeLists.txt
+++ b/src/fsfw/tcdistribution/CMakeLists.txt
@@ -6,4 +6,4 @@ target_sources(
           PusPacketChecker.cpp
           TcPacketCheckCFDP.cpp
           CFDPDistributor.cpp
-          CcsdsPacketCheckerBase.cpp)
+          CcsdsPacketChecker.cpp)
diff --git a/src/fsfw/tcdistribution/CcsdsPacketCheckIF.h b/src/fsfw/tcdistribution/CcsdsPacketCheckIF.h
index 55288ae4..6c83532e 100644
--- a/src/fsfw/tcdistribution/CcsdsPacketCheckIF.h
+++ b/src/fsfw/tcdistribution/CcsdsPacketCheckIF.h
@@ -26,7 +26,7 @@ class CcsdsPacketCheckIF {
    *            - @c INCORRECT_CHECKSUM if checksum is invalid.
    * 		- @c ILLEGAL_APID if APID does not match.
    */
-  virtual ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) = 0;
+  virtual ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) = 0;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECKIF_H_ */
diff --git a/src/fsfw/tcdistribution/CcsdsPacketChecker.cpp b/src/fsfw/tcdistribution/CcsdsPacketChecker.cpp
new file mode 100644
index 00000000..394f5a57
--- /dev/null
+++ b/src/fsfw/tcdistribution/CcsdsPacketChecker.cpp
@@ -0,0 +1,33 @@
+#include "CcsdsPacketChecker.h"
+
+#include "fsfw/tcdistribution/definitions.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
+
+CcsdsPacketChecker::CcsdsPacketChecker(ccsds::PacketType packetType_, uint8_t ccsdsVersion_)
+    : packetType(packetType_), ccsdsVersion(ccsdsVersion_) {}
+
+ReturnValue_t CcsdsPacketChecker::checkPacket(const SpacePacketReader& currentPacket,
+                                              size_t packetLen) {
+  if (checkApid) {
+    if (currentPacket.getApid() != apid) {
+      return tcdistrib::INVALID_APID;
+    }
+  }
+
+  if (currentPacket.getVersion() != ccsdsVersion) {
+    return tcdistrib::INVALID_CCSDS_VERSION;
+  }
+  if (currentPacket.getPacketType() != packetType) {
+    return tcdistrib::INVALID_PACKET_TYPE;
+  }
+  // This assumes that the getFullPacketLen version uses the space packet data length field
+  if (currentPacket.getFullPacketLen() != packetLen) {
+    return tcdistrib::INCOMPLETE_PACKET;
+  }
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
+void CcsdsPacketChecker::setApidToCheck(uint16_t apid_) {
+  apid = apid_;
+  checkApid = true;
+}
diff --git a/src/fsfw/tcdistribution/CcsdsPacketChecker.h b/src/fsfw/tcdistribution/CcsdsPacketChecker.h
new file mode 100644
index 00000000..f910aac4
--- /dev/null
+++ b/src/fsfw/tcdistribution/CcsdsPacketChecker.h
@@ -0,0 +1,20 @@
+#ifndef FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
+#define FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
+
+#include "CcsdsPacketCheckIF.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h"
+
+class CcsdsPacketChecker : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
+ public:
+  CcsdsPacketChecker(ccsds::PacketType packetType, uint8_t ccsdsVersion = 0b000);
+
+  void setApidToCheck(uint16_t apid);
+  ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) override;
+
+ protected:
+  bool checkApid = false;
+  uint16_t apid = 0;
+  ccsds::PacketType packetType;
+  uint8_t ccsdsVersion;
+};
+#endif  // FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
diff --git a/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.cpp b/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.cpp
deleted file mode 100644
index e1b6337f..00000000
--- a/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "CcsdsPacketCheckerBase.h"
-
-#include "fsfw/tcdistribution/definitions.h"
-#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
-
-CcsdsPacketCheckerBase::CcsdsPacketCheckerBase(uint16_t apid, ccsds::PacketType packetType_,
-                                               uint8_t ccsdsVersion_)
-    : apid(apid), packetType(packetType_), ccsdsVersion(ccsdsVersion_) {}
-
-ReturnValue_t CcsdsPacketCheckerBase::checkPacket(SpacePacketReader* currentPacket,
-                                                  size_t packetLen) {
-  if (currentPacket->getApid() != apid) {
-    return tcdistrib::INVALID_APID;
-  }
-  if (currentPacket->getVersion() != ccsdsVersion) {
-    return tcdistrib::INVALID_CCSDS_VERSION;
-  }
-  if (currentPacket->getPacketType() != packetType) {
-    return tcdistrib::INVALID_PACKET_TYPE;
-  }
-  return HasReturnvaluesIF::RETURN_OK;
-}
\ No newline at end of file
diff --git a/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.h b/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.h
deleted file mode 100644
index 0f7f7e14..00000000
--- a/src/fsfw/tcdistribution/CcsdsPacketCheckerBase.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#ifndef FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
-#define FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
-
-#include "CcsdsPacketCheckIF.h"
-#include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h"
-
-class CcsdsPacketCheckerBase : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
- public:
-  CcsdsPacketCheckerBase(uint16_t apid, ccsds::PacketType packetType, uint8_t ccsdsVersion = 0b000);
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
-
- protected:
-  uint16_t apid;
-  ccsds::PacketType packetType;
-  uint8_t ccsdsVersion;
-};
-#endif  // FSFW_TESTS_CCSDSPACKETCHECKERBASE_H
diff --git a/src/fsfw/tcdistribution/CfdpPacketChecker.h b/src/fsfw/tcdistribution/CfdpPacketChecker.h
index 0838efc2..5ad802f9 100644
--- a/src/fsfw/tcdistribution/CfdpPacketChecker.h
+++ b/src/fsfw/tcdistribution/CfdpPacketChecker.h
@@ -25,7 +25,7 @@ class CfdpPacketChecker : public CcsdsPacketCheckIF, public HasReturnvaluesIF {
    */
   explicit CfdpPacketChecker(uint16_t setApid);
 
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
+  ReturnValue_t checkPacket(const SpacePacketReader& currentPacket, size_t packetLen) override;
 
   [[nodiscard]] uint16_t getApid() const;
 };
diff --git a/src/fsfw/tcdistribution/PusDistributor.cpp b/src/fsfw/tcdistribution/PusDistributor.cpp
index 36bcd34b..85672439 100644
--- a/src/fsfw/tcdistribution/PusDistributor.cpp
+++ b/src/fsfw/tcdistribution/PusDistributor.cpp
@@ -6,7 +6,7 @@
 #include "fsfw/tcdistribution/CCSDSDistributorIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 
-#define PUS_DISTRIBUTOR_DEBUGGING 0
+#define PUS_DISTRIBUTOR_DEBUGGING 1
 
 PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId,
                                CCSDSDistributorIF* distributor, StorageManagerIF* store_)
@@ -20,71 +20,56 @@ PusDistributor::~PusDistributor() = default;
 
 PusDistributor::TcMqMapIter PusDistributor::selectDestination() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
-    store_address_t storeId = this->currentMessage.getStorageId());
-    sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
-               << storeId.packetIndex << std::endl;
+  store_address_t storeId = currentMessage.getStorageId();
+  sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
+             << storeId.packetIndex << std::endl;
 #endif
-    auto queueMapIt = this->queueMap.end();
-    if (reader.isNull()) {
-      return queueMapIt;
-    }
-    // TODO: Need to set the data
-    const uint8_t* packetPtr = nullptr;
-    size_t packetLen = 0;
-    if (store->getData(currentMessage.getStorageId(), &packetPtr, &packetLen) !=
-        HasReturnvaluesIF::RETURN_OK) {
-      return queueMapIt;
-    }
-    reader.setReadOnlyData(packetPtr, packetLen);
-    // this->currentPacket->setStoreAddress(this->currentMessage.getStorageId(), currentPacket);
-    if (reader.getFullData() != nullptr) {
-      tcStatus =
-          checker.checkPacket(dynamic_cast<PacketCheckIF*>(&reader), reader.getFullPacketLen());
-      if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
-#if FSFW_VERBOSE_LEVEL >= 1
-        const char* keyword = "unnamed error";
-        if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) {
-          keyword = "checksum";
-        } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) {
-          keyword = "incorrect primary header";
-        } else if (tcStatus == tcdistrib::INVALID_APID) {
-          keyword = "illegal APID";
-        } else if (tcStatus == tcdistrib::INCORRECT_SECONDARY_HEADER) {
-          keyword = "incorrect secondary header";
-        } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) {
-          keyword = "incomplete packet";
-        }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword
-                     << " error" << std::endl;
-#else
-        sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n",
-                          keyword);
-#endif
-#endif
-      }
-      uint32_t queue_id = reader.getService();
-      queueMapIt = this->queueMap.find(queue_id);
-    } else {
-      tcStatus = PACKET_LOST;
+  auto queueMapIt = queueMap.end();
+  // TODO: Need to set the data
+  const uint8_t* packetPtr = nullptr;
+  size_t packetLen = 0;
+  if (store->getData(currentMessage.getStorageId(), &packetPtr, &packetLen) !=
+      HasReturnvaluesIF::RETURN_OK) {
+    return queueMapIt;
+  }
+  ReturnValue_t result = reader.setReadOnlyData(packetPtr, packetLen);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    tcStatus = PACKET_LOST;
+    return queueMapIt;
+  }
+  // CRC check done by checker
+  result = reader.parseDataWithoutCrcCheck();
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    tcStatus = PACKET_LOST;
+    return queueMapIt;
+  }
+  if (reader.getFullData() != nullptr) {
+    tcStatus = checker.checkPacket(reader, reader.getFullPacketLen());
+    if (tcStatus != HasReturnvaluesIF::RETURN_OK) {
+      checkerFailurePrinter();
     }
+    uint32_t queue_id = reader.getService();
+    queueMapIt = queueMap.find(queue_id);
+  } else {
+    tcStatus = PACKET_LOST;
+  }
 
-    if (queueMapIt == this->queueMap.end()) {
-      tcStatus = DESTINATION_NOT_FOUND;
+  if (queueMapIt == this->queueMap.end()) {
+    tcStatus = DESTINATION_NOT_FOUND;
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
+    sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
 #else
-      sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
+    sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
 #endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif
-    }
+  }
 
-    if (tcStatus != RETURN_OK) {
-      return this->queueMap.end();
-    } else {
-      return queueMapIt;
-    }
+  if (tcStatus != RETURN_OK) {
+    return this->queueMap.end();
+  } else {
+    return queueMapIt;
+  }
 }
 
 ReturnValue_t PusDistributor::registerService(AcceptsTelecommandsIF* service) {
@@ -141,7 +126,7 @@ ReturnValue_t PusDistributor::initialize() {
       return ObjectManagerIF::CHILD_INIT_FAILED;
     }
   }
-  if(ccsdsDistributor == nullptr) {
+  if (ccsdsDistributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "PUSDistributor::initialize: Packet source invalid" << std::endl;
     sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
@@ -151,5 +136,34 @@ ReturnValue_t PusDistributor::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
+  if (verifyChannel == nullptr) {
+    verifyChannel = ObjectManager::instance()->get<VerificationReporterIF>(objects::TC_VERIFICATOR);
+    if (verifyChannel == nullptr) {
+      return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+  }
   return ccsdsDistributor->registerApplication(this);
 }
+
+void PusDistributor::checkerFailurePrinter() const {
+#if FSFW_VERBOSE_LEVEL >= 1
+  const char* keyword = "unnamed error";
+  if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) {
+    keyword = "checksum";
+  } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) {
+    keyword = "incorrect primary header";
+  } else if (tcStatus == tcdistrib::INVALID_APID) {
+    keyword = "illegal APID";
+  } else if (tcStatus == tcdistrib::INCORRECT_SECONDARY_HEADER) {
+    keyword = "incorrect secondary header";
+  } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) {
+    keyword = "incomplete packet";
+  }
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword << " error"
+               << std::endl;
+#else
+  sif::printWarning("PUSDistributor::handlePacket: Packet format invalid, %s error\n", keyword);
+#endif
+#endif
+}
diff --git a/src/fsfw/tcdistribution/PusDistributor.h b/src/fsfw/tcdistribution/PusDistributor.h
index bb0c7a04..7daf5d83 100644
--- a/src/fsfw/tcdistribution/PusDistributor.h
+++ b/src/fsfw/tcdistribution/PusDistributor.h
@@ -73,6 +73,8 @@ class PusDistributor : public TcDistributor, public PUSDistributorIF, public Acc
    * success/failure messages.
    */
   ReturnValue_t callbackAfterSending(ReturnValue_t queueStatus) override;
+
+  void checkerFailurePrinter() const;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_ */
diff --git a/src/fsfw/tcdistribution/PusPacketChecker.cpp b/src/fsfw/tcdistribution/PusPacketChecker.cpp
index 828303b2..8ea8b6a8 100644
--- a/src/fsfw/tcdistribution/PusPacketChecker.cpp
+++ b/src/fsfw/tcdistribution/PusPacketChecker.cpp
@@ -5,40 +5,26 @@
 #include "fsfw/tcdistribution/definitions.h"
 #include "fsfw/tmtcpacket/pus/tc/PusTcReader.h"
 
-PusPacketChecker::PusPacketChecker(uint16_t setApid, ccsds::PacketType packetType_,
+PusPacketChecker::PusPacketChecker(uint16_t apid, ccsds::PacketType packetType_,
                                    ecss::PusVersion pusVersion_)
-    : CcsdsPacketCheckerBase(setApid, packetType_), pusVersion(pusVersion_) {}
+    : pusVersion(pusVersion_), apid(apid) {}
 
-ReturnValue_t PusPacketChecker::checkPacket(PacketCheckIF* pusPacket, size_t packetLen) {
-  if (pusPacket == nullptr) {
-    return RETURN_FAILED;
-  }
+ReturnValue_t PusPacketChecker::checkPacket(const PusTcReader& pusPacket, size_t packetLen) {
   // Other primary header fields are checked by base class
-  if (not pusPacket->hasSecHeader()) {
+  if (not pusPacket.hasSecHeader()) {
     return tcdistrib::INVALID_SEC_HEADER_FIELD;
   }
-  uint16_t calculated_crc =
-      CRC::crc16ccitt(pusPacket->getFullData(), pusPacket->getFullPacketLen());
+  uint16_t calculated_crc = CRC::crc16ccitt(pusPacket.getFullData(), pusPacket.getFullPacketLen());
   if (calculated_crc != 0) {
     return tcdistrib::INCORRECT_CHECKSUM;
   }
-
-  // This assumes that the getFullPacketLen version uses the space packet data length field
-  if (pusPacket->getFullPacketLen() != packetLen) {
-    return tcdistrib::INCOMPLETE_PACKET;
+  if (pusPacket.getApid() != apid) {
+    return tcdistrib::INVALID_APID;
   }
-  if (pusPacket->getPusVersion() != pusVersion) {
+  if (pusPacket.getPusVersion() != pusVersion) {
     return tcdistrib::INVALID_PUS_VERSION;
   }
-  return RETURN_OK;
+  return HasReturnvaluesIF::RETURN_OK;
 }
 
 uint16_t PusPacketChecker::getApid() const { return apid; }
-
-ReturnValue_t PusPacketChecker::checkPacket(SpacePacketReader* currentPacket, size_t packetLen) {
-  ReturnValue_t result = CcsdsPacketCheckerBase::checkPacket(currentPacket, packetLen);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    return result;
-  }
-  return checkPacket(dynamic_cast<PacketCheckIF*>(currentPacket), packetLen);
-}
diff --git a/src/fsfw/tcdistribution/PusPacketChecker.h b/src/fsfw/tcdistribution/PusPacketChecker.h
index 431ee5a8..ee56e1f8 100644
--- a/src/fsfw/tcdistribution/PusPacketChecker.h
+++ b/src/fsfw/tcdistribution/PusPacketChecker.h
@@ -1,38 +1,34 @@
 #ifndef FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_
 #define FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_
 
-#include "CcsdsPacketCheckIF.h"
-#include "CcsdsPacketCheckerBase.h"
 #include "fsfw/FSFW.h"
 #include "fsfw/returnvalues/HasReturnvaluesIF.h"
 #include "fsfw/tmtcpacket/pus/defs.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 
-class PacketCheckIF : public ReadablePacketIF, public PusIF {};
-
 /**
  * This class performs a formal packet check for incoming PUS Telecommand Packets.
  * Currently, it only checks if the APID and CRC are correct.
  * @ingroup tc_distribution
  */
-class PusPacketChecker : public CcsdsPacketCheckerBase {
+class PusPacketChecker {
  public:
   /**
    * The constructor only sets the APID attribute.
    * @param set_apid The APID to set.
    */
-  PusPacketChecker(uint16_t setApid, ccsds::PacketType packetType,
-                   ecss::PusVersion = ecss::PusVersion::PUS_C);
+  explicit PusPacketChecker(uint16_t apid, ccsds::PacketType packetType,
+                            ecss::PusVersion = ecss::PusVersion::PUS_C);
 
-  ReturnValue_t checkPacket(PacketCheckIF* currentPacket, size_t packetLen);
+  ReturnValue_t checkPacket(const PusTcReader& currentPacket, size_t packetLen);
 
   [[nodiscard]] uint16_t getApid() const;
 
  protected:
   ecss::PusVersion pusVersion;
+  uint16_t apid;
 
  private:
-  ReturnValue_t checkPacket(SpacePacketReader* currentPacket, size_t packetLen) override;
 };
 
 #endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECKPUS_H_ */
diff --git a/src/fsfw/tcdistribution/TcDistributor.cpp b/src/fsfw/tcdistribution/TcDistributor.cpp
index 600b3ca9..4ec85ed9 100644
--- a/src/fsfw/tcdistribution/TcDistributor.cpp
+++ b/src/fsfw/tcdistribution/TcDistributor.cpp
@@ -11,25 +11,24 @@ TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) {
 TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
 
 ReturnValue_t TcDistributor::performOperation(uint8_t opCode) {
-  ReturnValue_t status = RETURN_OK;
+  ReturnValue_t status;
   for (status = tcQueue->receiveMessage(&currentMessage); status == RETURN_OK;
        status = tcQueue->receiveMessage(&currentMessage)) {
     status = handlePacket();
   }
   if (status == MessageQueueIF::EMPTY) {
     return RETURN_OK;
-  } else {
-    return status;
   }
+  return status;
 }
 
 ReturnValue_t TcDistributor::handlePacket() {
-  auto queueMapIt = this->selectDestination();
-  ReturnValue_t returnValue = RETURN_FAILED;
-  if (queueMapIt != this->queueMap.end()) {
-    returnValue = this->tcQueue->sendMessage(queueMapIt->second, &this->currentMessage);
+  auto queueMapIt = selectDestination();
+  ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED;
+  if (queueMapIt != queueMap.end()) {
+    result = tcQueue->sendMessage(queueMapIt->second, &currentMessage);
   }
-  return this->callbackAfterSending(returnValue);
+  return callbackAfterSending(result);
 }
 
 void TcDistributor::print() {
diff --git a/src/fsfw/tcdistribution/TcPacketCheckCFDP.cpp b/src/fsfw/tcdistribution/TcPacketCheckCFDP.cpp
index 555c85b4..9a0b89bd 100644
--- a/src/fsfw/tcdistribution/TcPacketCheckCFDP.cpp
+++ b/src/fsfw/tcdistribution/TcPacketCheckCFDP.cpp
@@ -3,7 +3,8 @@
 
 CfdpPacketChecker::CfdpPacketChecker(uint16_t setApid) : apid(setApid) {}
 
-ReturnValue_t CfdpPacketChecker::checkPacket(SpacePacketReader* currentPacket, size_t packetLen) {
+ReturnValue_t CfdpPacketChecker::checkPacket(const SpacePacketReader& currentPacket,
+                                             size_t packetLen) {
   return RETURN_OK;
 }
 
diff --git a/src/fsfw/timemanager/TimeStamperIF.h b/src/fsfw/timemanager/TimeStamperIF.h
index df680b6c..44cf0212 100644
--- a/src/fsfw/timemanager/TimeStamperIF.h
+++ b/src/fsfw/timemanager/TimeStamperIF.h
@@ -26,6 +26,8 @@ class TimeStamperIF : public SerializeIF {
 
   virtual ReturnValue_t addTimeStamp(uint8_t* buffer, uint8_t maxSize) = 0;
   ~TimeStamperIF() override = default;
+
+ protected:
 };
 
 #endif /* FSFW_TIMEMANAGER_TIMESTAMPERIF_H_ */
diff --git a/src/fsfw/tmtcpacket/ReadablePacketIF.h b/src/fsfw/tmtcpacket/ReadablePacketIF.h
index bbffbe10..4f2f09e8 100644
--- a/src/fsfw/tmtcpacket/ReadablePacketIF.h
+++ b/src/fsfw/tmtcpacket/ReadablePacketIF.h
@@ -10,6 +10,6 @@ class ReadablePacketIF {
   /**
    * This method returns a raw uint8_t pointer to the readable data
    */
-  virtual const uint8_t* getFullData() = 0;
+  [[nodiscard]] virtual const uint8_t* getFullData() const = 0;
 };
 #endif  // FSFW_TMTCPACKET_SPACEPACKETREADERIF_H
diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp
index 76f92107..b6f03de9 100644
--- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp
+++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp
@@ -42,7 +42,7 @@ ReturnValue_t SpacePacketReader::setInternalFields(const uint8_t* data, size_t m
   return checkSize();
 }
 
-const uint8_t* SpacePacketReader::getFullData() {
+const uint8_t* SpacePacketReader::getFullData() const {
   return reinterpret_cast<const uint8_t*>(spHeader);
 }
 size_t SpacePacketReader::getBufSize() const { return bufSize; }
diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h
index 0d075bae..dbffa7bf 100644
--- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h
+++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h
@@ -66,7 +66,7 @@ class SpacePacketReader : public SpacePacketIF,
   [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override;
   [[nodiscard]] uint16_t getPacketDataLen() const override;
 
-  const uint8_t* getFullData() override;
+  [[nodiscard]] const uint8_t* getFullData() const override;
 
   // Helper methods:
   [[nodiscard]] ReturnValue_t checkSize() const;
diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp
index f50affa7..31bb1011 100644
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp
@@ -67,7 +67,7 @@ uint16_t PusTcReader::getPacketSeqCtrlRaw() const { return spReader.getPacketSeq
 
 uint16_t PusTcReader::getPacketDataLen() const { return spReader.getPacketDataLen(); }
 
-const uint8_t* PusTcReader::getFullData() { return pointers.spHeaderStart; }
+const uint8_t* PusTcReader::getFullData() const { return pointers.spHeaderStart; }
 
 ReturnValue_t PusTcReader::setData(uint8_t* pData, size_t size_, void* args) {
   pointers.spHeaderStart = pData;
diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h
index 1b9eea49..03af43a9 100644
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h
@@ -54,7 +54,7 @@ class PusTcReader : public PusTcIF,
   [[nodiscard]] uint8_t getSubService() const override;
   [[nodiscard]] uint16_t getSourceId() const override;
   [[nodiscard]] uint16_t getErrorControl() const;
-  const uint8_t* getFullData() override;
+  const uint8_t* getFullData() const override;
 
   ReturnValue_t setReadOnlyData(const uint8_t* data, size_t size);
   [[nodiscard]] const uint8_t* getUserData() const override;
diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
index 850eb078..348308d1 100644
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
@@ -12,13 +12,9 @@ PusTmCreator::PusTmCreator(SpacePacketParams initSpParams, PusTmParams initPusPa
 
 PusTmCreator::PusTmCreator() { setup(); }
 
-void PusTmCreator::disableCrcCalculation() {
-  calculateCrcOnSerialization = false;
-}
+void PusTmCreator::disableCrcCalculation() { calculateCrcOnSerialization = false; }
 
-void PusTmCreator::enableCrcCalculation() {
-  calculateCrcOnSerialization = true;
-}
+void PusTmCreator::enableCrcCalculation() { calculateCrcOnSerialization = true; }
 
 uint16_t PusTmCreator::getPacketIdRaw() const { return spCreator.getPacketIdRaw(); }
 
@@ -74,10 +70,13 @@ ReturnValue_t PusTmCreator::serialize(uint8_t** buffer, size_t* size, size_t max
   if (result != HasReturnvaluesIF::RETURN_OK) {
     return result;
   }
-  result = pusParams.secHeader.timeStamper->serialize(buffer, size, maxSize, streamEndianness);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    return result;
+  if (getTimestamper() != nullptr) {
+    result = pusParams.secHeader.timeStamper->serialize(buffer, size, maxSize, streamEndianness);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
   }
+
   if (pusParams.dataWrapper.type == ecss::DataTypes::RAW and
       pusParams.dataWrapper.dataUnion.raw.data != nullptr) {
     std::memcpy(*buffer, pusParams.dataWrapper.dataUnion.raw.data, userDataLen);
diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp
index 0dfab4ce..a3dbb7b0 100644
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.cpp
@@ -15,7 +15,7 @@ ReturnValue_t PusTmReader::parseDataWithCrcCheck() { return parseData(true); }
 
 ReturnValue_t PusTmReader::parseDataWithoutCrcCheck() { return parseData(false); }
 
-const uint8_t *PusTmReader::getFullData() { return spReader.getFullData(); }
+const uint8_t *PusTmReader::getFullData() const { return spReader.getFullData(); }
 
 ReturnValue_t PusTmReader::setReadOnlyData(const uint8_t *data, size_t size) {
   return setData(const_cast<uint8_t *>(data), size, nullptr);
diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h
index 780b3e5f..fdad1b6c 100644
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmReader.h
@@ -19,7 +19,7 @@ class PusTmReader : public PusTmIF,
 
   ReturnValue_t parseDataWithoutCrcCheck();
   ReturnValue_t parseDataWithCrcCheck();
-  const uint8_t* getFullData() override;
+  const uint8_t* getFullData() const override;
 
   explicit operator bool() const;
   [[nodiscard]] bool isNull() const;
diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
index 0be7c3c8..08ba599a 100644
--- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
@@ -103,6 +103,12 @@ ReturnValue_t CommandingServiceBase::initialize() {
   // Generally, all TM packets will pass through a layer where the sequence count is set.
   // This avoids duplicate calculation of the CRC16
   tmStoreHelper.disableCrcCalculation();
+  if (tmTimeStamper == nullptr) {
+    auto timerStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
+    if (timerStamper != nullptr) {
+      tmStoreHelper.setTimeStamper(*timerStamper);
+    }
+  }
 
   if (errReporter == nullptr) {
     errReporter =
diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.h b/src/fsfw/tmtcservices/CommandingServiceBase.h
index 8017ecc8..f9070d17 100644
--- a/src/fsfw/tmtcservices/CommandingServiceBase.h
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.h
@@ -267,6 +267,7 @@ class CommandingServiceBase : public SystemObject,
   MessageQueueIF* commandQueue = nullptr;
   MessageQueueIF* requestQueue = nullptr;
 
+  TimeStamperIF* tmTimeStamper = nullptr;
   VerificationReporterIF* verificationReporter;
 
   InternalErrorReporterIF* errReporter = nullptr;
diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp
index 094a4156..d3d23ed7 100644
--- a/src/fsfw/tmtcservices/PusServiceBase.cpp
+++ b/src/fsfw/tmtcservices/PusServiceBase.cpp
@@ -4,6 +4,7 @@
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tcdistribution/PUSDistributorIF.h"
+#include "fsfw/timemanager/CdsShortTimeStamper.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
@@ -163,6 +164,12 @@ void PusServiceBase::initializeTmSendHelper(TmSendHelper& tmSendHelper) {
 
 void PusServiceBase::initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const {
   tmStoreHelper.setApid(psbParams.apid);
+  if (psbParams.timeStamper == nullptr) {
+    auto timerStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
+    if (timerStamper != nullptr) {
+      tmStoreHelper.setTimeStamper(*timerStamper);
+    }
+  }
 }
 
 void PusServiceBase::setVerificationReporter(VerificationReporterIF& reporter) {
diff --git a/src/fsfw/tmtcservices/PusServiceBase.h b/src/fsfw/tmtcservices/PusServiceBase.h
index 9c75870a..43a2c0d4 100644
--- a/src/fsfw/tmtcservices/PusServiceBase.h
+++ b/src/fsfw/tmtcservices/PusServiceBase.h
@@ -65,6 +65,7 @@ struct PsbParams {
    * register itself at that object.
    */
   PUSDistributorIF* pusDistributor = nullptr;
+  TimeStamperIF* timeStamper = nullptr;
 };
 
 namespace Factory {
@@ -133,7 +134,9 @@ class PusServiceBase : public ExecutableObjectIF,
    */
   void initializeTmSendHelper(TmSendHelper& tmSendHelper);
   /**
-   * Helper methods if the implementing class wants to store telemetry
+   * Helper methods if the implementing class wants to store telemetry. It will set the correct APID
+   * and it will also attempt to set a valid time stamper. If the manually specified time stamper is
+   * null, it will attempt to find a suitable one using @objects::TIME_STAMPER
    * @param tmSendHelper
    */
   void initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const;
diff --git a/src/fsfw/tmtcservices/TmSendHelper.cpp b/src/fsfw/tmtcservices/TmSendHelper.cpp
index ddbb86ba..a8e23186 100644
--- a/src/fsfw/tmtcservices/TmSendHelper.cpp
+++ b/src/fsfw/tmtcservices/TmSendHelper.cpp
@@ -5,8 +5,10 @@
 TmSendHelper::TmSendHelper() = default;
 
 TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &reporter,
-                           MessageQueueId_t tmtcMsgDest)
-    : defaultDest(tmtcMsgDest), queue(&queue), errReporter(&reporter) {}
+                           MessageQueueId_t defaultDest)
+    : queue(&queue), errReporter(&reporter) {
+  queue.setDefaultDestination(defaultDest);
+}
 
 TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &reporter)
     : queue(&queue), errReporter(&reporter) {}
@@ -14,11 +16,26 @@ TmSendHelper::TmSendHelper(MessageQueueIF &queue, InternalErrorReporterIF &repor
 TmSendHelper::TmSendHelper(InternalErrorReporterIF &reporter) : errReporter(&reporter) {}
 
 ReturnValue_t TmSendHelper::sendPacket(const store_address_t &storeId) {
+  return sendPacket(getDefaultDestination(), storeId);
+}
+
+void TmSendHelper::setDefaultDestination(MessageQueueId_t msgDest) {
+  if (queue != nullptr) {
+    queue->setDefaultDestination(msgDest);
+  };
+}
+
+void TmSendHelper::setInternalErrorReporter(InternalErrorReporterIF &reporter) {
+  errReporter = &reporter;
+}
+void TmSendHelper::setMsgQueue(MessageQueueIF &queue_) { queue = &queue_; }
+
+ReturnValue_t TmSendHelper::sendPacket(MessageQueueId_t dest, const store_address_t &storeId) {
   if (queue == nullptr) {
     return HasReturnvaluesIF::RETURN_FAILED;
   }
   TmTcMessage message(storeId);
-  ReturnValue_t result = queue->sendMessage(defaultDest, &message, ignoreFault);
+  ReturnValue_t result = queue->sendMessage(dest, &message, ignoreFault);
   if (result != HasReturnvaluesIF::RETURN_OK) {
     if (errReporter != nullptr and not ignoreFault) {
       errReporter->lostTm();
@@ -28,19 +45,12 @@ ReturnValue_t TmSendHelper::sendPacket(const store_address_t &storeId) {
   return result;
 }
 
-void TmSendHelper::setDefaultDestination(MessageQueueId_t msgDest) { defaultDest = msgDest; }
-
-void TmSendHelper::setInternalErrorReporter(InternalErrorReporterIF &reporter) {
-  errReporter = &reporter;
+MessageQueueId_t TmSendHelper::getDefaultDestination() const {
+  if (queue != nullptr) {
+    return queue->getDefaultDestination();
+  };
+  return MessageQueueIF::NO_QUEUE;
 }
-void TmSendHelper::setMsgQueue(MessageQueueIF &queue_) { queue = &queue_; }
-
-ReturnValue_t TmSendHelper::sendPacket(MessageQueueId_t dest, const store_address_t &storeId) {
-  setDefaultDestination(dest);
-  return sendPacket(storeId);
-}
-
-MessageQueueId_t TmSendHelper::getDefaultDestination() const { return defaultDest; }
 
 bool TmSendHelper::areFaultsIgnored() const { return ignoreFault; }
 void TmSendHelper::ignoreFaults() { ignoreFault = true; }
diff --git a/src/fsfw/tmtcservices/TmSendHelper.h b/src/fsfw/tmtcservices/TmSendHelper.h
index a9feab75..5d39d5d1 100644
--- a/src/fsfw/tmtcservices/TmSendHelper.h
+++ b/src/fsfw/tmtcservices/TmSendHelper.h
@@ -29,7 +29,6 @@ class TmSendHelper {
   ReturnValue_t sendPacket(const store_address_t& storeId);
 
  private:
-  MessageQueueId_t defaultDest = MessageQueueIF::NO_QUEUE;
   bool ignoreFault = false;
   MessageQueueIF* queue = nullptr;
   InternalErrorReporterIF* errReporter = nullptr;