switched to new tmtc stack API

2022-07-20 22:21:15 +02:00 · 2022-07-20 22:21:15 +02:00 · be35bd53a6
parent 9860061fc6
commit be35bd53a6
51 changed files with 753 additions and 374 deletions
--- a/src/fsfw/monitoring/MonitoringMessageContent.h
+++ b/src/fsfw/monitoring/MonitoringMessageContent.h
@ -34,7 +34,7 @@ class MonitoringReportContent : public SerialLinkedListAdapter<SerializeIF> {
  SerializeElement<T> limitValue;
  SerializeElement<ReturnValue_t> oldState;
  SerializeElement<ReturnValue_t> newState;
-  uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE] = {};
+  uint8_t rawTimestamp[TimeStamperIF::MAXIMUM_TIMESTAMP_LEN] = {};
  SerializeElement<SerialBufferAdapter<uint8_t>> timestampSerializer;
  TimeStamperIF* timeStamper;
  MonitoringReportContent()
@ -47,7 +47,7 @@ class MonitoringReportContent : public SerialLinkedListAdapter<SerializeIF> {
        oldState(0),
        newState(0),
        timestampSerializer(rawTimestamp, sizeof(rawTimestamp)),
-        timeStamper(NULL) {
+        timeStamper(nullptr) {
    setAllNext();
  }
  MonitoringReportContent(gp_id_t globalPoolId, T value, T limitValue, ReturnValue_t oldState,
@ -61,7 +61,7 @@ class MonitoringReportContent : public SerialLinkedListAdapter<SerializeIF> {
        oldState(oldState),
        newState(newState),
        timestampSerializer(rawTimestamp, sizeof(rawTimestamp)),
-        timeStamper(NULL) {
+        timeStamper(nullptr) {
    setAllNext();
    if (checkAndSetStamper()) {
      timeStamper->addTimeStamp(rawTimestamp, sizeof(rawTimestamp));
--- a/src/fsfw/osal/host/MessageQueue.cpp
+++ b/src/fsfw/osal/host/MessageQueue.cpp
@ -84,7 +84,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
           message->getMaximumMessageSize());
  } else {
    if (not ignoreFault) {
-      InternalErrorReporterIF* internalErrorReporter =
+      auto* internalErrorReporter =
          ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
      if (internalErrorReporter != nullptr) {
        internalErrorReporter->queueMessageNotSent();
--- a/src/fsfw/pus/Service17Test.cpp
+++ b/src/fsfw/pus/Service17Test.cpp
@ -1,15 +1,14 @@
 #include "fsfw/pus/Service17Test.h"

 #include "fsfw/FSFW.h"
+#include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/objectmanager/SystemObject.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
-#include "fsfw/tmtcpacket/pus/tm/TmPacketStored.h"
+#include "fsfw/tmtcservices/sendAndStoreHelper.h"

-Service17Test::Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                             StorageManagerIF* tmStore, StorageManagerIF* ipcStore,
-                             InternalErrorReporterIF* errReporter)
-    : PusServiceBase(objectId, apid, serviceId, ipcStore),
-      helper(tmStore, MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, errReporter),
+Service17Test::Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId)
+    : PusServiceBase(objectId, apid, serviceId),
+      storeHelper(apid, nullptr),
+      sendHelper(nullptr),
      packetSubCounter(0) {}

 Service17Test::~Service17Test() = default;
@ -17,15 +16,13 @@ Service17Test::~Service17Test() = default;
 ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
  switch (subservice) {
    case Subservice::CONNECTION_TEST: {
-      helper.preparePacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT, packetSubCounter);
-      helper.sendPacket();
-      return HasReturnvaluesIF::RETURN_OK;
+      storeHelper.preparePacket(serviceId, Subservice::CONNECTION_TEST_REPORT, packetSubCounter);
+      return tm::storeAndSendTmPacket(storeHelper, sendHelper);
    }
    case Subservice::EVENT_TRIGGER_TEST: {
-      helper.preparePacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
-      helper.sendPacket();
+      storeHelper.preparePacket(serviceId, Subservice::CONNECTION_TEST_REPORT, packetSubCounter++);
      triggerEvent(TEST, 1234, 5678);
-      return RETURN_OK;
+      return tm::storeAndSendTmPacket(storeHelper, sendHelper);
    }
    default:
      return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@ -39,6 +36,17 @@ ReturnValue_t Service17Test::initialize() {
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
-  helper.setMsgDestination(requestQueue->getDefaultDestination());
-  helper.setMsgSource(requestQueue->getId());
-  if (tm) }
+  initializeTmHelpers(sendHelper, storeHelper);
+  if (storeHelper.getTmStore() == nullptr) {
+    auto* tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+    if (tmStore == nullptr) {
+      return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+    storeHelper.setTmStore(tmStore);
+  }
+  return result;
+}
+
+void Service17Test::setCustomTmStore(StorageManagerIF* tmStore_) {
+  storeHelper.setTmStore(tmStore_);
+}
--- a/src/fsfw/pus/Service17Test.h
+++ b/src/fsfw/pus/Service17Test.h
@ -33,16 +33,19 @@ class Service17Test : public PusServiceBase {
    EVENT_TRIGGER_TEST = 128,
  };

-  Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                InternalErrorReporterIF* errReporter, StorageManagerIF* tmStore = nullptr,
-                StorageManagerIF* ipcStore = nullptr);
+  Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId);
+
+  void setCustomTmStore(StorageManagerIF* tmStore);
+
  ~Service17Test() override;
  ReturnValue_t handleRequest(uint8_t subservice) override;
  ReturnValue_t performService() override;
  ReturnValue_t initialize() override;

 protected:
-  TmStoreHelper helper;
+  TmStoreHelper storeHelper;
+  TmSendHelper sendHelper;
+
  uint16_t packetSubCounter = 0;
 };

--- a/src/fsfw/pus/Service1TelecommandVerification.cpp
+++ b/src/fsfw/pus/Service1TelecommandVerification.cpp
@ -3,10 +3,10 @@
 #include "fsfw/ipc/QueueFactory.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/pus/servicepackets/Service1Packets.h"
-#include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcpacket/pus/tm/TmPacketStored.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
+#include "fsfw/tmtcservices/sendAndStoreHelper.h"

 Service1TelecommandVerification::Service1TelecommandVerification(object_id_t objectId,
                                                                 uint16_t apid, uint8_t serviceId,
@ -15,7 +15,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(object_id_t obj
    : SystemObject(objectId),
      apid(apid),
      serviceId(serviceId),
-      targetDestination(targetDestination) {
+      targetDestination(targetDestination),
+      sendHelper(nullptr),
+      storeHelper(apid, nullptr) {
  tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }

@ -67,32 +69,23 @@ ReturnValue_t Service1TelecommandVerification::generateFailureReport(
  FailureReport report(message->getReportId(), message->getTcPacketId(),
                       message->getTcSequenceControl(), message->getStep(), message->getErrorCode(),
                       message->getParameter1(), message->getParameter2());
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(), packetSubCounter++, &report);
-#else
-  TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(), packetSubCounter++, &report);
-#endif
-  ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(), tmQueue->getId());
-  return result;
+  storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  storeHelper.setSourceDataSerializable(&report);
+  return tm::storeAndSendTmPacket(storeHelper, sendHelper);
 }

 ReturnValue_t Service1TelecommandVerification::generateSuccessReport(
    PusVerificationMessage* message) {
  SuccessReport report(message->getReportId(), message->getTcPacketId(),
                       message->getTcSequenceControl(), message->getStep());
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacket(apid, serviceId, message->getReportId(), packetSubCounter++, &report);
-#else
-  TmPacketStoredPusC tmPacket(apid, serviceId, message->getReportId(), packetSubCounter++, &report);
-#endif
-  ReturnValue_t result = tmPacket.sendPacket(tmQueue->getDefaultDestination(), tmQueue->getId());
-  return result;
+  storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  storeHelper.setSourceDataSerializable(&report);
+  return tm::storeAndSendTmPacket(storeHelper, sendHelper);
 }

 ReturnValue_t Service1TelecommandVerification::initialize() {
  // Get target object for TC verification messages
-  AcceptsTelemetryIF* funnel =
-      ObjectManager::instance()->get<AcceptsTelemetryIF>(targetDestination);
+  auto* funnel = ObjectManager::instance()->get<AcceptsTelemetryIF>(targetDestination);
  if (funnel == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "Service1TelecommandVerification::initialize: Specified"
@ -103,5 +96,12 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
  tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
+  if (tmStore == nullptr) {
+    tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+    if (tmStore == nullptr) {
+      return ObjectManager::CHILD_INIT_FAILED;
+    }
+    storeHelper.setTmStore(tmStore);
+  }
  return SystemObject::initialize();
 }
--- a/src/fsfw/pus/Service1TelecommandVerification.h
+++ b/src/fsfw/pus/Service1TelecommandVerification.h
@ -7,6 +7,8 @@
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tmtcservices/AcceptsVerifyMessageIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
+#include "fsfw/tmtcservices/TmSendHelper.h"
+#include "fsfw/tmtcservices/TmStoreHelper.h"

 /**
 * @brief Verify TC acceptance, start, progress and execution.
@ -45,13 +47,13 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,

  Service1TelecommandVerification(object_id_t objectId, uint16_t apid, uint8_t serviceId,
                                  object_id_t targetDestination, uint16_t messageQueueDepth);
-  virtual ~Service1TelecommandVerification();
+  ~Service1TelecommandVerification() override;

  /**
   *
   * @return ID of Verification Queue
   */
-  virtual MessageQueueId_t getVerificationQueue() override;
+  MessageQueueId_t getVerificationQueue() override;

  /**
   * Performs the service periodically as specified in init_mission().
@ -59,7 +61,7 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,
   * @param operationCode
   * @return
   */
-  ReturnValue_t performOperation(uint8_t operationCode = 0) override;
+  ReturnValue_t performOperation(uint8_t operationCode) override;

  /**
   * Initializes the destination for TC verification messages and initializes
@ -80,6 +82,9 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,

  uint16_t packetSubCounter = 0;

+  TmSendHelper sendHelper;
+  TmStoreHelper storeHelper;
+  StorageManagerIF* tmStore = nullptr;
  MessageQueueIF* tmQueue = nullptr;

  enum class Subservice : uint8_t {
--- a/src/fsfw/pus/Service20ParameterManagement.cpp
+++ b/src/fsfw/pus/Service20ParameterManagement.cpp
@ -137,7 +137,7 @@ ReturnValue_t Service20ParameterManagement::prepareLoadCommand(CommandMessage* m
  if (parameterDataLen == 0) {
    return CommandingServiceBase::INVALID_TC;
  }
-  ReturnValue_t result = IPCStore->getFreeElement(&storeAddress, parameterDataLen, &storePointer);
+  ReturnValue_t result = ipcStore->getFreeElement(&storeAddress, parameterDataLen, &storePointer);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
@ -169,7 +169,7 @@ ReturnValue_t Service20ParameterManagement::handleReply(const CommandMessage* re

  switch (replyId) {
    case ParameterMessage::REPLY_PARAMETER_DUMP: {
-      ConstAccessorPair parameterData = IPCStore->getData(ParameterMessage::getStoreId(reply));
+      ConstAccessorPair parameterData = ipcStore->getData(ParameterMessage::getStoreId(reply));
      if (parameterData.first != HasReturnvaluesIF::RETURN_OK) {
        return HasReturnvaluesIF::RETURN_FAILED;
      }
--- a/src/fsfw/pus/Service2DeviceAccess.cpp
+++ b/src/fsfw/pus/Service2DeviceAccess.cpp
@ -75,7 +75,7 @@ ReturnValue_t Service2DeviceAccess::prepareRawCommand(CommandMessage* messageToS
  // store command into the Inter Process Communication Store
  store_address_t storeAddress;
  ReturnValue_t result =
-      IPCStore->addData(&storeAddress, RawCommand.getCommand(), RawCommand.getCommandSize());
+      ipcStore->addData(&storeAddress, RawCommand.getCommand(), RawCommand.getCommandSize());
  DeviceHandlerMessage::setDeviceHandlerRawCommandMessage(messageToSet, storeAddress);
  return result;
 }
@ -135,7 +135,7 @@ void Service2DeviceAccess::sendWiretappingTm(CommandMessage* reply, uint8_t subs
  store_address_t storeAddress = DeviceHandlerMessage::getStoreAddress(reply);
  const uint8_t* data = nullptr;
  size_t size = 0;
-  ReturnValue_t result = IPCStore->getData(storeAddress, &data, &size);
+  ReturnValue_t result = ipcStore->getData(storeAddress, &data, &size);
  if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "Service2DeviceAccess::sendWiretappingTm: Data Lost in "
@ -147,10 +147,8 @@ void Service2DeviceAccess::sendWiretappingTm(CommandMessage* reply, uint8_t subs

  // Init our dummy packet and correct endianness of object ID before
  // sending it back.
-  WiretappingPacket TmPacket(DeviceHandlerMessage::getDeviceObjectId(reply), data);
-  TmPacket.objectId = EndianConverter::convertBigEndian(TmPacket.objectId);
-  sendTmPacket(subservice, TmPacket.data, size, reinterpret_cast<uint8_t*>(&TmPacket.objectId),
-               sizeof(TmPacket.objectId));
+  WiretappingPacket tmPacket(DeviceHandlerMessage::getDeviceObjectId(reply), data);
+  sendTmPacket(subservice, tmPacket.objectId, tmPacket.data, size);
 }

 MessageQueueId_t Service2DeviceAccess::getDeviceQueue() { return commandQueue->getId(); }
--- a/src/fsfw/pus/Service3Housekeeping.cpp
+++ b/src/fsfw/pus/Service3Housekeeping.cpp
@ -284,14 +284,13 @@ ReturnValue_t Service3Housekeeping::generateHkReply(const CommandMessage* hkMess
  store_address_t storeId;

  sid_t sid = HousekeepingMessage::getHkDataReply(hkMessage, &storeId);
-  auto resultPair = IPCStore->getData(storeId);
+  auto resultPair = ipcStore->getData(storeId);
  if (resultPair.first != HasReturnvaluesIF::RETURN_OK) {
    return resultPair.first;
  }

  HkPacket hkPacket(sid, resultPair.second.data(), resultPair.second.size());
-  return sendTmPacket(static_cast<uint8_t>(subserviceId), hkPacket.hkData, hkPacket.hkSize, nullptr,
-                      0);
+  return sendTmPacket(static_cast<uint8_t>(subserviceId), hkPacket.hkData, hkPacket.hkSize);
 }

 sid_t Service3Housekeeping::buildSid(object_id_t objectId, const uint8_t** tcData,
--- a/src/fsfw/pus/Service5EventReporting.cpp
+++ b/src/fsfw/pus/Service5EventReporting.cpp
@ -6,11 +6,14 @@
 #include "fsfw/pus/servicepackets/Service5Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/tmtcpacket/pus/tm/TmPacketStored.h"
+#include "fsfw/tmtcservices/sendAndStoreHelper.h"

 Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t apid,
                                               uint8_t serviceId, size_t maxNumberReportsPerCycle,
                                               uint32_t messageQueueDepth)
    : PusServiceBase(objectId, apid, serviceId),
+      sendHelper(nullptr),
+      storeHelper(apid, nullptr),
      maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
  eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
@ -45,15 +48,9 @@ ReturnValue_t Service5EventReporting::performService() {
 ReturnValue_t Service5EventReporting::generateEventReport(EventMessage message) {
  EventReport report(message.getEventId(), message.getReporter(), message.getParameter1(),
                     message.getParameter2());
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
-                              message.getSeverity(), packetSubCounter++, &report);
-#else
-  TmPacketStoredPusC tmPacket(PusServiceBase::apid, PusServiceBase::serviceId,
-                              message.getSeverity(), packetSubCounter++, &report);
-#endif
-  ReturnValue_t result =
-      tmPacket.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
+  storeHelper.preparePacket(serviceId, message.getSeverity(), packetSubCounter);
+  storeHelper.setSourceDataSerializable(&report);
+  ReturnValue_t result = tm::storeAndSendTmPacket(storeHelper, sendHelper);
  if (result != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "Service5EventReporting::generateEventReport: "
@ -64,6 +61,8 @@ ReturnValue_t Service5EventReporting::generateEventReport(EventMessage message)
        "Service5EventReporting::generateEventReport: "
        "Could not send TM packet\n");
 #endif
+  } else {
+    packetSubCounter++;
  }
  return result;
 }
@ -86,14 +85,18 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) {
 // In addition to the default PUSServiceBase initialization, this service needs
 // to be registered to the event manager to listen for events.
 ReturnValue_t Service5EventReporting::initialize() {
+  ReturnValue_t result = PusServiceBase::initialize();
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
  auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
  if (manager == nullptr) {
    return RETURN_FAILED;
  }
  // register Service 5 as listener for events
-  ReturnValue_t result = manager->registerListener(eventQueue->getId(), true);
+  result = manager->registerListener(eventQueue->getId(), true);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
-  return PusServiceBase::initialize();
+  initializeTmHelpers(sendHelper, storeHelper);
 }
--- a/src/fsfw/pus/Service5EventReporting.h
+++ b/src/fsfw/pus/Service5EventReporting.h
@ -42,7 +42,7 @@ class Service5EventReporting : public PusServiceBase {
 public:
  Service5EventReporting(object_id_t objectId, uint16_t apid, uint8_t serviceId,
                         size_t maxNumberReportsPerCycle = 10, uint32_t messageQueueDepth = 10);
-  virtual ~Service5EventReporting();
+  ~Service5EventReporting() override;

  /***
   * Check for events and generate event reports if required.
@ -77,6 +77,8 @@ class Service5EventReporting : public PusServiceBase {
  uint16_t packetSubCounter = 0;
  MessageQueueIF* eventQueue = nullptr;
  bool enableEventReport = true;
+  TmSendHelper sendHelper;
+  TmStoreHelper storeHelper;
  const uint8_t maxNumberReportsPerCycle;

  ReturnValue_t generateEventReport(EventMessage message);
--- a/src/fsfw/pus/Service8FunctionManagement.cpp
+++ b/src/fsfw/pus/Service8FunctionManagement.cpp
@ -78,7 +78,7 @@ ReturnValue_t Service8FunctionManagement::prepareDirectCommand(CommandMessage* m
  // store additional parameters into the IPC Store
  store_address_t parameterAddress;
  ReturnValue_t result =
-      IPCStore->addData(&parameterAddress, command.getParameters(), command.getParametersSize());
+      ipcStore->addData(&parameterAddress, command.getParameters(), command.getParametersSize());

  // setCommand expects a Command Message, an Action ID and a store adress
  // pointing to additional parameters
@ -130,7 +130,7 @@ ReturnValue_t Service8FunctionManagement::handleDataReply(const CommandMessage*
  store_address_t storeId = ActionMessage::getStoreId(reply);
  size_t size = 0;
  const uint8_t* buffer = nullptr;
-  ReturnValue_t result = IPCStore->getData(storeId, &buffer, &size);
+  ReturnValue_t result = ipcStore->getData(storeId, &buffer, &size);
  if (result != RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "Service 8: Could not retrieve data for data reply" << std::endl;
@ -140,7 +140,7 @@ ReturnValue_t Service8FunctionManagement::handleDataReply(const CommandMessage*
  DataReply dataReply(objectId, actionId, buffer, size);
  result = sendTmPacket(static_cast<uint8_t>(Subservice::REPLY_DIRECT_COMMANDING_DATA), &dataReply);

-  auto deletionResult = IPCStore->deleteData(storeId);
+  auto deletionResult = ipcStore->deleteData(storeId);
  if (deletionResult != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::warning << "Service8FunctionManagement::handleReply: Deletion"
--- a/src/fsfw/pus/Service9TimeManagement.cpp
+++ b/src/fsfw/pus/Service9TimeManagement.cpp
@ -9,7 +9,7 @@ Service9TimeManagement::Service9TimeManagement(object_id_t objectId, uint16_t ap
                                               uint8_t serviceId)
    : PusServiceBase(objectId, apid, serviceId) {}

-Service9TimeManagement::~Service9TimeManagement() {}
+Service9TimeManagement::~Service9TimeManagement() = default;

 ReturnValue_t Service9TimeManagement::performService() { return RETURN_OK; }

@ -25,7 +25,9 @@ ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {

 ReturnValue_t Service9TimeManagement::setTime() {
  Clock::TimeOfDay_t timeToSet;
-  TimePacket timePacket(currentPacket.getUserData(), currentPacket.getUserDataSize());
+  size_t userDataLen = 0;
+  const uint8_t* timeData = currentPacket.getUserData(userDataLen);
+  TimePacket timePacket(timeData, userDataLen);
  ReturnValue_t result =
      CCSDSTime::convertFromCcsds(&timeToSet, timePacket.getTime(), timePacket.getTimeSize());
  if (result != RETURN_OK) {
--- a/src/fsfw/pus/servicepackets/Service9Packets.h
+++ b/src/fsfw/pus/servicepackets/Service9Packets.h
@ -1,7 +1,7 @@
 #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE9PACKETS_H_
 #define FSFW_PUS_SERVICEPACKETS_SERVICE9PACKETS_H_

-#include "../../serialize/SerialLinkedListAdapter.h"
+#include "fsfw/serialize/SerialLinkedListAdapter.h"

 /**
 * @brief Subservice 128
@ -11,16 +11,16 @@
 */
 class TimePacket : SerialLinkedListAdapter<SerializeIF> {  //!< [EXPORT] : [SUBSERVICE] 128
 public:
+  TimePacket(const TimePacket& command) = delete;
  TimePacket(const uint8_t* timeBuffer_, uint32_t timeSize_) {
    timeBuffer = timeBuffer_;
    timeSize = timeSize_;
  }
  const uint8_t* getTime() { return timeBuffer; }

-  uint32_t getTimeSize() const { return timeSize; }
+  [[nodiscard]] uint32_t getTimeSize() const { return timeSize; }

 private:
-  TimePacket(const TimePacket& command);
  const uint8_t* timeBuffer;
  uint32_t timeSize;  //!< [EXPORT] : [IGNORE]
 };
--- a/src/fsfw/storagemanager/StorageManagerIF.h
+++ b/src/fsfw/storagemanager/StorageManagerIF.h
@ -55,7 +55,7 @@ class StorageManagerIF : public HasReturnvaluesIF {
  /**
   * @brief This is the empty virtual destructor as required for C++ interfaces.
   */
-  virtual ~StorageManagerIF(){};
+  ~StorageManagerIF() override = default;
  /**
   * @brief	With addData, a free storage position is allocated and data
   * 			stored there.
@ -160,8 +160,8 @@ class StorageManagerIF : public HasReturnvaluesIF {
   * 					@li	RETURN_FAILED if data could not be added.
   * 						storageId is unchanged then.
   */
-  virtual ReturnValue_t getFreeElement(store_address_t* storageId, const size_t size,
-                                       uint8_t** p_data, bool ignoreFault = false) = 0;
+  virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size, uint8_t** p_data,
+                                       bool ignoreFault = false) = 0;

  /**
   * Clears the whole store.
@ -192,7 +192,7 @@ class StorageManagerIF : public HasReturnvaluesIF {
   * Get number of pools.
   * @return
   */
-  virtual max_subpools_t getNumberOfSubPools() const = 0;
+  [[nodiscard]] virtual max_subpools_t getNumberOfSubPools() const = 0;
 };

 #endif /* FSFW_STORAGEMANAGER_STORAGEMANAGERIF_H_ */
--- a/src/fsfw/tcdistribution/PusDistributor.cpp
+++ b/src/fsfw/tcdistribution/PusDistributor.cpp
@ -36,7 +36,7 @@ PusDistributor::TcMqMapIter PusDistributor::selectDestination() {
        HasReturnvaluesIF::RETURN_OK) {
      return queueMapIt;
    }
-    reader.setData(packetPtr, packetLen);
+    reader.setReadOnlyData(packetPtr, packetLen);
    // this->currentPacket->setStoreAddress(this->currentMessage.getStorageId(), currentPacket);
    if (reader.getFullData() != nullptr) {
      tcStatus =
--- a/src/fsfw/timemanager/CdsShortTimeStamper.h
+++ b/src/fsfw/timemanager/CdsShortTimeStamper.h
@ -14,14 +14,15 @@
 * overriding the #addTimeStamp function.
 * @ingroup utility
 */
-class TimeStamper : public TimeStamperIF, public SystemObject {
+class CdsShortTimeStamper : public TimeStamperIF, public SystemObject {
 public:
+  static constexpr size_t TIMESTAMP_LEN = 7;
  /**
   * @brief   Default constructor which also registers the time stamper as a
   *          system object so it can be found with the #objectManager.
   * @param objectId
   */
-  TimeStamper(object_id_t objectId);
+  explicit CdsShortTimeStamper(object_id_t objectId);

  /**
   * Adds a CCSDS CDC short 8 byte timestamp to the given buffer.
@ -30,7 +31,13 @@ class TimeStamper : public TimeStamperIF, public SystemObject {
   * @param maxSize
   * @return
   */
-  virtual ReturnValue_t addTimeStamp(uint8_t* buffer, const uint8_t maxSize);
+  ReturnValue_t addTimeStamp(uint8_t *buffer, uint8_t maxSize) override;
+  ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize,
+                          Endianness streamEndianness) const override;
+  size_t getSerializedSize() const override;
+  ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size,
+                            Endianness streamEndianness) override;
+  size_t getTimestampSize() const override;
 };

 #endif /* FSFW_TIMEMANAGER_TIMESTAMPER_H_ */
--- a/src/fsfw/timemanager/TimeStamper.cpp
+++ b/src/fsfw/timemanager/TimeStamper.cpp
@ -1,23 +1,35 @@
-#include "fsfw/timemanager/TimeStamper.h"
-
 #include <cstring>

+#include "fsfw/timemanager/CdsShortTimeStamper.h"
 #include "fsfw/timemanager/Clock.h"

-TimeStamper::TimeStamper(object_id_t objectId) : SystemObject(objectId) {}
+CdsShortTimeStamper::CdsShortTimeStamper(object_id_t objectId) : SystemObject(objectId) {}

-ReturnValue_t TimeStamper::addTimeStamp(uint8_t* buffer, const uint8_t maxSize) {
-  if (maxSize < TimeStamperIF::MISSION_TIMESTAMP_SIZE) {
-    return HasReturnvaluesIF::RETURN_FAILED;
+ReturnValue_t CdsShortTimeStamper::addTimeStamp(uint8_t *buffer, const uint8_t maxSize) {
+  size_t serLen = 0;
+  return serialize(&buffer, &serLen, maxSize, SerializeIF::Endianness::NETWORK);
+}
+
+ReturnValue_t CdsShortTimeStamper::serialize(uint8_t **buffer, size_t *size, size_t maxSize,
+                                             SerializeIF::Endianness streamEndianness) const {
+  if (*size + getSerializedSize() > maxSize) {
+    return SerializeIF::BUFFER_TOO_SHORT;
  }
-
-  timeval now;
+  timeval now{};
  Clock::getClock_timeval(&now);
-  CCSDSTime::CDS_short cds;
+  CCSDSTime::CDS_short cds{};
  ReturnValue_t result = CCSDSTime::convertToCcsds(&cds, &now);
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
-  std::memcpy(buffer, &cds, sizeof(cds));
+  std::memcpy(*buffer, &cds, sizeof(cds));
+  *buffer += getSerializedSize();
+  *size += getSerializedSize();
  return result;
 }
+size_t CdsShortTimeStamper::getSerializedSize() const { return getTimestampSize(); }
+ReturnValue_t CdsShortTimeStamper::deSerialize(const uint8_t **buffer, size_t *size,
+                                               SerializeIF::Endianness streamEndianness) {
+  return HasReturnvaluesIF::RETURN_FAILED;
+}
+size_t CdsShortTimeStamper::getTimestampSize() const { return TIMESTAMP_LEN; }
--- a/src/fsfw/timemanager/TimeStamperIF.h
+++ b/src/fsfw/timemanager/TimeStamperIF.h
@ -1,9 +1,8 @@
 #ifndef FSFW_TIMEMANAGER_TIMESTAMPERIF_H_
 #define FSFW_TIMEMANAGER_TIMESTAMPERIF_H_

-#include <FSFWConfig.h>
-
-#include "../returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/serialize/SerializeIF.h"

 /**
 * A class implementing this IF provides facilities to add a time stamp to the
@ -11,17 +10,21 @@
 * Implementors need to ensure that calling the method is thread-safe, i.e.
 * addTimeStamp may be called in parallel from a different context.
 */
-class TimeStamperIF {
+class TimeStamperIF : public SerializeIF {
 public:
  static const uint8_t INTERFACE_ID = CLASS_ID::TIME_STAMPER_IF;
  static const ReturnValue_t BAD_TIMESTAMP = MAKE_RETURN_CODE(1);

+  // I am going to assume there are no larger timestamps
+  static constexpr size_t MAXIMUM_TIMESTAMP_LEN = 16;
+
  //! This is a mission-specific constant and determines the total
  //! size reserved for timestamps.
-  static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;
+  // static const uint8_t MISSION_TIMESTAMP_SIZE = fsfwconfig::FSFW_MISSION_TIMESTAMP_SIZE;

-  virtual ReturnValue_t addTimeStamp(uint8_t* buffer, const uint8_t maxSize) = 0;
-  virtual ~TimeStamperIF() {}
+  [[nodiscard]] virtual size_t getTimestampSize() const = 0;
+  virtual ReturnValue_t addTimeStamp(uint8_t* buffer, uint8_t maxSize) = 0;
+  ~TimeStamperIF() override = default;
 };

 #endif /* FSFW_TIMEMANAGER_TIMESTAMPERIF_H_ */
--- a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp
+++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp
@ -75,3 +75,7 @@ void SpacePacketCreator::checkFieldValidity() {
    valid = false;
  }
 }
+
+void SpacePacketCreator::setParams(SpacePacketParams params_) { params = std::move(params_); }
+
+SpacePacketParams &SpacePacketCreator::getParams() { return params; }
--- a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h
+++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.h
@ -9,6 +9,8 @@
 #include "fsfw/serialize/SerializeIF.h"

 struct SpacePacketParams {
+  SpacePacketParams() = default;
+
  SpacePacketParams(PacketId packetId, PacketSeqCtrl psc, uint16_t dataLen)
      : packetId(std::move(packetId)), packetSeqCtrl(std::move(psc)), dataLen(dataLen) {}

@ -20,6 +22,7 @@ struct SpacePacketParams {

 class SpacePacketCreator : public SpacePacketIF, public SerializeIF {
 public:
+  SpacePacketCreator() = default;
  explicit SpacePacketCreator(SpacePacketParams params);
  SpacePacketCreator(ccsds::PacketType packetType, bool secHeaderFlag, uint16_t apid,
                     ccsds::SequenceFlags seqFlags, uint16_t seqCount, uint16_t dataLen,
@ -30,6 +33,8 @@ class SpacePacketCreator : public SpacePacketIF, public SerializeIF {
  [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override;
  [[nodiscard]] uint16_t getPacketDataLen() const override;

+  SpacePacketParams &getParams();
+  void setParams(SpacePacketParams params);
  void setApid(uint16_t apid);
  void setSeqCount(uint16_t seqCount);
  void setSeqFlags(ccsds::SequenceFlags flags);
@ -45,9 +50,7 @@ class SpacePacketCreator : public SpacePacketIF, public SerializeIF {

 private:
  void checkFieldValidity();
-
-  SpacePacketCreator() = default;
  bool valid{};
-  SpacePacketParams params;
+  SpacePacketParams params{};
 };
 #endif  // FSFW_TMTCPACKET_SPACEPACKETCREATOR_H
--- a/src/fsfw/tmtcpacket/ccsds/SpacePacketIF.h
+++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketIF.h
@ -18,7 +18,7 @@ class SpacePacketIF {
  /**
   * This is the minimum size of a SpacePacket.
   */
-  static const uint16_t MINIMUM_SIZE = sizeof(ccsds::PrimaryHeader) + CRC_SIZE;
+  static const uint16_t MIN_CCSDS_SIZE = sizeof(ccsds::PrimaryHeader) + CRC_SIZE;

  virtual ~SpacePacketIF() = default;

--- a/src/fsfw/tmtcpacket/packetmatcher/PacketMatchTree.cpp
+++ b/src/fsfw/tmtcpacket/packetmatcher/PacketMatchTree.cpp
@ -31,7 +31,7 @@ PacketMatchTree::~PacketMatchTree() = default;

 ReturnValue_t PacketMatchTree::addMatch(uint16_t apid, uint8_t type, uint8_t subtype) {
  // We assume adding APID is always requested.
-  PusTmMinimal::TmPacketMinimalPointer data{};
+  mintm::MinimalPusTm data{};
  data.secHeader.service = type;
  data.secHeader.subservice = subtype;
  PusTmMinimal testPacket((uint8_t*)&data);
@ -122,7 +122,7 @@ ReturnValue_t PacketMatchTree::findOrInsertMatch(iterator startAt, VALUE_T test,
 }

 ReturnValue_t PacketMatchTree::removeMatch(uint16_t apid, uint8_t type, uint8_t subtype) {
-  PusTmMinimal::TmPacketMinimalPointer data{};
+  mintm::MinimalPusTm data{};
  data.secHeader.service = type;
  data.secHeader.subservice = subtype;
  PusTmMinimal testPacket((uint8_t*)&data);
--- a/src/fsfw/tmtcpacket/pus/CreatorDataIF.h
+++ b/src/fsfw/tmtcpacket/pus/CreatorDataIF.h
@ -0,0 +1,10 @@
+#ifndef FSFW_TMTCPACKET_CREATORDATAIF_H
+#define FSFW_TMTCPACKET_CREATORDATAIF_H
+
+#include "definitions.h"
+
+class CreatorDataIF {
+ public:
+  virtual ecss::DataWrapper& getDataWrapper() = 0;
+};
+#endif  // FSFW_TMTCPACKET_CREATORDATAIF_H
--- a/src/fsfw/tmtcpacket/pus/PusIF.h
+++ b/src/fsfw/tmtcpacket/pus/PusIF.h
@ -29,24 +29,6 @@ class PusIF : public SpacePacketIF {
   * @return	The packet's PUS Service Subtype.
   */
  [[nodiscard]] virtual uint8_t getSubService() const = 0;
-
-  /**
-   * This is a getter for a pointer to the packet's Application data.
-   *
-   * These are the bytes that follow after the Data Field Header. They form
-   * the packet's application data.
-   * @return	A pointer to the PUS Application Data.
-   */
-  [[nodiscard]] virtual const uint8_t* getUserData(size_t& appDataLen) const = 0;
-  /**
-   * This method calculates the size of the PUS Application data field.
-   *
-   * It takes the information stored in the CCSDS Packet Data Length field
-   * and subtracts the Data Field Header size and the CRC size.
-   * @return	The size of the PUS Application Data (without Error Control
-   * 		field)
-   */
-  [[nodiscard]] virtual uint16_t getUserDataSize() const = 0;
 };

 #endif  // FSFW_TMTCPACKET_PUSIF_H
--- a/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h
+++ b/src/fsfw/tmtcpacket/pus/RawUserDataReaderIF.h
@ -0,0 +1,12 @@
+#ifndef FSFW_TMTCPACKET_RAWDATAIF_H
+#define FSFW_TMTCPACKET_RAWDATAIF_H
+
+#include <cstddef>
+#include <cstdint>
+
+class RawUserDataReaderIF {
+ public:
+  virtual const uint8_t* getUserData(size_t& userDataLen) = 0;
+};
+
+#endif  // FSFW_TMTCPACKET_RAWDATAIF_H
--- a/src/fsfw/tmtcpacket/pus/definitions.h
+++ b/src/fsfw/tmtcpacket/pus/definitions.h
@ -3,11 +3,39 @@

 #include <cstdint>

+#include "fsfw/serialize/SerializeIF.h"
+
 namespace ecss {

 //! Version numbers according to ECSS-E-ST-70-41C p.439
 enum PusVersion : uint8_t { PUS_A = 1, PUS_C = 2 };

+struct RawData {
+  const uint8_t* data;
+  size_t len;
+};
+
+enum DataTypes { RAW, SERIALIZABLE };
+
+union DataUnion {
+  RawData raw;
+  SerializeIF* serializable;
+};
+
+struct DataWrapper {
+  DataTypes type;
+  DataUnion dataUnion;
+
+  [[nodiscard]] size_t getLength() const {
+    if (type == DataTypes::RAW) {
+      return dataUnion.raw.len;
+    } else if (type == DataTypes::SERIALIZABLE and dataUnion.serializable != nullptr) {
+      return dataUnion.serializable->getSerializedSize();
+    }
+    return 0;
+  }
+};
+
 }  // namespace ecss

 #endif /* FSFW_SRC_FSFW_TMTCPACKET_PUS_TM_DEFINITIONS_H_ */
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp
@ -1,17 +1,20 @@
 #include "PusTcCreator.h"

+#include <utility>
+
 #include "PusTcIF.h"
 #include "fsfw/globalfunctions/CRC.h"
 #include "fsfw/serialize/SerializeAdapter.h"

 PusTcCreator::PusTcCreator(SpacePacketParams spParams, PusTcParams pusParams)
-    : spCreator(spParams), pusParams(pusParams) {
+    : spCreator(std::move(spParams)), pusParams(pusParams) {
  updateSpLengthField();
 }

 ReturnValue_t PusTcCreator::serialize(uint8_t **buffer, size_t *size, size_t maxSize,
                                      SerializeIF::Endianness streamEndianness) const {
-  if (*size + PusTcIF::MIN_LEN + pusParams.appDataLen > maxSize) {
+  size_t userDataLen = pusParams.dataWrapper.getLength();
+  if (*size + PusTcIF::MIN_LEN + userDataLen > maxSize) {
    return SerializeIF::BUFFER_TOO_SHORT;
  }
  ReturnValue_t result = spCreator.serialize(buffer, size, maxSize, streamEndianness);
@ -33,15 +36,28 @@ ReturnValue_t PusTcCreator::serialize(uint8_t **buffer, size_t *size, size_t max
  if (result != HasReturnvaluesIF::RETURN_OK) {
    return result;
  }
-  std::memcpy(*buffer, pusParams.appData, pusParams.appDataLen);
-  *buffer += pusParams.appDataLen;
-  *size += pusParams.appDataLen;
+  if (pusParams.dataWrapper.type == ecss::DataTypes::RAW) {
+    const uint8_t *data = pusParams.dataWrapper.dataUnion.raw.data;
+    if (data != nullptr and userDataLen > 0) {
+      std::memcpy(*buffer, data, userDataLen);
+      *buffer += userDataLen;
+      *size += userDataLen;
+    }
+  } else if (pusParams.dataWrapper.type == ecss::DataTypes::SERIALIZABLE and
+             pusParams.dataWrapper.dataUnion.serializable != nullptr) {
+    result = pusParams.dataWrapper.dataUnion.serializable->serialize(buffer, size, maxSize,
+                                                                     streamEndianness);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+  }
+
  uint16_t crc16 = CRC::crc16ccitt(*buffer, getFullPacketLen() - 2);
  return SerializeAdapter::serialize(&crc16, buffer, size, maxSize, streamEndianness);
 }

 void PusTcCreator::updateSpLengthField() {
-  spCreator.setDataLen(ecss::PusTcDataFieldHeader::MIN_LEN + pusParams.appDataLen + 1);
+  spCreator.setDataLen(ecss::PusTcDataFieldHeader::MIN_LEN + pusParams.dataWrapper.getLength() + 1);
 }

 size_t PusTcCreator::getSerializedSize() const { return spCreator.getFullPacketLen(); }
@ -66,10 +82,4 @@ uint8_t PusTcCreator::getService() const { return pusParams.service; }
 uint8_t PusTcCreator::getSubService() const { return pusParams.subservice; }

 uint16_t PusTcCreator::getSourceId() const { return pusParams.sourceId; }
-
-const uint8_t *PusTcCreator::getUserData(size_t &appDataLen) const {
-  appDataLen = getUserDataSize();
-  return pusParams.appData;
-}
-
-uint16_t PusTcCreator::getUserDataSize() const { return pusParams.appDataLen; }
+ecss::DataWrapper &PusTcCreator::getDataWrapper() { return pusParams.dataWrapper; }
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.h
@ -4,6 +4,7 @@
 #include "fsfw/tmtcpacket/RedirectableDataPointerIF.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketIF.h"
+#include "fsfw/tmtcpacket/pus/CreatorDataIF.h"
 #include "fsfw/tmtcpacket/pus/definitions.h"
 #include "fsfw/tmtcpacket/pus/tc/PusTcIF.h"

@ -14,12 +15,11 @@ struct PusTcParams {
  uint8_t subservice;
  uint8_t ackFlags = ecss::ACK_ALL;
  uint16_t sourceId = 0;
-  uint8_t *appData = nullptr;
-  size_t appDataLen = 0;
+  ecss::DataWrapper dataWrapper{};
  uint8_t pusVersion = ecss::PusVersion::PUS_C;
 };

-class PusTcCreator : public PusTcIF, public SerializeIF {
+class PusTcCreator : public PusTcIF, public SerializeIF, public CreatorDataIF {
 public:
  PusTcCreator(SpacePacketParams spParams, PusTcParams pusParams);

@ -37,8 +37,7 @@ class PusTcCreator : public PusTcIF, public SerializeIF {
  [[nodiscard]] uint8_t getService() const override;
  [[nodiscard]] uint8_t getSubService() const override;
  [[nodiscard]] uint16_t getSourceId() const override;
-  const uint8_t *getUserData(size_t &appDataLen) const override;
-  [[nodiscard]] uint16_t getUserDataSize() const override;
+  ecss::DataWrapper &getDataWrapper() override;

 private:
  SpacePacketCreator spCreator;
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcIF.h
@ -23,7 +23,7 @@ enum AckField {
 static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP | ACK_COMPLETION;

 /**
- * This struct defines a byte-wise structured PUS C ata Field Header.
+ * This struct defines a byte-wise structured PUS C data Field Header.
 * Any optional fields in the header must be added or removed here.
 * Currently, the Source Id field is present with one byte.
 * No spare byte support for now.
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.cpp
@ -7,7 +7,7 @@
 #include "fsfw/serialize.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"

-PusTcReader::PusTcReader(const uint8_t* data, size_t size) { setData(data, size); }
+PusTcReader::PusTcReader(const uint8_t* data, size_t size) { setReadOnlyData(data, size); }

 PusTcReader::~PusTcReader() = default;

@ -39,12 +39,6 @@ uint8_t PusTcReader::getSubService() const { return pointers.secHeaderStart[2];
 uint16_t PusTcReader::getSourceId() const {
  return (pointers.secHeaderStart[3] << 8) | pointers.secHeaderStart[4];
 }
-const uint8_t* PusTcReader::getUserData(size_t& appDataLen) const {
-  appDataLen = appDataSize;
-  return pointers.userDataStart;
-}
-
-uint16_t PusTcReader::getUserDataSize() const { return appDataSize; }

 uint16_t PusTcReader::getErrorControl() const {
  return pointers.crcStart[0] << 8 | pointers.crcStart[1];
@ -62,11 +56,17 @@ ReturnValue_t PusTcReader::setData(uint8_t* pData, size_t size_, void* args) {
  spReader.setData(pData, size_, args);
  return HasReturnvaluesIF::RETURN_OK;
 }
-ReturnValue_t PusTcReader::setData(const uint8_t* data, size_t size_) {
+
+ReturnValue_t PusTcReader::setReadOnlyData(const uint8_t* data, size_t size_) {
  setData(const_cast<uint8_t*>(data), size_, nullptr);
  return HasReturnvaluesIF::RETURN_OK;
 }

+const uint8_t* PusTcReader::getUserData(size_t& userDataLen) {
+  userDataLen = appDataSize;
+  return pointers.userDataStart;
+}
+
 /*
 void PusTcReader::print() {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
--- a/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h
+++ b/src/fsfw/tmtcpacket/pus/tc/PusTcReader.h
@ -6,6 +6,7 @@
 #include "PusTcIF.h"
 #include "fsfw/tmtcpacket/RedirectableDataPointerIF.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
+#include "fsfw/tmtcpacket/pus/RawUserDataReaderIF.h"

 /**
 * This class is the basic data handler for any ECSS PUS Telecommand packet.
@ -18,7 +19,10 @@
 * check can be performed by making use of the getWholeData method.
 * @ingroup tmtcpackets
 */
-class PusTcReader : public PusTcIF, public ReadablePacketIF, public RedirectableDataPointerIF {
+class PusTcReader : public PusTcIF,
+                    public RawUserDataReaderIF,
+                    public ReadablePacketIF,
+                    public RedirectableDataPointerIF {
 public:
  PusTcReader() = default;
  /**
@ -48,12 +52,11 @@ class PusTcReader : public PusTcIF, public ReadablePacketIF, public Redirectable
  [[nodiscard]] uint8_t getService() const override;
  [[nodiscard]] uint8_t getSubService() const override;
  [[nodiscard]] uint16_t getSourceId() const override;
-  [[nodiscard]] const uint8_t* getUserData(size_t& appDataLen) const override;
-  [[nodiscard]] uint16_t getUserDataSize() const override;
  [[nodiscard]] uint16_t getErrorControl() const;
  const uint8_t* getFullData() override;

-  ReturnValue_t setData(const uint8_t* data, size_t size);
+  ReturnValue_t setReadOnlyData(const uint8_t* data, size_t size);
+  const uint8_t* getUserData(size_t& userDataLen) override;

 protected:
  /**
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp
@ -1,6 +1,19 @@
 #include "PusTmCreator.h"

-PusTmCreator::PusTmCreator(PusTmParams& params_) : params(params_){};
+#include <utility>
+
+#include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/timemanager/TimeStamperIF.h"
+
+PusTmCreator::PusTmCreator(SpacePacketParams initSpParams, PusTmParams initPusParams,
+                           TimeStamperIF* timeStamper)
+    : pusParams(initPusParams), spCreator(std::move(initSpParams)){};
+
+PusTmCreator::PusTmCreator(TimeStamperIF* timeStamper_) {
+  pusParams.secHeader.timeStamper = timeStamper_;
+};
+
+PusTmCreator::PusTmCreator() = default;

 uint16_t PusTmCreator::getPacketIdRaw() const { return 0; }
 uint16_t PusTmCreator::getPacketSeqCtrlRaw() const { return 0; }
@ -8,5 +21,79 @@ uint16_t PusTmCreator::getPacketDataLen() const { return 0; }
 uint8_t PusTmCreator::getPusVersion() const { return 0; }
 uint8_t PusTmCreator::getService() const { return 0; }
 uint8_t PusTmCreator::getSubService() const { return 0; }
-const uint8_t* PusTmCreator::getUserData(size_t& appDataLen) const { return nullptr; }
-uint16_t PusTmCreator::getUserDataSize() const { return 0; }
+PusTmParams& PusTmCreator::getParams() { return pusParams; }
+void PusTmCreator::setTimeStamper(TimeStamperIF* timeStamper_) {
+  pusParams.secHeader.timeStamper = timeStamper_;
+}
+uint8_t PusTmCreator::getScTimeRefStatus() { return 0; }
+uint16_t PusTmCreator::getMessageTypeCounter() { return 0; }
+uint16_t PusTmCreator::getDestId() { return 0; }
+
+ReturnValue_t PusTmCreator::serialize(uint8_t** buffer, size_t* size, size_t maxSize,
+                                      SerializeIF::Endianness streamEndianness) const {
+  if (*size + getSerializedSize() > maxSize) {
+    return SerializeIF::BUFFER_TOO_SHORT;
+  }
+  ReturnValue_t result = spCreator.serialize(buffer, size, maxSize);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  size_t userDataLen = pusParams.dataWrapper.getLength();
+  **buffer =
+      ((pusParams.secHeader.pusVersion << 4) & 0xF0) | (pusParams.secHeader.scTimeRefStatus & 0x0F);
+  *buffer += 1;
+  **buffer = pusParams.secHeader.service;
+  *buffer += 1;
+  **buffer = pusParams.secHeader.subservice;
+  *buffer += 1;
+  *size += 3;
+  result = SerializeAdapter::serialize(&pusParams.secHeader.messageTypeCounter, buffer, size,
+                                       maxSize, streamEndianness);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  result = SerializeAdapter::serialize(&pusParams.secHeader.destId, buffer, size, maxSize,
+                                       streamEndianness);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    return result;
+  }
+  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);
+    *buffer += userDataLen;
+    *size += userDataLen;
+  } else if (pusParams.dataWrapper.type == ecss::DataTypes::SERIALIZABLE and
+             pusParams.dataWrapper.dataUnion.serializable != nullptr) {
+    result = pusParams.dataWrapper.dataUnion.serializable->serialize(buffer, size, maxSize,
+                                                                     streamEndianness);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+  }
+  uint16_t crc16 = CRC::crc16ccitt(*buffer, getFullPacketLen() - 2);
+  return SerializeAdapter::serialize(&crc16, buffer, size, maxSize, streamEndianness);
+}
+
+size_t PusTmCreator::getSerializedSize() const { return getFullPacketLen(); }
+ReturnValue_t PusTmCreator::deSerialize(const uint8_t** buffer, size_t* size,
+                                        SerializeIF::Endianness streamEndianness) {
+  return HasReturnvaluesIF::RETURN_FAILED;
+}
+
+ecss::DataWrapper& PusTmCreator::getDataWrapper() { return pusParams.dataWrapper; }
+TimeStamperIF* PusTmCreator::getTimestamper() { return pusParams.secHeader.timeStamper; }
+SpacePacketParams& PusTmCreator::getSpParams() { return spCreator.getParams(); }
+
+void PusTmCreator::updateSpLengthField() {
+  size_t headerLen = PusTmIF::MIN_TM_SIZE;
+  if (pusParams.secHeader.timeStamper != nullptr) {
+    headerLen += pusParams.secHeader.timeStamper->getSerializedSize();
+  }
+  spCreator.setDataLen(headerLen + pusParams.dataWrapper.getLength() + 1);
+}
+
+void PusTmCreator::setApid(uint16_t apid) { spCreator.setApid(apid); };
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.h
@ -1,33 +1,62 @@
 #ifndef FSFW_TMTCPACKET_TMPACKETCREATOR_H
 #define FSFW_TMTCPACKET_TMPACKETCREATOR_H

-#include "fsfw/tmtcpacket/pus/PusIF.h"
+#include "PusTmIF.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketCreator.h"
+#include "fsfw/tmtcpacket/pus/CreatorDataIF.h"

-struct PusTmParams {
+struct PusTmSecHeader {
  uint8_t pusVersion;
  uint8_t scTimeRefStatus;
  uint8_t service;
  uint8_t subservice;
  uint16_t messageTypeCounter;
  uint16_t destId;
-  uint8_t* timestamp;
-  size_t timestampLen;
+  TimeStamperIF* timeStamper;
 };
-class PusTmCreator : public PusIF {
- public:
-  ~PusTmCreator() override = default;
-  explicit PusTmCreator(PusTmParams& params);

+struct PusTmParams {
+  PusTmSecHeader secHeader;
+  ecss::DataWrapper dataWrapper;
+};
+
+class TimeStamperIF;
+
+class PusTmCreator : public SerializeIF, public PusTmIF, public CreatorDataIF {
+ public:
+  PusTmCreator();
+
+  explicit PusTmCreator(TimeStamperIF* timeStamper);
+  PusTmCreator(SpacePacketParams initSpParams, PusTmParams initPusParams,
+               TimeStamperIF* timeStamper);
+  ~PusTmCreator() override = default;
+
+  void setTimeStamper(TimeStamperIF* timeStamper);
+  SpacePacketParams& getSpParams();
+  void setApid(uint16_t apid);
+  PusTmParams& getParams();
+  void updateSpLengthField();
  [[nodiscard]] uint16_t getPacketIdRaw() const override;
  [[nodiscard]] uint16_t getPacketSeqCtrlRaw() const override;
  [[nodiscard]] uint16_t getPacketDataLen() const override;
  [[nodiscard]] uint8_t getPusVersion() const override;
  [[nodiscard]] uint8_t getService() const override;
  [[nodiscard]] uint8_t getSubService() const override;
-  const uint8_t* getUserData(size_t& appDataLen) const override;
-  [[nodiscard]] uint16_t getUserDataSize() const override;
+  uint8_t getScTimeRefStatus() override;
+  uint16_t getMessageTypeCounter() override;
+  uint16_t getDestId() override;
+  ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
+                          Endianness streamEndianness) const override;
+  [[nodiscard]] size_t getSerializedSize() const override;
+  ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+                            Endianness streamEndianness) override;
+  TimeStamperIF* getTimestamper() override;

 private:
-  PusTmParams& params;
+  ecss::DataWrapper& getDataWrapper() override;
+
+ private:
+  PusTmParams pusParams{};
+  SpacePacketCreator spCreator;
 };
 #endif  // FSFW_TMTCPACKET_TMPACKETCREATOR_H
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmIF.h
@ -4,17 +4,27 @@
 #include <cstddef>
 #include <cstdint>

+#include "fsfw/timemanager/TimeStamperIF.h"
 #include "fsfw/tmtcpacket/pus/PusIF.h"
+#include "fsfw/tmtcpacket/pus/definitions.h"

 class PusTmIF : public PusIF {
 public:
  ~PusTmIF() override = default;

+  /**
+   * Minimum length without timestamp
+   */
+  static constexpr size_t MIN_SEC_HEADER_LEN = 7;
+  /**
+   * 2 bytes for CRC16
+   */
+  static constexpr size_t MIN_TM_SIZE = sizeof(ccsds::PrimaryHeader) + MIN_SEC_HEADER_LEN + 2;
+
  virtual uint8_t getScTimeRefStatus() = 0;
  virtual uint16_t getMessageTypeCounter() = 0;
  virtual uint16_t getDestId() = 0;
-  virtual const uint8_t* getTimestamp(size_t& timeStampLen) = 0;
-  virtual size_t getTimestampLen() = 0;
+  virtual TimeStamperIF* getTimestamper() = 0;
 };

 #endif  // FSFW_TMTCPACKET_PUSTMIF_H
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.cpp
@ -34,15 +34,16 @@ uint16_t PusTmMinimal::getPacketDataLen() const { return 0; }
 uint8_t PusTmMinimal::getPusVersion() const { return 0; }
 uint8_t PusTmMinimal::getService() const { return tmData->secHeader.service; }
 uint8_t PusTmMinimal::getSubService() const { return tmData->secHeader.subservice; }
-const uint8_t* PusTmMinimal::getUserData(size_t& appDataLen) const { return nullptr; }
-uint16_t PusTmMinimal::getUserDataSize() const { return 0; }
 uint8_t PusTmMinimal::getScTimeRefStatus() { return 0; }
 uint16_t PusTmMinimal::getMessageTypeCounter() {
  return (tmData->secHeader.messageTypeH << 8) | tmData->secHeader.messageTypeL;
 }
 uint16_t PusTmMinimal::getDestId() { return 0; }
-const uint8_t* PusTmMinimal::getTimestamp(size_t& timeStampLen) { return nullptr; }
-size_t PusTmMinimal::getTimestampLen() { return 0; }
 void PusTmMinimal::setApid(uint16_t apid) {
  /* TODO: Implement. Maybe provide low level function to do this */
 }
+const uint8_t* PusTmMinimal::getUserData(size_t& userDataLen_) {
+  userDataLen_ = userDataLen;
+  return reinterpret_cast<const uint8_t*>(&tmData->rest);
+}
+TimeStamperIF* PusTmMinimal::getTimestamper() { return nullptr; }
--- a/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h
+++ b/src/fsfw/tmtcpacket/pus/tm/PusTmMinimal.h
@ -4,6 +4,7 @@
 #include "PusTmIF.h"
 #include "fsfw/returnvalues/HasReturnvaluesIF.h"
 #include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
+#include "fsfw/tmtcpacket/pus/RawUserDataReaderIF.h"

 struct timeval;

@ -12,7 +13,7 @@ class PacketTimestampInterpreterIF;
 namespace mintm {

 // NOTE: Only PUS C compatible!
-struct PusTmMinimalSecHeader {
+struct PusTmMinimalSecHeaderPacked {
  uint8_t versionAndScTimeRefStatus;
  uint8_t service;
  uint8_t subservice;
@ -27,7 +28,7 @@ struct PusTmMinimalSecHeader {
 */
 struct MinimalPusTm {
  ccsds::PrimaryHeader primary;
-  PusTmMinimalSecHeader secHeader;
+  PusTmMinimalSecHeaderPacked secHeader;
  uint8_t rest;
 };

@ -42,7 +43,7 @@ static const uint16_t MINIMUM_SIZE = sizeof(MinimalPusTm) + 1;
 * This is required for handling TM packets with different APIDs with different
 * secondary headers.
 */
-class PusTmMinimal : public PusTmIF, public RedirectableDataPointerIF {
+class PusTmMinimal : public PusTmIF, public RawUserDataReaderIF, public RedirectableDataPointerIF {
 public:
  explicit PusTmMinimal(mintm::MinimalPusTm* data);
  /**
@ -69,13 +70,11 @@ class PusTmMinimal : public PusTmIF, public RedirectableDataPointerIF {
  [[nodiscard]] uint8_t getPusVersion() const override;
  [[nodiscard]] uint8_t getService() const override;
  [[nodiscard]] uint8_t getSubService() const override;
-  const uint8_t* getUserData(size_t& appDataLen) const override;
-  [[nodiscard]] uint16_t getUserDataSize() const override;
  uint8_t getScTimeRefStatus() override;
  uint16_t getMessageTypeCounter() override;
  uint16_t getDestId() override;
-  const uint8_t* getTimestamp(size_t& timeStampLen) override;
-  size_t getTimestampLen() override;
+  const uint8_t* getUserData(size_t& userDataLen) override;
+  TimeStamperIF* getTimestamper() override;

 protected:
  /**
@ -84,6 +83,7 @@ class PusTmMinimal : public PusTmIF, public RedirectableDataPointerIF {
   *
   * To be hardware-safe, all elements are of byte size.
   */
+  size_t userDataLen = 0;
  mintm::MinimalPusTm* tmData;

  static PacketTimestampInterpreterIF* timestampInterpreter;
--- a/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h
+++ b/src/fsfw/tmtcpacket/pus/tm/TmPacketPusC.h
@ -26,7 +26,7 @@ struct PUSTmDataFieldHeaderPusC {
  uint8_t subcounterLsb;
  uint8_t destinationIdMsb;
  uint8_t destinationIdLsb;
-  uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
+  // uint8_t time[TimeStamperIF::MISSION_TIMESTAMP_SIZE];
 };

 /**
--- a/src/fsfw/tmtcservices/CMakeLists.txt
+++ b/src/fsfw/tmtcservices/CMakeLists.txt
@ -7,4 +7,5 @@ target_sources(
          TmTcMessage.cpp
          VerificationReporter.cpp
          SpacePacketParser.cpp
-          TmStoreHelper.cpp)
+          TmStoreHelper.cpp
+          TmSendHelper.cpp)
--- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
@ -8,6 +8,7 @@
 #include "fsfw/tmtcpacket/pus/tm.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
+#include "fsfw/tmtcservices/sendAndStoreHelper.h"

 object_id_t CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
 object_id_t CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;
@ -19,17 +20,19 @@ CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t a
      apid(apid),
      service(service),
      timeoutSeconds(commandTimeoutSeconds),
+      tmStoreHelper(apid, nullptr),
+      tmSendHelper(nullptr),
      commandMap(numberOfParallelCommands) {
  commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
  requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
 }

-void CommandingServiceBase::setPacketSource(object_id_t packetSource) {
-  this->packetSource = packetSource;
+void CommandingServiceBase::setPacketSource(object_id_t packetSource_) {
+  packetSource = packetSource_;
 }

-void CommandingServiceBase::setPacketDestination(object_id_t packetDestination) {
-  this->packetDestination = packetDestination;
+void CommandingServiceBase::setPacketDestination(object_id_t packetDestination_) {
+  packetDestination = packetDestination_;
 }

 CommandingServiceBase::~CommandingServiceBase() {
@ -58,13 +61,12 @@ ReturnValue_t CommandingServiceBase::initialize() {
  if (packetDestination == objects::NO_OBJECT) {
    packetDestination = defaultPacketDestination;
  }
-  AcceptsTelemetryIF* packetForwarding =
-      ObjectManager::instance()->get<AcceptsTelemetryIF>(packetDestination);
+  auto* packetForwarding = ObjectManager::instance()->get<AcceptsTelemetryIF>(packetDestination);

  if (packetSource == objects::NO_OBJECT) {
    packetSource = defaultPacketSource;
  }
-  PUSDistributorIF* distributor = ObjectManager::instance()->get<PUSDistributorIF>(packetSource);
+  auto* distributor = ObjectManager::instance()->get<PUSDistributorIF>(packetSource);

  if (packetForwarding == nullptr or distributor == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -78,10 +80,10 @@ ReturnValue_t CommandingServiceBase::initialize() {
  distributor->registerService(this);
  requestQueue->setDefaultDestination(packetForwarding->getReportReceptionQueue());

-  IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
-  TCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
+  ipcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
+  tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);

-  if (IPCStore == nullptr or TCStore == nullptr) {
+  if (ipcStore == nullptr or tcStore == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "CommandingServiceBase::intialize: IPC store or TC store "
                  "not initialized yet!"
@ -89,7 +91,20 @@ ReturnValue_t CommandingServiceBase::initialize() {
 #endif
    return ObjectManagerIF::CHILD_INIT_FAILED;
  }
-
+  if (tmStoreHelper.getTmStore() == nullptr) {
+    auto* tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+    if (tmStore == nullptr) {
+      return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+    tmStoreHelper.setTmStore(tmStore);
+  }
+  if (errReporter == nullptr) {
+    errReporter =
+        ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+    if (errReporter != nullptr) {
+      tmSendHelper.setInternalErrorReporter(errReporter);
+    }
+  }
  return RETURN_OK;
 }

@ -226,23 +241,31 @@ void CommandingServiceBase::handleRequestQueue() {
  TmTcMessage message;
  ReturnValue_t result;
  store_address_t address;
-  TcPacketStoredPus packet;
  MessageQueueId_t queue;
  object_id_t objectId;
  for (result = requestQueue->receiveMessage(&message); result == RETURN_OK;
       result = requestQueue->receiveMessage(&message)) {
    address = message.getStorageId();
-    packet.setStoreAddress(address, &packet);
+    const uint8_t* dataPtr;
+    size_t dataLen = 0;
+    result = tcStore->getData(message.getStorageId(), &dataPtr, &dataLen);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      // TODO: Warning?
+    }
+    tcReader.setReadOnlyData(dataPtr, dataLen);

-    if ((packet.getSubService() == 0) or (isValidSubservice(packet.getSubService()) != RETURN_OK)) {
-      rejectPacket(tc_verification::START_FAILURE, &packet, INVALID_SUBSERVICE);
+    if ((tcReader.getSubService() == 0) or
+        (isValidSubservice(tcReader.getSubService()) != RETURN_OK)) {
+      rejectPacket(tc_verification::START_FAILURE, address, &tcReader, INVALID_SUBSERVICE);
      continue;
    }

-    result = getMessageQueueAndObject(packet.getSubService(), packet.getApplicationData(),
-                                      packet.getApplicationDataSize(), &queue, &objectId);
+    size_t appDataLen = 0;
+    const uint8_t* appData = tcReader.getUserData(appDataLen);
+    result =
+        getMessageQueueAndObject(tcReader.getSubService(), appData, appDataLen, &queue, &objectId);
    if (result != HasReturnvaluesIF::RETURN_OK) {
-      rejectPacket(tc_verification::START_FAILURE, &packet, result);
+      rejectPacket(tc_verification::START_FAILURE, address, &tcReader, result);
      continue;
    }

@ -253,33 +276,26 @@ void CommandingServiceBase::handleRequestQueue() {
    if (iter != commandMap.end()) {
      result = iter->second.fifo.insert(address);
      if (result != RETURN_OK) {
-        rejectPacket(tc_verification::START_FAILURE, &packet, OBJECT_BUSY);
+        rejectPacket(tc_verification::START_FAILURE, address, &tcReader, OBJECT_BUSY);
      }
    } else {
      CommandInfo newInfo;  // Info will be set by startExecution if neccessary
      newInfo.objectId = objectId;
      result = commandMap.insert(queue, newInfo, &iter);
      if (result != RETURN_OK) {
-        rejectPacket(tc_verification::START_FAILURE, &packet, BUSY);
+        rejectPacket(tc_verification::START_FAILURE, address, &tcReader, BUSY);
      } else {
-        startExecution(&packet, iter);
+        startExecution(address, &tcReader, iter);
      }
    }
  }
 }

-ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, const uint8_t* data,
-                                                  size_t dataLen, const uint8_t* headerData,
-                                                  size_t headerSize) {
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    data, dataLen, headerData, headerSize);
-#else
-  TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    data, dataLen, headerData, headerSize);
-#endif
-  ReturnValue_t result =
-      tmPacketStored.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, const uint8_t* sourceData,
+                                                  size_t sourceDataLen) {
+  tmStoreHelper.preparePacket(service, subservice, tmPacketCounter);
+  tmStoreHelper.setSourceDataRaw(sourceData, sourceDataLen);
+  ReturnValue_t result = tm::storeAndSendTmPacket(tmStoreHelper, tmSendHelper);
  if (result == HasReturnvaluesIF::RETURN_OK) {
    this->tmPacketCounter++;
  }
@ -288,54 +304,38 @@ ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, const uint

 ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, object_id_t objectId,
                                                  const uint8_t* data, size_t dataLen) {
-  uint8_t buffer[sizeof(object_id_t)];
-  uint8_t* pBuffer = buffer;
-  size_t size = 0;
-  SerializeAdapter::serialize(&objectId, &pBuffer, &size, sizeof(object_id_t),
-                              SerializeIF::Endianness::BIG);
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    data, dataLen, buffer, size);
-#else
-  TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    data, dataLen, buffer, size);
-#endif
-  ReturnValue_t result =
-      tmPacketStored.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
+  tm::SourceDataWithObjectIdPrefix dataWithObjId(objectId, data, dataLen);
+  tmStoreHelper.preparePacket(service, subservice, tmPacketCounter);
+  tmStoreHelper.setSourceDataSerializable(&dataWithObjId);
+  ReturnValue_t result = tm::storeAndSendTmPacket(tmStoreHelper, tmSendHelper);
  if (result == HasReturnvaluesIF::RETURN_OK) {
    this->tmPacketCounter++;
  }
  return result;
 }

-ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, SerializeIF* content,
-                                                  SerializeIF* header) {
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-  TmPacketStoredPusA tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    content, header);
-#else
-  TmPacketStoredPusC tmPacketStored(this->apid, this->service, subservice, this->tmPacketCounter,
-                                    content, header);
-#endif
-  ReturnValue_t result =
-      tmPacketStored.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
+ReturnValue_t CommandingServiceBase::sendTmPacket(uint8_t subservice, SerializeIF* sourceData) {
+  tmStoreHelper.preparePacket(service, subservice, tmPacketCounter);
+  tmStoreHelper.setSourceDataSerializable(sourceData);
+  ReturnValue_t result = tm::storeAndSendTmPacket(tmStoreHelper, tmSendHelper);
  if (result == HasReturnvaluesIF::RETURN_OK) {
    this->tmPacketCounter++;
  }
  return result;
 }

-void CommandingServiceBase::startExecution(TcPacketStoredPus* storedPacket, CommandMapIter iter) {
+void CommandingServiceBase::startExecution(store_address_t storeId, PusTcReader* storedPacket,
+                                           CommandMapIter iter) {
  ReturnValue_t result = RETURN_OK;
  CommandMessage command;
-  // TcPacketPusBase* tcPacketBase = storedPacket->getPacketBase();
  if (storedPacket == nullptr) {
    return;
  }
  iter->second.subservice = storedPacket->getSubService();
-  result = prepareCommand(&command, iter->second.subservice, storedPacket->getApplicationData(),
-                          storedPacket->getApplicationDataSize(), &iter->second.state,
-                          iter->second.objectId);
+  size_t appDataLen = 0;
+  const uint8_t* appData = storedPacket->getUserData(appDataLen);
+  result = prepareCommand(&command, iter->second.subservice, appData, appDataLen,
+                          &iter->second.state, iter->second.objectId);

  ReturnValue_t sendResult = RETURN_OK;
  switch (result) {
@ -349,12 +349,12 @@ void CommandingServiceBase::startExecution(TcPacketStoredPus* storedPacket, Comm
        iter->second.subservice = storedPacket->getSubService();
        iter->second.command = command.getCommand();
        iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
-        iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
-        iter->second.tcInfo.tcSequenceControl = storedPacket->getPacketSeqCtrl();
-        acceptPacket(tc_verification::START_SUCCESS, storedPacket);
+        iter->second.tcInfo.tcPacketId = storedPacket->getPacketIdRaw();
+        iter->second.tcInfo.tcSequenceControl = storedPacket->getPacketSeqCtrlRaw();
+        acceptPacket(tc_verification::START_SUCCESS, storeId, storedPacket);
      } else {
        command.clearCommandMessage();
-        rejectPacket(tc_verification::START_FAILURE, storedPacket, sendResult);
+        rejectPacket(tc_verification::START_FAILURE, storeId, storedPacket, sendResult);
        checkAndExecuteFifo(iter);
      }
      break;
@ -364,33 +364,32 @@ void CommandingServiceBase::startExecution(TcPacketStoredPus* storedPacket, Comm
        sendResult = commandQueue->sendMessage(iter.value->first, &command);
      }
      if (sendResult == RETURN_OK) {
-        verificationReporter.sendSuccessReport(tc_verification::START_SUCCESS,
-                                               storedPacket->getPacketBase());
-        acceptPacket(tc_verification::COMPLETION_SUCCESS, storedPacket);
+        verificationReporter.sendSuccessReport(tc_verification::START_SUCCESS, storedPacket);
+        acceptPacket(tc_verification::COMPLETION_SUCCESS, storeId, storedPacket);
        checkAndExecuteFifo(iter);
      } else {
        command.clearCommandMessage();
-        rejectPacket(tc_verification::START_FAILURE, storedPacket, sendResult);
+        rejectPacket(tc_verification::START_FAILURE, storeId, storedPacket, sendResult);
        checkAndExecuteFifo(iter);
      }
      break;
    default:
-      rejectPacket(tc_verification::START_FAILURE, storedPacket, result);
+      rejectPacket(tc_verification::START_FAILURE, storeId, storedPacket, result);
      checkAndExecuteFifo(iter);
      break;
  }
 }

-void CommandingServiceBase::rejectPacket(uint8_t reportId, TcPacketStoredPus* packet,
-                                         ReturnValue_t errorCode) {
-  verificationReporter.sendFailureReport(reportId, dynamic_cast<TcPacketPusBase*>(packet),
-                                         errorCode);
-  packet->deletePacket();
+void CommandingServiceBase::rejectPacket(uint8_t reportId, store_address_t tcStoreId,
+                                         PusTcReader* correspondingTc, ReturnValue_t errorCode) {
+  verificationReporter.sendFailureReport(reportId, correspondingTc, errorCode);
+  tcStore->deleteData(tcStoreId);
 }

-void CommandingServiceBase::acceptPacket(uint8_t reportId, TcPacketStoredPus* packet) {
-  verificationReporter.sendSuccessReport(reportId, dynamic_cast<TcPacketPusBase*>(packet));
-  packet->deletePacket();
+void CommandingServiceBase::acceptPacket(uint8_t reportId, store_address_t tcStoreId,
+                                         PusTcReader* packet) {
+  verificationReporter.sendSuccessReport(reportId, packet);
+  tcStore->deleteData(tcStoreId);
 }

 void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
@ -398,8 +397,15 @@ void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) {
  if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
    commandMap.erase(&iter);
  } else {
-    TcPacketStoredPus newPacket(address);
-    startExecution(&newPacket, iter);
+    const uint8_t* dataPtr;
+    size_t dataLen = 0;
+    ReturnValue_t result = tcStore->getData(address, &dataPtr, &dataLen);
+    if (result == HasReturnvaluesIF::RETURN_OK) {
+      tcReader.setReadOnlyData(dataPtr, dataLen);
+      startExecution(address, &tcReader, iter);
+    } else {
+      // TODO: Warning?
+    }
  }
 }

@ -426,3 +432,7 @@ void CommandingServiceBase::checkTimeout() {
 }

 void CommandingServiceBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
+
+void CommandingServiceBase::setCustomTmStore(StorageManagerIF* store) {
+  tmStoreHelper.setTmStore(store);
+}
--- a/src/fsfw/tmtcservices/CommandingServiceBase.h
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.h
@ -2,6 +2,8 @@
 #define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_

 #include "AcceptsTelecommandsIF.h"
+#include "TmSendHelper.h"
+#include "TmStoreHelper.h"
 #include "VerificationReporter.h"
 #include "fsfw/FSFW.h"
 #include "fsfw/container/FIFO.h"
@ -69,8 +71,9 @@ class CommandingServiceBase : public SystemObject,
  CommandingServiceBase(object_id_t setObjectId, uint16_t apid, uint8_t service,
                        uint8_t numberOfParallelCommands, uint16_t commandTimeoutSeconds,
                        size_t queueDepth = 20);
-  virtual ~CommandingServiceBase();
+  ~CommandingServiceBase() override;

+  void setCustomTmStore(StorageManagerIF* store);
  /**
   * This setter can be used to set the packet source individually instead
   * of using the default static framework ID set in the factory.
@ -93,9 +96,9 @@ class CommandingServiceBase : public SystemObject,
   * @param opCode is unused here at the moment
   * @return RETURN_OK
   */
-  virtual ReturnValue_t performOperation(uint8_t opCode) override;
+  ReturnValue_t performOperation(uint8_t opCode) override;

-  virtual uint16_t getIdentifier() override;
+  uint16_t getIdentifier() override;

  /**
   * Returns the requestQueue MessageQueueId_t
@ -104,7 +107,7 @@ class CommandingServiceBase : public SystemObject,
   *
   * @return requestQueue messageQueueId_t
   */
-  virtual MessageQueueId_t getRequestQueue() override;
+  MessageQueueId_t getRequestQueue() override;

  /**
   * Returns the commandQueue MessageQueueId_t
@ -114,7 +117,7 @@ class CommandingServiceBase : public SystemObject,
   */
  virtual MessageQueueId_t getCommandQueue();

-  virtual ReturnValue_t initialize() override;
+  ReturnValue_t initialize() override;

  /**
   * Implementation of ExecutableObjectIF function
@ -122,7 +125,7 @@ class CommandingServiceBase : public SystemObject,
   * Used to setup the reference of the task, that executes this component
   * @param task Pointer to the taskIF of this task
   */
-  virtual void setTaskIF(PeriodicTaskIF* task) override;
+  void setTaskIF(PeriodicTaskIF* task) override;

 protected:
  /**
@ -230,15 +233,15 @@ class CommandingServiceBase : public SystemObject,
    object_id_t objectId;
    FIFO<store_address_t, COMMAND_INFO_FIFO_DEPTH> fifo;

-    virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
-                                    Endianness streamEndianness) const override {
+    ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
+                            Endianness streamEndianness) const override {
      return HasReturnvaluesIF::RETURN_FAILED;
    };

-    virtual size_t getSerializedSize() const override { return 0; };
+    [[nodiscard]] size_t getSerializedSize() const override { return 0; };

-    virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
-                                      Endianness streamEndianness) override {
+    ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+                              Endianness streamEndianness) override {
      return HasReturnvaluesIF::RETURN_FAILED;
    };
  };
@ -253,9 +256,13 @@ class CommandingServiceBase : public SystemObject,

  uint8_t tmPacketCounter = 0;

-  StorageManagerIF* IPCStore = nullptr;
+  StorageManagerIF* ipcStore = nullptr;

-  StorageManagerIF* TCStore = nullptr;
+  PusTcReader tcReader;
+  TmStoreHelper tmStoreHelper;
+  TmSendHelper tmSendHelper;
+
+  StorageManagerIF* tcStore = nullptr;

  MessageQueueIF* commandQueue = nullptr;

@ -263,6 +270,8 @@ class CommandingServiceBase : public SystemObject,

  VerificationReporter verificationReporter;

+  InternalErrorReporterIF* errReporter = nullptr;
+
  FixedMap<MessageQueueId_t, CommandInfo> commandMap;

  /* May be set be children to return a more precise failure condition. */
@ -284,13 +293,10 @@ class CommandingServiceBase : public SystemObject,
   * @brief   Send TM data from pointer to data.
   *          If a header is supplied it is added before data
   * @param subservice Number of subservice
-   * @param data Pointer to the data in the Packet
-   * @param dataLen Lenght of data in the Packet
-   * @param headerData HeaderData will be placed before data
-   * @param headerSize Size of HeaderData
+   * @param sourceData Custom source data
+   * @param sourceDataLen Lenght of data in the Packet
   */
-  ReturnValue_t sendTmPacket(uint8_t subservice, const uint8_t* data, size_t dataLen,
-                             const uint8_t* headerData = nullptr, size_t headerSize = 0);
+  ReturnValue_t sendTmPacket(uint8_t subservice, const uint8_t* sourceData, size_t sourceDataLen);

  /**
   * @brief   To send TM packets of objects that still need to be serialized
@ -310,8 +316,7 @@ class CommandingServiceBase : public SystemObject,
   * @param content This is a pointer to the serialized packet
   * @param header Serialize IF header which will be placed before content
   */
-  ReturnValue_t sendTmPacket(uint8_t subservice, SerializeIF* content,
-                             SerializeIF* header = nullptr);
+  ReturnValue_t sendTmPacket(uint8_t subservice, SerializeIF* sourceData);

  void checkAndExecuteFifo(CommandMapIter& iter);

@ -345,11 +350,12 @@ class CommandingServiceBase : public SystemObject,
   */
  void handleRequestQueue();

-  void rejectPacket(uint8_t reportId, TcPacketStoredPus* packet, ReturnValue_t errorCode);
+  void rejectPacket(uint8_t reportId, store_address_t tcStoreId, PusTcReader* tcPacket,
+                    ReturnValue_t errorCode);

-  void acceptPacket(uint8_t reportId, TcPacketStoredPus* packet);
+  void acceptPacket(uint8_t reportId, store_address_t tcStoreId, PusTcReader* tcPacket);

-  void startExecution(TcPacketStoredPus* storedPacket, CommandMapIter iter);
+  void startExecution(store_address_t storeId, PusTcReader* storedPacket, CommandMapIter iter);

  void handleCommandMessage(CommandMessage* reply);
  void handleReplyHandlerResult(ReturnValue_t result, CommandMapIter iter,
--- a/src/fsfw/tmtcservices/PusServiceBase.cpp
+++ b/src/fsfw/tmtcservices/PusServiceBase.cpp
@ -11,9 +11,8 @@
 object_id_t PusServiceBase::packetSource = 0;
 object_id_t PusServiceBase::packetDestination = 0;

-PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId,
-                               StorageManagerIF* ipcStore_)
-    : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId), ipcStore(ipcStore_) {
+PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId)
+    : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId) {
  requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION);
 }

@ -36,7 +35,7 @@ void PusServiceBase::setTaskIF(PeriodicTaskIF* taskHandle_) { this->taskHandle =

 void PusServiceBase::handleRequestQueue() {
  TmTcMessage message;
-  ReturnValue_t result = RETURN_FAILED;
+  ReturnValue_t result;
  for (uint8_t count = 0; count < PUS_SERVICE_MAX_RECEPTION; count++) {
    ReturnValue_t status = this->requestQueue->receiveMessage(&message);
    //		if(status != MessageQueueIF::EMPTY) {
@ -57,14 +56,9 @@ void PusServiceBase::handleRequestQueue() {
        // TODO: Warning?
      }

-      currentPacket.setData(dataPtr, dataLen);
-      // info << "Service " << (uint16_t) this->serviceId <<
-      //      ": new packet!" << std::endl;
+      currentPacket.setReadOnlyData(dataPtr, dataLen);

      result = this->handleRequest(currentPacket.getSubService());
-
-      // debug << "Service " << (uint16_t)this->serviceId <<
-      //    ": handleRequest returned: " << (int)return_code << std::endl;
      if (result == RETURN_OK) {
        this->verifyReporter.sendSuccessReport(tc_verification::COMPLETION_SUCCESS,
                                               &this->currentPacket);
@ -78,8 +72,6 @@ void PusServiceBase::handleRequestQueue() {
      errorParameter2 = 0;
    } else if (status == MessageQueueIF::EMPTY) {
      status = RETURN_OK;
-      // debug << "PusService " << (uint16_t)this->serviceId <<
-      //      ": no new packet." << std::endl;
      break;
    } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -127,3 +119,30 @@ ReturnValue_t PusServiceBase::initializeAfterTaskCreation() {
  // be used to get those parameters.
  return HasReturnvaluesIF::RETURN_OK;
 }
+
+void PusServiceBase::setCustomIpcStore(StorageManagerIF* ipcStore_) { ipcStore = ipcStore_; }
+
+void PusServiceBase::setCustomErrorReporter(InternalErrorReporterIF* errReporter_) {
+  errReporter = errReporter_;
+}
+
+void PusServiceBase::initializeTmHelpers(TmSendHelper& tmSendHelper, TmStoreHelper& tmStoreHelper) {
+  initializeTmSendHelper(tmSendHelper);
+  initializeTmStoreHelper(tmStoreHelper);
+}
+
+void PusServiceBase::initializeTmSendHelper(TmSendHelper& tmSendHelper) {
+  tmSendHelper.setMsgSource(requestQueue->getId());
+  tmSendHelper.setMsgDestination(requestQueue->getDefaultDestination());
+  if (errReporter == nullptr) {
+    errReporter =
+        ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+    if (errReporter != nullptr) {
+      tmSendHelper.setInternalErrorReporter(errReporter);
+    }
+  }
+}
+
+void PusServiceBase::initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const {
+  tmStoreHelper.setApid(apid);
+}
--- a/src/fsfw/tmtcservices/PusServiceBase.h
+++ b/src/fsfw/tmtcservices/PusServiceBase.h
@ -2,6 +2,8 @@
 #define FSFW_TMTCSERVICES_PUSSERVICEBASE_H_

 #include "AcceptsTelecommandsIF.h"
+#include "TmSendHelper.h"
+#include "TmStoreHelper.h"
 #include "VerificationCodes.h"
 #include "VerificationReporter.h"
 #include "fsfw/ipc/MessageQueueIF.h"
@ -51,12 +53,18 @@ class PusServiceBase : public ExecutableObjectIF,
   * @param setServiceId
   * The Service Identifier as specified in ECSS PUS.
   */
-  PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId,
-                 StorageManagerIF* ipcStore);
+  PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId);
  /**
   * The destructor is empty.
   */
  ~PusServiceBase() override;
+
+  void setCustomIpcStore(StorageManagerIF* ipcStore);
+  void setCustomErrorReporter(InternalErrorReporterIF* errReporter);
+
+  void initializeTmSendHelper(TmSendHelper& tmSendHelper);
+  void initializeTmStoreHelper(TmStoreHelper& tmStoreHelper) const;
+  void initializeTmHelpers(TmSendHelper& tmSendHelper, TmStoreHelper& tmStoreHelper);
  /**
   * @brief 	The handleRequest method shall handle any kind of Telecommand
   * 			Request immediately.
@ -139,13 +147,14 @@ class PusServiceBase : public ExecutableObjectIF,
   * sending any kind of verification message to the TC Verification Service.
   */
  VerificationReporter verifyReporter;
+
  /**
   * The current Telecommand to be processed.
   * It is deleted after handleRequest was executed.
   */
-  // TcPacketStoredPus currentPacket;
-  StorageManagerIF* ipcStore;
  PusTcReader currentPacket;
+  StorageManagerIF* ipcStore = nullptr;
+  InternalErrorReporterIF* errReporter = nullptr;

  static object_id_t packetSource;

--- a/src/fsfw/tmtcservices/TmSendHelper.cpp
+++ b/src/fsfw/tmtcservices/TmSendHelper.cpp
@ -0,0 +1,29 @@
+#include "TmSendHelper.h"
+
+#include "fsfw/ipc/MessageQueueSenderIF.h"
+
+TmSendHelper::TmSendHelper(MessageQueueId_t tmtcMsgDest, MessageQueueId_t tmtcMsgSrc,
+                           InternalErrorReporterIF *reporter)
+    : tmtcMsgDest(tmtcMsgDest), tmtcMsgSrc(tmtcMsgSrc), errReporter(reporter) {}
+
+TmSendHelper::TmSendHelper(InternalErrorReporterIF *reporter) : errReporter(reporter) {}
+
+ReturnValue_t TmSendHelper::sendPacket(const store_address_t &storeId) {
+  TmTcMessage message(storeId);
+  ReturnValue_t result = MessageQueueSenderIF::sendMessage(tmtcMsgDest, &message, tmtcMsgSrc);
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    if (errReporter != nullptr) {
+      errReporter->lostTm();
+    }
+    return result;
+  }
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
+void TmSendHelper::setMsgDestination(MessageQueueId_t msgDest) { tmtcMsgDest = msgDest; }
+
+void TmSendHelper::setMsgSource(MessageQueueId_t msgSrc) { tmtcMsgSrc = msgSrc; }
+
+void TmSendHelper::setInternalErrorReporter(InternalErrorReporterIF *reporter) {
+  errReporter = reporter;
+}
--- a/src/fsfw/tmtcservices/TmSendHelper.h
+++ b/src/fsfw/tmtcservices/TmSendHelper.h
@ -0,0 +1,26 @@
+#ifndef FSFW_TMTCPACKET_TMSENDHELPER_H
+#define FSFW_TMTCPACKET_TMSENDHELPER_H
+
+#include "TmTcMessage.h"
+#include "fsfw/internalerror/InternalErrorReporterIF.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/ipc/messageQueueDefinitions.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
+
+class TmSendHelper {
+ public:
+  explicit TmSendHelper(InternalErrorReporterIF* reporter);
+  TmSendHelper(MessageQueueId_t tmtcMsgDest, MessageQueueId_t tmtcMsgSrc,
+               InternalErrorReporterIF* reporter);
+  void setMsgDestination(MessageQueueId_t msgDest);
+  void setMsgSource(MessageQueueId_t msgSrc);
+  void setInternalErrorReporter(InternalErrorReporterIF* reporter);
+  ReturnValue_t sendPacket(const store_address_t& storeId);
+
+ private:
+  MessageQueueId_t tmtcMsgDest = MessageQueueIF::NO_QUEUE;
+  MessageQueueId_t tmtcMsgSrc = MessageQueueIF::NO_QUEUE;
+  InternalErrorReporterIF* errReporter;
+};
+
+#endif  // FSFW_TMTCPACKET_TMSENDHELPER_H
--- a/src/fsfw/tmtcservices/TmStoreHelper.cpp
+++ b/src/fsfw/tmtcservices/TmStoreHelper.cpp
@ -1,43 +1,58 @@
 #include "TmStoreHelper.h"

 #include "TmTcMessage.h"
-#include "fsfw/ipc/MessageQueueSenderIF.h"

-TmStoreHelper::TmStoreHelper(StorageManagerIF *tmStore, MessageQueueId_t tmtcMsgDest,
-                             MessageQueueId_t tmtcMsgSrc, InternalErrorReporterIF *reporter)
-    : creator(params),
-      tmtcMsgDest(tmtcMsgDest),
-      tmtcMsgSrc(tmtcMsgSrc),
-      errReporter(reporter),
-      tmStore(tmStore) {}
-
-void TmStoreHelper::preparePacket(uint16_t apid, uint8_t service, uint8_t subservice,
-                                  uint16_t counter) {
-  // TODO: Implement
-  // creator.setApid(apid);
-  params.service = service;
-  params.subservice = subservice;
-  params.messageTypeCounter = counter;
-  // TODO: Implement serialize and then serialize into the store
+TmStoreHelper::TmStoreHelper(uint16_t defaultApid, StorageManagerIF* tmStore,
+                             TimeStamperIF* timeStamper)
+    : creator(timeStamper), tmStore(tmStore) {
+  creator.setApid(defaultApid);
 }

-ReturnValue_t TmStoreHelper::sendPacket() {
-  TmTcMessage tmMessage(currentAddr);
-  ReturnValue_t result = MessageQueueSenderIF::sendMessage(tmtcMsgDest, &tmMessage, tmtcMsgSrc);
-  if (result != HasReturnvaluesIF::RETURN_OK) {
-    tmStore->deleteData(currentAddr);
-    if (errReporter != nullptr) {
-      errReporter->lostTm();
-    }
-    return result;
-  }
+TmStoreHelper::TmStoreHelper(uint16_t defaultApid, StorageManagerIF* tmStore)
+    : creator(nullptr), tmStore(tmStore) {
+  creator.setApid(defaultApid);
+}
+
+ReturnValue_t TmStoreHelper::preparePacket(uint8_t service, uint8_t subservice, uint16_t counter) {
+  // TODO: Implement
+  // creator.setApid(apid);
+  PusTmParams& params = creator.getParams();
+  params.secHeader.service = service;
+  params.secHeader.subservice = subservice;
+  params.secHeader.messageTypeCounter = counter;
+  // TODO: Implement serialize and then serialize into the store
  return HasReturnvaluesIF::RETURN_OK;
 }

-void TmStoreHelper::setMsgDestination(MessageQueueId_t msgDest) { tmtcMsgDest = msgDest; }
+StorageManagerIF* TmStoreHelper::getTmStore() { return tmStore; }

-void TmStoreHelper::setMsgSource(MessageQueueId_t msgSrc) { tmtcMsgSrc = msgSrc; }
+void TmStoreHelper::setTmStore(StorageManagerIF* store) { tmStore = store; }
+const store_address_t& TmStoreHelper::getCurrentAddr() const { return currentAddr; }
+ReturnValue_t TmStoreHelper::deletePacket() { return tmStore->deleteData(currentAddr); }

-void TmStoreHelper::setInternalErrorReporter(InternalErrorReporterIF *reporter) {
-  errReporter = reporter;
+void TmStoreHelper::setSourceDataRaw(const uint8_t* data, size_t len) {
+  PusTmParams& params = creator.getParams();
+  params.dataWrapper.type = ecss::DataTypes::RAW;
+  params.dataWrapper.dataUnion.raw.data = data;
+  params.dataWrapper.dataUnion.raw.len = len;
 }
+
+void TmStoreHelper::setSourceDataSerializable(SerializeIF* serializable) {
+  PusTmParams& params = creator.getParams();
+  params.dataWrapper.type = ecss::DataTypes::SERIALIZABLE;
+  params.dataWrapper.dataUnion.serializable = serializable;
+}
+
+ReturnValue_t TmStoreHelper::addPacketToStore() {
+  creator.updateSpLengthField();
+  uint8_t* dataPtr;
+  tmStore->getFreeElement(&currentAddr, creator.getSerializedSize(), &dataPtr);
+  size_t serLen = 0;
+  return creator.serialize(&dataPtr, &serLen, creator.getSerializedSize(),
+                           SerializeIF::Endianness::NETWORK);
+}
+
+void TmStoreHelper::setTimeStamper(TimeStamperIF* timeStamper_) {
+  creator.setTimeStamper(timeStamper_);
+}
+void TmStoreHelper::setApid(uint16_t apid) { creator.setApid(apid); }
--- a/src/fsfw/tmtcservices/TmStoreHelper.h
+++ b/src/fsfw/tmtcservices/TmStoreHelper.h
@ -4,35 +4,27 @@
 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueMessageIF.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
+#include "fsfw/timemanager/TimeStamperIF.h"
 #include "fsfw/tmtcpacket/pus/tm/PusTmCreator.h"

-class TmStoreAndSendWrapper {
-
-};
-
-class TmSendHelper {
- public:
-  TmSendHelper(MessageQueueId_t tmtcMsgDest,
-               MessageQueueId_t tmtcMsgSrc, InternalErrorReporterIF* reporter);
-  void setMsgDestination(MessageQueueId_t msgDest);
-  void setMsgSource(MessageQueueId_t msgSrc);
-  void setInternalErrorReporter(InternalErrorReporterIF* reporter);
-  ReturnValue_t sendPacket();
- private:
-  MessageQueueId_t tmtcMsgDest;
-  MessageQueueId_t tmtcMsgSrc;
-  InternalErrorReporterIF* errReporter;
-};
-
-// TODO: Serializing a packet into a store and sending the message are two different tasks
-//       Move them into separate classes
 class TmStoreHelper {
 public:
-  TmStoreHelper(StorageManagerIF* tmStore);
+  explicit TmStoreHelper(uint16_t defaultApid, StorageManagerIF* tmStore);
+  TmStoreHelper(uint16_t defaultApid, StorageManagerIF* tmStore, TimeStamperIF* timeStamper);
+
+  ReturnValue_t preparePacket(uint8_t service, uint8_t subservice, uint16_t counter);
+
+  void setTimeStamper(TimeStamperIF* timeStamper);
+  [[nodiscard]] const store_address_t& getCurrentAddr() const;
+  void setSourceDataRaw(const uint8_t* data, size_t len);
+  void setSourceDataSerializable(SerializeIF* serializable);
+  void setApid(uint16_t apid);
+  StorageManagerIF* getTmStore();
+  void setTmStore(StorageManagerIF* store);
+  ReturnValue_t addPacketToStore();
+  ReturnValue_t deletePacket();

-  void preparePacket(uint16_t apid, uint8_t service, uint8_t subservice, uint16_t counter);
 private:
-  PusTmParams params{};
  PusTmCreator creator;
  store_address_t currentAddr{};
  StorageManagerIF* tmStore;
--- a/src/fsfw/tmtcservices/VerificationReporter.cpp
+++ b/src/fsfw/tmtcservices/VerificationReporter.cpp
@ -13,17 +13,17 @@ VerificationReporter::VerificationReporter() : acknowledgeQueue(MessageQueueIF::

 VerificationReporter::~VerificationReporter() = default;

-void VerificationReporter::sendSuccessReport(uint8_t set_report_id, PusTcReader* currentPacket,
+void VerificationReporter::sendSuccessReport(uint8_t set_report_id, PusTcReader* correspondingTc,
                                             uint8_t set_step) {
  if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
    this->initialize();
  }
-  if (currentPacket == nullptr) {
+  if (correspondingTc == nullptr) {
    return;
  }
-  PusVerificationMessage message(set_report_id, currentPacket->getAcknowledgeFlags(),
-                                 currentPacket->getPacketIdRaw(),
-                                 currentPacket->getPacketSeqCtrlRaw(), 0, set_step);
+  PusVerificationMessage message(set_report_id, correspondingTc->getAcknowledgeFlags(),
+                                 correspondingTc->getPacketIdRaw(),
+                                 correspondingTc->getPacketSeqCtrlRaw(), 0, set_step);
  ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
  if (status != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@ -50,18 +50,18 @@ void VerificationReporter::sendSuccessReport(uint8_t set_report_id, uint8_t ackF
  }
 }

-void VerificationReporter::sendFailureReport(uint8_t report_id, PusTcReader* currentPacket,
+void VerificationReporter::sendFailureReport(uint8_t report_id, PusTcReader* correspondingTc,
                                             ReturnValue_t error_code, uint8_t step,
                                             uint32_t parameter1, uint32_t parameter2) {
  if (acknowledgeQueue == MessageQueueIF::NO_QUEUE) {
    this->initialize();
  }
-  if (currentPacket == nullptr) {
+  if (correspondingTc == nullptr) {
    return;
  }
  PusVerificationMessage message(
-      report_id, currentPacket->getAcknowledgeFlags(), currentPacket->getPacketIdRaw(),
-      currentPacket->getPacketSeqCtrlRaw(), error_code, step, parameter1, parameter2);
+      report_id, correspondingTc->getAcknowledgeFlags(), correspondingTc->getPacketIdRaw(),
+      correspondingTc->getPacketSeqCtrlRaw(), error_code, step, parameter1, parameter2);
  ReturnValue_t status = MessageQueueSenderIF::sendMessage(acknowledgeQueue, &message);
  if (status != HasReturnvaluesIF::RETURN_OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
--- a/src/fsfw/tmtcservices/VerificationReporter.h
+++ b/src/fsfw/tmtcservices/VerificationReporter.h
@ -27,11 +27,11 @@ class VerificationReporter {
  VerificationReporter();
  virtual ~VerificationReporter();

-  void sendSuccessReport(uint8_t set_report_id, PusTcReader* current_packet, uint8_t set_step = 0);
+  void sendSuccessReport(uint8_t set_report_id, PusTcReader* correspondingTc, uint8_t set_step = 0);
  void sendSuccessReport(uint8_t set_report_id, uint8_t ackFlags, uint16_t tcPacketId,
                         uint16_t tcSequenceControl, uint8_t set_step = 0);

-  void sendFailureReport(uint8_t report_id, PusTcReader* current_packet,
+  void sendFailureReport(uint8_t report_id, PusTcReader* correspondingTc,
                         ReturnValue_t error_code = 0, uint8_t step = 0, uint32_t parameter1 = 0,
                         uint32_t parameter2 = 0);
  void sendFailureReport(uint8_t report_id, uint8_t ackFlags, uint16_t tcPacketId,
--- a/src/fsfw/tmtcservices/sendAndStoreHelper.h
+++ b/src/fsfw/tmtcservices/sendAndStoreHelper.h
@ -0,0 +1,54 @@
+#ifndef FSFW_TMTCSERVICES_SENDANDSTOREHELPER_H
+#define FSFW_TMTCSERVICES_SENDANDSTOREHELPER_H
+
+#include "TmSendHelper.h"
+#include "TmStoreHelper.h"
+
+namespace tm {
+
+ReturnValue_t storeAndSendTmPacket(TmStoreHelper& storeHelper, TmSendHelper& sendHelper) {
+  storeHelper.addPacketToStore();
+  ReturnValue_t result = sendHelper.sendPacket(storeHelper.getCurrentAddr());
+  if (result != HasReturnvaluesIF::RETURN_OK) {
+    storeHelper.deletePacket();
+  }
+  return result;
+}
+
+class SourceDataWithObjectIdPrefix : public SerializeIF {
+ public:
+  SourceDataWithObjectIdPrefix(object_id_t objectId, const uint8_t* srcData, size_t srcDataLen)
+      : objectId(objectId), srcData(srcData), srcDataLen(srcDataLen) {}
+  ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize,
+                          Endianness streamEndianness) const override {
+    if (*size + getSerializedSize() > maxSize) {
+      return SerializeIF::BUFFER_TOO_SHORT;
+    }
+    ReturnValue_t result =
+        SerializeAdapter::serialize(&objectId, buffer, size, maxSize, streamEndianness);
+    if (result != HasReturnvaluesIF::RETURN_OK) {
+      return result;
+    }
+    std::memcpy(*buffer, srcData, srcDataLen);
+    *buffer += srcDataLen;
+    *size += srcDataLen;
+    return HasReturnvaluesIF::RETURN_OK;
+  }
+
+  [[nodiscard]] size_t getSerializedSize() const override { return sizeof(objectId) + srcDataLen; }
+
+  ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+                            Endianness streamEndianness) override {
+    // TODO: Implement
+    return HasReturnvaluesIF::RETURN_FAILED;
+  }
+
+ private:
+  object_id_t objectId;
+  const uint8_t* srcData;
+  size_t srcDataLen;
+};
+
+}  // namespace tm
+
+#endif  // FSFW_TMTCSERVICES_SENDANDSTOREHELPER_H
--- a/unittests/CatchFactory.cpp
+++ b/unittests/CatchFactory.cpp
@ -70,8 +70,6 @@ void Factory::setStaticFrameworkObjectIds() {
  LocalDataPoolManager::defaultHkDestination = objects::HK_RECEIVER_MOCK;

  DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT;
-
-  TmPacketBase::timeStamperId = objects::NO_OBJECT;
 }

 #endif