dumb bug

Reviewed-on: #106

CHANGELOG.md

@@ -8,16 +8,30 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
 
 # [unreleased]
 
+# [v6.0.0]
+
+## Added
+
+- DHB TM handler `handleDeviceTM` renamed to `handleDeviceTm` and now takes
+  `util::DataWrapper` as the data input argument. This allows more flexibility in the possible
+  types of telemetry.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/669
+- Add `util::DataWrapper` class inside the `util` module. This is a tagged union which allows
+  to specify raw data either as a classic C-style raw pointer and size or as a `SerializeIF`
+  pointer.
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/668
+- Add new `UnsignedByteField` class
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660
+
 ## Changes
 
 - Removed `HasReturnvaluesIF` class in favor of `returnvalue` namespace with `OK` and `FAILED`
   constants.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/659
-
-## Added
-
-- Add new `UnsignedByteField` class
-  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660
+- Overhaul of the TMTC stack, including various changes and improvements
+  for other modules
+  PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/655
+  which also includes a migration guide
 
 # [v5.0.0] 25.07.2022
 
@@ -152,7 +166,6 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593
     - https://gitlab.kitware.com/cmake/cmake/-/issues/21696
   Easiest solution for now: Keep this option OFF by default.
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616
-- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1
 - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information
   inside `fsfw/version.h`
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559
@@ -167,6 +180,17 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593
   PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590
 - `Subsystem`: New API to add table and sequence entries
 
+## HAL
+
+- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a
+  lot more sense because each ComIF should be responsible for one SPI bus.
+- SPI: Move the empty transfer to update the line polarity to separate function. This means
+  it is not automatically called when calling the setter function for SPI speed and mode.
+  The user should call this function after locking the CS mutex if multiple SPI devices with
+  differing speeds and modes are attached to one bus.
+- SPI: Getter functions for SPI speed and mode.
+- I2C: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1.
+
 ## Fixed
 
 - TCP TMTC Server: `MutexGuard` was not created properly in

CMakeLists.txt

@@ -195,7 +195,7 @@ message(
 )
 
 # Check whether the user has already installed ETL first
-find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET)
+find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET)
 # Not installed, so use FetchContent to download and provide etl
 if(NOT ${FSFW_ETL_LIB_NAME}_FOUND)
   message(

automation/Jenkinsfile

@@ -1,6 +1,6 @@
 pipeline {
     environment {
-        BUILDDIR = 'build-tests'
+        BUILDDIR = 'cmake-build-tests'
     }
     agent {
         docker { image 'fsfw-ci:d3'}

misc/defaultcfg/fsfwconfig/objects/FsfwFactory.cpp

@@ -35,8 +35,8 @@ void Factory::produceFsfwObjects(void) {
 }
 
 void Factory::setStaticFrameworkObjectIds() {
-	PusServiceBase::packetSource = objects::NO_OBJECT;
-	PusServiceBase::packetDestination = objects::NO_OBJECT;
+	PusServiceBase::PUS_DISTRIBUTOR = objects::NO_OBJECT;
+	PusServiceBase::PACKET_DESTINATION = objects::NO_OBJECT;
 
 	CommandingServiceBase::defaultPacketSource = objects::NO_OBJECT;
 	CommandingServiceBase::defaultPacketDestination = objects::NO_OBJECT;

scripts/helper.py

@@ -13,7 +13,7 @@ from shutil import which
 from typing import List
 
 
-UNITTEST_FOLDER_NAME = "build-tests"
+UNITTEST_FOLDER_NAME = "cmake-build-tests"
 DOCS_FOLDER_NAME = "build-docs"
 
 

src/fsfw/action/CommandActionHelper.h

@@ -16,8 +16,8 @@ class CommandActionHelper {
  public:
   explicit CommandActionHelper(CommandsActionsIF* owner);
   virtual ~CommandActionHelper();
-  ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t* data,
-                              uint32_t size);
+  ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId,
+                              const uint8_t* data = nullptr, uint32_t size = 0);
   ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF* data);
   ReturnValue_t initialize();
   ReturnValue_t handleReply(CommandMessage* reply);

src/fsfw/cfdp/CFDPMessage.cpp

@@ -1,17 +0,0 @@
-#include "CFDPMessage.h"
-
-CFDPMessage::CFDPMessage() {}
-
-CFDPMessage::~CFDPMessage() {}
-
-void CFDPMessage::setCommand(CommandMessage *message, store_address_t cfdpPacket) {
-  message->setParameter(cfdpPacket.raw);
-}
-
-store_address_t CFDPMessage::getStoreId(const CommandMessage *message) {
-  store_address_t storeAddressCFDPPacket;
-  storeAddressCFDPPacket = message->getParameter();
-  return storeAddressCFDPPacket;
-}
-
-void CFDPMessage::clear(CommandMessage *message) {}

src/fsfw/cfdp/CMakeLists.txt

@@ -1,4 +1,4 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE CFDPHandler.cpp CFDPMessage.cpp)
+target_sources(${LIB_FSFW_NAME} PRIVATE CfdpHandler.cpp CfdpMessage.cpp)
 
 add_subdirectory(pdu)
 add_subdirectory(tlv)

src/fsfw/cfdp/CfdpHandler.cpp

@@ -1,24 +1,25 @@
-#include "fsfw/cfdp/CFDPHandler.h"
+#include "fsfw/cfdp/CfdpHandler.h"
 
-#include "fsfw/cfdp/CFDPMessage.h"
+#include "fsfw/cfdp/CfdpMessage.h"
 #include "fsfw/ipc/CommandMessage.h"
 #include "fsfw/ipc/QueueFactory.h"
-#include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/storagemanager/storeAddress.h"
 #include "fsfw/tmtcservices/AcceptsTelemetryIF.h"
 
-object_id_t CFDPHandler::packetSource = 0;
-object_id_t CFDPHandler::packetDestination = 0;
+object_id_t CfdpHandler::packetSource = 0;
+object_id_t CfdpHandler::packetDestination = 0;
 
-CFDPHandler::CFDPHandler(object_id_t setObjectId, CFDPDistributor* dist)
+CfdpHandler::CfdpHandler(object_id_t setObjectId, CFDPDistributor* dist)
     : SystemObject(setObjectId) {
-  requestQueue = QueueFactory::instance()->createMessageQueue(CFDP_HANDLER_MAX_RECEPTION);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  requestQueue = QueueFactory::instance()->createMessageQueue(
+      CFDP_HANDLER_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
   distributor = dist;
 }
 
-CFDPHandler::~CFDPHandler() {}
+CfdpHandler::~CfdpHandler() = default;
 
-ReturnValue_t CFDPHandler::initialize() {
+ReturnValue_t CfdpHandler::initialize() {
   ReturnValue_t result = SystemObject::initialize();
   if (result != returnvalue::OK) {
     return result;
@@ -27,7 +28,7 @@ ReturnValue_t CFDPHandler::initialize() {
   return returnvalue::OK;
 }
 
-ReturnValue_t CFDPHandler::handleRequest(store_address_t storeId) {
+ReturnValue_t CfdpHandler::handleRequest(store_address_t storeId) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
   sif::debug << "CFDPHandler::handleRequest" << std::endl;
@@ -41,17 +42,17 @@ ReturnValue_t CFDPHandler::handleRequest(store_address_t storeId) {
   return returnvalue::OK;
 }
 
-ReturnValue_t CFDPHandler::performOperation(uint8_t opCode) {
+ReturnValue_t CfdpHandler::performOperation(uint8_t opCode) {
   ReturnValue_t status = returnvalue::OK;
   CommandMessage currentMessage;
   for (status = this->requestQueue->receiveMessage(&currentMessage); status == returnvalue::OK;
        status = this->requestQueue->receiveMessage(&currentMessage)) {
-    store_address_t storeId = CFDPMessage::getStoreId(&currentMessage);
+    store_address_t storeId = CfdpMessage::getStoreId(&currentMessage);
     this->handleRequest(storeId);
   }
   return returnvalue::OK;
 }
 
-uint16_t CFDPHandler::getIdentifier() { return 0; }
+uint16_t CfdpHandler::getIdentifier() { return 0; }
 
-MessageQueueId_t CFDPHandler::getRequestQueue() { return this->requestQueue->getId(); }
+MessageQueueId_t CfdpHandler::getRequestQueue() { return this->requestQueue->getId(); }

src/fsfw/cfdp/CfdpHandler.h

@@ -12,15 +12,15 @@ namespace Factory {
 void setStaticFrameworkObjectIds();
 }
 
-class CFDPHandler : public ExecutableObjectIF, public AcceptsTelecommandsIF, public SystemObject {
+class CfdpHandler : public ExecutableObjectIF, public AcceptsTelecommandsIF, public SystemObject {
   friend void(Factory::setStaticFrameworkObjectIds)();
 
  public:
-  CFDPHandler(object_id_t setObjectId, CFDPDistributor* distributor);
+  CfdpHandler(object_id_t setObjectId, CFDPDistributor* distributor);
   /**
    * The destructor is empty.
    */
-  virtual ~CFDPHandler();
+  virtual ~CfdpHandler();
 
   virtual ReturnValue_t handleRequest(store_address_t storeId);
 
@@ -42,7 +42,7 @@ class CFDPHandler : public ExecutableObjectIF, public AcceptsTelecommandsIF, pub
    * The current CFDP packet to be processed.
    * It is deleted after handleRequest was executed.
    */
-  CFDPPacketStored currentPacket;
+  CfdpPacketStored currentPacket;
 
   static object_id_t packetSource;
 

src/fsfw/cfdp/CfdpMessage.cpp

@@ -0,0 +1,17 @@
+#include "CfdpMessage.h"
+
+CfdpMessage::CfdpMessage() = default;
+
+CfdpMessage::~CfdpMessage() = default;
+
+void CfdpMessage::setCommand(CommandMessage *message, store_address_t cfdpPacket) {
+  message->setParameter(cfdpPacket.raw);
+}
+
+store_address_t CfdpMessage::getStoreId(const CommandMessage *message) {
+  store_address_t storeId;
+  storeId = static_cast<store_address_t>(message->getParameter());
+  return storeId;
+}
+
+void CfdpMessage::clear(CommandMessage *message) {}

src/fsfw/cfdp/CfdpMessage.h

@@ -5,14 +5,14 @@
 #include "fsfw/objectmanager/ObjectManagerIF.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
 
-class CFDPMessage {
+class CfdpMessage {
  private:
-  CFDPMessage();
+  CfdpMessage();
 
  public:
   static const uint8_t MESSAGE_ID = messagetypes::CFDP;
 
-  virtual ~CFDPMessage();
+  virtual ~CfdpMessage();
   static void setCommand(CommandMessage* message, store_address_t cfdpPacket);
 
   static store_address_t getStoreId(const CommandMessage* message);

src/fsfw/controller/ControllerBase.cpp

@@ -13,7 +13,9 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId,
       submode(SUBMODE_NONE),
       modeHelper(this),
       healthHelper(this, setObjectId) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      commandQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }

src/fsfw/datalinklayer/MapPacketExtraction.cpp

@@ -6,7 +6,7 @@
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/storagemanager/StorageManagerIF.h"
-#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
 #include "fsfw/tmtcservices/AcceptsTelecommandsIF.h"
 #include "fsfw/tmtcservices/TmTcMessage.h"
 

src/fsfw/datapoollocal/HasLocalDataPoolIF.h

@@ -79,8 +79,7 @@ class HasLocalDataPoolIF {
    * @param clearMessage  If this is set to true, the pool manager will take care of
    *                      clearing the store automatically
    */
-  virtual void handleChangedDataset(sid_t sid,
-                                    store_address_t storeId = storeId::INVALID_STORE_ADDRESS,
+  virtual void handleChangedDataset(sid_t sid, store_address_t storeId = store_address_t::invalid(),
                                     bool* clearMessage = nullptr) {
     if (clearMessage != nullptr) {
       *clearMessage = true;
@@ -100,7 +99,7 @@ class HasLocalDataPoolIF {
    *                      after the callback.
    */
   virtual void handleChangedPoolVariable(gp_id_t gpid,
-                                         store_address_t storeId = storeId::INVALID_STORE_ADDRESS,
+                                         store_address_t storeId = store_address_t::invalid(),
                                          bool* clearMessage = nullptr) {
     if (clearMessage != nullptr) {
       *clearMessage = true;

src/fsfw/datapoollocal/LocalDataPoolManager.cpp

@@ -1,6 +1,5 @@
 #include "fsfw/datapoollocal/LocalDataPoolManager.h"
 
-#include <array>
 #include <cmath>
 
 #include "fsfw/datapoollocal.h"
@@ -15,6 +14,7 @@
 #include "internal/HasLocalDpIFManagerAttorney.h"
 #include "internal/LocalPoolDataSetAttorney.h"
 
+// TODO: Get rid of this. This should be a constructor argument, not something hardcoded in any way
 object_id_t LocalDataPoolManager::defaultHkDestination = objects::PUS_SERVICE_3_HOUSEKEEPING;
 
 LocalDataPoolManager::LocalDataPoolManager(HasLocalDataPoolIF* owner, MessageQueueIF* queueToUse,
@@ -57,7 +57,7 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) {
   }
 
   if (defaultHkDestination != objects::NO_OBJECT) {
-    AcceptsHkPacketsIF* hkPacketReceiver =
+    auto* hkPacketReceiver =
         ObjectManager::instance()->get<AcceptsHkPacketsIF>(defaultHkDestination);
     if (hkPacketReceiver != nullptr) {
       hkDestinationId = hkPacketReceiver->getHkQueue();
@@ -209,9 +209,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(HkReceiver& recei
     }
 
     /* Prepare and send update snapshot */
-    timeval now;
+    timeval now{};
     Clock::getClock_timeval(&now);
-    CCSDSTime::CDS_short cds;
+    CCSDSTime::CDS_short cds{};
     CCSDSTime::convertToCcsds(&cds, &now);
     HousekeepingSnapshot updatePacket(
         reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
@@ -245,9 +245,9 @@ ReturnValue_t LocalDataPoolManager::handleNotificationSnapshot(HkReceiver& recei
     }
 
     /* Prepare and send update snapshot */
-    timeval now;
+    timeval now{};
     Clock::getClock_timeval(&now);
-    CCSDSTime::CDS_short cds;
+    CCSDSTime::CDS_short cds{};
     CCSDSTime::convertToCcsds(&cds, &now);
     HousekeepingSnapshot updatePacket(
         reinterpret_cast<uint8_t*>(&cds), sizeof(cds),
@@ -291,12 +291,7 @@ ReturnValue_t LocalDataPoolManager::addUpdateToStore(HousekeepingSnapshot& updat
 
 void LocalDataPoolManager::handleChangeResetLogic(DataType type, DataId dataId,
                                                   MarkChangedIF* toReset) {
-  if (hkUpdateResetList == nullptr) {
-    /* Config error */
-    return;
-  }
-  HkUpdateResetList& listRef = *hkUpdateResetList;
-  for (auto& changeInfo : listRef) {
+  for (auto& changeInfo : hkUpdateResetList) {
     if (changeInfo.dataType != type) {
       continue;
     }
@@ -326,38 +321,37 @@ void LocalDataPoolManager::handleChangeResetLogic(DataType type, DataId dataId,
 }
 
 void LocalDataPoolManager::resetHkUpdateResetHelper() {
-  if (hkUpdateResetList == nullptr) {
-    return;
-  }
-
-  for (auto& changeInfo : *hkUpdateResetList) {
+  for (auto& changeInfo : hkUpdateResetList) {
     changeInfo.currentUpdateCounter = changeInfo.updateCounter;
   }
 }
 
-ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid, bool enableReporting,
-                                                               float collectionInterval,
-                                                               bool isDiagnostics,
-                                                               object_id_t packetDestination) {
-  AcceptsHkPacketsIF* hkReceiverObject =
-      ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
-  if (hkReceiverObject == nullptr) {
-    printWarningOrError(sif::OutputTypes::OUT_WARNING, "subscribeForPeriodicPacket",
-                        QUEUE_OR_DESTINATION_INVALID);
-    return QUEUE_OR_DESTINATION_INVALID;
-  }
+ReturnValue_t LocalDataPoolManager::subscribeForRegularPeriodicPacket(
+    subdp::RegularHkPeriodicParams params) {
+  return subscribeForPeriodicPacket(params);
+}
 
+ReturnValue_t LocalDataPoolManager::subscribeForDiagPeriodicPacket(
+    subdp::DiagnosticsHkPeriodicParams params) {
+  return subscribeForPeriodicPacket(params);
+}
+
+ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(subdp::ParamsBase& params) {
   struct HkReceiver hkReceiver;
-  hkReceiver.dataId.sid = sid;
+  hkReceiver.dataId.sid = params.sid;
   hkReceiver.reportingType = ReportingType::PERIODIC;
   hkReceiver.dataType = DataType::DATA_SET;
-  hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+  if (params.receiver == MessageQueueIF::NO_QUEUE) {
+    hkReceiver.destinationQueue = hkDestinationId;
+  } else {
+    hkReceiver.destinationQueue = params.receiver;
+  }
 
-  LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+  LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, params.sid);
   if (dataSet != nullptr) {
-    LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enableReporting);
-    LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
-    LocalPoolDataSetAttorney::initializePeriodicHelper(*dataSet, collectionInterval,
+    LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, params.enableReporting);
+    LocalPoolDataSetAttorney::setDiagnostic(*dataSet, params.isDiagnostics());
+    LocalPoolDataSetAttorney::initializePeriodicHelper(*dataSet, params.collectionInterval,
                                                        owner->getPeriodicOperationFrequency());
   }
 
@@ -365,27 +359,30 @@ ReturnValue_t LocalDataPoolManager::subscribeForPeriodicPacket(sid_t sid, bool e
   return returnvalue::OK;
 }
 
-ReturnValue_t LocalDataPoolManager::subscribeForUpdatePacket(sid_t sid, bool isDiagnostics,
-                                                             bool reportingEnabled,
-                                                             object_id_t packetDestination) {
-  AcceptsHkPacketsIF* hkReceiverObject =
-      ObjectManager::instance()->get<AcceptsHkPacketsIF>(packetDestination);
-  if (hkReceiverObject == nullptr) {
-    printWarningOrError(sif::OutputTypes::OUT_WARNING, "subscribeForPeriodicPacket",
-                        QUEUE_OR_DESTINATION_INVALID);
-    return QUEUE_OR_DESTINATION_INVALID;
-  }
+ReturnValue_t LocalDataPoolManager::subscribeForRegularUpdatePacket(
+    subdp::RegularHkUpdateParams params) {
+  return subscribeForUpdatePacket(params);
+}
+ReturnValue_t LocalDataPoolManager::subscribeForDiagUpdatePacket(
+    subdp::DiagnosticsHkUpdateParams params) {
+  return subscribeForUpdatePacket(params);
+}
 
+ReturnValue_t LocalDataPoolManager::subscribeForUpdatePacket(subdp::ParamsBase& params) {
   struct HkReceiver hkReceiver;
-  hkReceiver.dataId.sid = sid;
+  hkReceiver.dataId.sid = params.sid;
   hkReceiver.reportingType = ReportingType::UPDATE_HK;
   hkReceiver.dataType = DataType::DATA_SET;
-  hkReceiver.destinationQueue = hkReceiverObject->getHkQueue();
+  if (params.receiver == MessageQueueIF::NO_QUEUE) {
+    hkReceiver.destinationQueue = hkDestinationId;
+  } else {
+    hkReceiver.destinationQueue = params.receiver;
+  }
 
-  LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid);
+  LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, params.sid);
   if (dataSet != nullptr) {
     LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, true);
-    LocalPoolDataSetAttorney::setDiagnostic(*dataSet, isDiagnostics);
+    LocalPoolDataSetAttorney::setDiagnostic(*dataSet, params.isDiagnostics());
   }
 
   hkReceivers.push_back(hkReceiver);
@@ -436,11 +433,7 @@ ReturnValue_t LocalDataPoolManager::subscribeForVariableUpdateMessage(
 }
 
 void LocalDataPoolManager::handleHkUpdateResetListInsertion(DataType dataType, DataId dataId) {
-  if (hkUpdateResetList == nullptr) {
-    hkUpdateResetList = new std::vector<struct HkUpdateResetHelper>();
-  }
-
-  for (auto& updateResetStruct : *hkUpdateResetList) {
+  for (auto& updateResetStruct : hkUpdateResetList) {
     if (dataType == DataType::DATA_SET) {
       if (updateResetStruct.dataId.sid == dataId.sid) {
         updateResetStruct.updateCounter++;
@@ -464,7 +457,7 @@ void LocalDataPoolManager::handleHkUpdateResetListInsertion(DataType dataType, D
   } else {
     hkUpdateResetHelper.dataId.localPoolId = dataId.localPoolId;
   }
-  hkUpdateResetList->push_back(hkUpdateResetHelper);
+  hkUpdateResetList.push_back(hkUpdateResetHelper);
 }
 
 ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(CommandMessage* message) {
@@ -577,6 +570,10 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(CommandMessage* me
 
   CommandMessage reply;
   if (result != returnvalue::OK) {
+    if (result == WRONG_HK_PACKET_TYPE) {
+      printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage",
+                          WRONG_HK_PACKET_TYPE);
+    }
     HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result);
   } else {
     HousekeepingMessage::setHkRequestSuccessReply(&reply, sid);
@@ -639,6 +636,7 @@ ReturnValue_t LocalDataPoolManager::generateHousekeepingPacket(sid_t sid,
       /* Error, all destinations invalid */
       printWarningOrError(sif::OutputTypes::OUT_WARNING, "generateHousekeepingPacket",
                           QUEUE_OR_DESTINATION_INVALID);
+      return QUEUE_OR_DESTINATION_INVALID;
     }
     destination = hkDestinationId;
   }
@@ -815,9 +813,7 @@ void LocalDataPoolManager::clearReceiversList() {
   /* Clear the vector completely and releases allocated memory. */
   HkReceivers().swap(hkReceivers);
   /* Also clear the reset helper if it exists */
-  if (hkUpdateResetList != nullptr) {
-    HkUpdateResetList().swap(*hkUpdateResetList);
-  }
+  HkUpdateResetList().swap(hkUpdateResetList);
 }
 
 MutexIF* LocalDataPoolManager::getLocalPoolMutex() { return this->mutex; }
@@ -833,6 +829,8 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
       errorPrint = "Dataset not found";
     } else if (error == POOLOBJECT_NOT_FOUND) {
       errorPrint = "Pool Object not found";
+    } else if (error == WRONG_HK_PACKET_TYPE) {
+      errorPrint = "Wrong Packet Type";
     } else if (error == returnvalue::FAILED) {
       if (outputType == sif::OutputTypes::OUT_WARNING) {
         errorPrint = "Generic Warning";
@@ -877,3 +875,7 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType,
 }
 
 LocalDataPoolManager* LocalDataPoolManager::getPoolManagerHandle() { return this; }
+
+void LocalDataPoolManager::setHkDestinationId(MessageQueueId_t hkDestId) {
+  hkDestinationId = hkDestId;
+}

src/fsfw/datapoollocal/LocalDataPoolManager.h

@@ -8,6 +8,7 @@
 #include "ProvidesDataPoolSubscriptionIF.h"
 #include "fsfw/datapool/DataSetIF.h"
 #include "fsfw/datapool/PoolEntry.h"
+#include "fsfw/housekeeping/AcceptsHkPacketsIF.h"
 #include "fsfw/housekeeping/HousekeepingMessage.h"
 #include "fsfw/housekeeping/HousekeepingPacketDownlink.h"
 #include "fsfw/housekeeping/PeriodicHousekeepingHelper.h"
@@ -80,7 +81,9 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    */
   LocalDataPoolManager(HasLocalDataPoolIF* owner, MessageQueueIF* queueToUse,
                        bool appendValidityBuffer = true);
-  virtual ~LocalDataPoolManager();
+  ~LocalDataPoolManager() override;
+
+  void setHkDestinationId(MessageQueueId_t hkDestId);
 
   /**
    * Assigns the queue to use. Make sure to call this in the #initialize
@@ -112,31 +115,6 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    */
   virtual ReturnValue_t performHkOperation();
 
-  /**
-   * @brief   Subscribe for the generation of periodic packets.
-   * @details
-   * This subscription mechanism will generally be used by the data creator
-   * to generate housekeeping packets which are downlinked directly.
-   * @return
-   */
-  ReturnValue_t subscribeForPeriodicPacket(
-      sid_t sid, bool enableReporting, float collectionInterval, bool isDiagnostics,
-      object_id_t packetDestination = defaultHkDestination) override;
-
-  /**
-   * @brief   Subscribe for the  generation of packets if the dataset
-   *          is marked as changed.
-   * @details
-   * This subscription mechanism will generally be used by the data creator.
-   * @param sid
-   * @param isDiagnostics
-   * @param packetDestination
-   * @return
-   */
-  ReturnValue_t subscribeForUpdatePacket(
-      sid_t sid, bool reportingEnabled, bool isDiagnostics,
-      object_id_t packetDestination = defaultHkDestination) override;
-
   /**
    * @brief   Subscribe for a notification message which will be sent
    *          if a dataset has changed.
@@ -151,7 +129,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    * Otherwise, only an notification message is sent.
    * @return
    */
-  ReturnValue_t subscribeForSetUpdateMessage(const uint32_t setId, object_id_t destinationObject,
+  ReturnValue_t subscribeForSetUpdateMessage(uint32_t setId, object_id_t destinationObject,
                                              MessageQueueId_t targetQueueId,
                                              bool generateSnapshot) override;
 
@@ -169,7 +147,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    * Otherwise, only an notification message is sent.
    * @return
    */
-  ReturnValue_t subscribeForVariableUpdateMessage(const lp_id_t localPoolId,
+  ReturnValue_t subscribeForVariableUpdateMessage(lp_id_t localPoolId,
                                                   object_id_t destinationObject,
                                                   MessageQueueId_t targetQueueId,
                                                   bool generateSnapshot) override;
@@ -252,7 +230,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    */
   void clearReceiversList();
 
-  object_id_t getCreatorObjectId() const;
+  [[nodiscard]] object_id_t getCreatorObjectId() const;
 
   /**
    * Get the pointer to the mutex. Can be used to lock the data pool
@@ -262,9 +240,17 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
    */
   MutexIF* getMutexHandle();
 
-  virtual LocalDataPoolManager* getPoolManagerHandle() override;
+  LocalDataPoolManager* getPoolManagerHandle() override;
+  ReturnValue_t subscribeForRegularPeriodicPacket(subdp::RegularHkPeriodicParams params) override;
+  ReturnValue_t subscribeForDiagPeriodicPacket(subdp::DiagnosticsHkPeriodicParams params) override;
+
+  ReturnValue_t subscribeForRegularUpdatePacket(subdp::RegularHkUpdateParams params) override;
+  ReturnValue_t subscribeForDiagUpdatePacket(subdp::DiagnosticsHkUpdateParams params) override;
 
  protected:
+  ReturnValue_t subscribeForPeriodicPacket(subdp::ParamsBase& params);
+  ReturnValue_t subscribeForUpdatePacket(subdp::ParamsBase& params);
+
   /** Core data structure for the actual pool data */
   localpool::DataPool localPoolMap;
   /** Every housekeeping data manager has a mutex to protect access
@@ -312,8 +298,8 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces
 
   using HkUpdateResetList = std::vector<struct HkUpdateResetHelper>;
   /** This list is used to manage creating multiple update packets and only resetting
-  the update flag if all of them were created. Will only be created when needed. */
-  HkUpdateResetList* hkUpdateResetList = nullptr;
+  the update flag if all of them were created. */
+  HkUpdateResetList hkUpdateResetList = HkUpdateResetList();
 
   /** This is the map holding the actual data. Should only be initialized
    * once ! */

src/fsfw/datapoollocal/LocalPoolDataSetBase.h

@@ -162,6 +162,7 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF {
   object_id_t getCreatorObjectId();
 
   bool getReportingEnabled() const;
+  void setReportingEnabled(bool enabled);
 
   /**
    * Returns the current periodic HK generation interval this set
@@ -189,7 +190,6 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF {
    * Used for periodic generation.
    */
   bool reportingEnabled = false;
-  void setReportingEnabled(bool enabled);
 
   void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval,
                                 uint8_t nonDiagIntervalFactor = 5);

src/fsfw/datapoollocal/LocalPoolObjectBase.cpp

@@ -48,12 +48,12 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId,
   if (hkOwner == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "LocalPoolVariable: The supplied pool owner 0x" << std::hex << poolOwner
-               << std::dec << " did not implement the correct interface "
+               << std::dec << "  does not exist or does not implement the correct interface "
                << "HasLocalDataPoolIF" << std::endl;
 #else
     sif::printError(
-        "LocalPoolVariable: The supplied pool owner 0x%08x did not implement the correct "
-        "interface HasLocalDataPoolIF\n",
+        "LocalPoolVariable: The supplied pool owner 0x%08x does not exist or does not implement "
+        "the correct interface HasLocalDataPoolIF\n",
         poolOwner);
 #endif
     return;

src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h

@@ -1,24 +1,90 @@
 #ifndef FSFW_DATAPOOLLOCAL_PROVIDESDATAPOOLSUBSCRIPTION_H_
 #define FSFW_DATAPOOLLOCAL_PROVIDESDATAPOOLSUBSCRIPTION_H_
 
-#include "../ipc/messageQueueDefinitions.h"
-#include "../returnvalues/returnvalue.h"
+#include "fsfw/housekeeping/AcceptsHkPacketsIF.h"
+#include "fsfw/ipc/MessageQueueIF.h"
+#include "fsfw/ipc/messageQueueDefinitions.h"
+#include "fsfw/returnvalues/returnvalue.h"
 #include "localPoolDefinitions.h"
 
+namespace subdp {
+
+struct ParamsBase {
+  ParamsBase(sid_t sid, bool enableReporting, float collectionInterval, bool diagnostics)
+      : sid(sid),
+        enableReporting(enableReporting),
+        collectionInterval(collectionInterval),
+        diagnostics(diagnostics) {}
+
+  [[nodiscard]] bool isDiagnostics() const { return diagnostics; }
+
+  sid_t sid;
+  bool enableReporting;
+  float collectionInterval;
+  MessageQueueId_t receiver = MessageQueueIF::NO_QUEUE;
+
+ protected:
+  bool diagnostics;
+};
+
+struct RegularHkPeriodicParams : public ParamsBase {
+  RegularHkPeriodicParams(sid_t sid, bool enableReporting, float collectionInterval)
+      : ParamsBase(sid, enableReporting, collectionInterval, false) {}
+};
+
+struct DiagnosticsHkPeriodicParams : public ParamsBase {
+  DiagnosticsHkPeriodicParams(sid_t sid, bool enableReporting, float collectionInterval)
+      : ParamsBase(sid, enableReporting, collectionInterval, true) {}
+};
+
+struct RegularHkUpdateParams : public ParamsBase {
+  RegularHkUpdateParams(sid_t sid, bool enableReporting)
+      : ParamsBase(sid, enableReporting, 0.0, false) {}
+};
+
+struct DiagnosticsHkUpdateParams : public ParamsBase {
+  DiagnosticsHkUpdateParams(sid_t sid, bool enableReporting)
+      : ParamsBase(sid, enableReporting, 0.0, true) {}
+};
+}  // namespace subdp
+
 class ProvidesDataPoolSubscriptionIF {
  public:
-  virtual ~ProvidesDataPoolSubscriptionIF(){};
-
+  virtual ~ProvidesDataPoolSubscriptionIF() = default;
   /**
-   * @brief   Subscribe for the generation of periodic packets.
+   * @brief   Subscribe for the generation of periodic packets. Used for regular HK packets
    * @details
    * This subscription mechanism will generally be used by the data creator
    * to generate housekeeping packets which are downlinked directly.
    * @return
    */
-  virtual ReturnValue_t subscribeForPeriodicPacket(sid_t sid, bool enableReporting,
-                                                   float collectionInterval, bool isDiagnostics,
-                                                   object_id_t packetDestination) = 0;
+  virtual ReturnValue_t subscribeForRegularPeriodicPacket(
+      subdp::RegularHkPeriodicParams params) = 0;
+  /**
+   * @brief   Subscribe for the generation of periodic packets. Used for diagnostic packets
+   * @details
+   * This subscription mechanism will generally be used by the data creator
+   * to generate housekeeping packets which are downlinked directly.
+   * @return
+   */
+  virtual ReturnValue_t subscribeForDiagPeriodicPacket(
+      subdp::DiagnosticsHkPeriodicParams params) = 0;
+
+  [[deprecated(
+      "Please use the new API which takes all arguments as one wrapper "
+      "struct")]] virtual ReturnValue_t
+  subscribeForPeriodicPacket(sid_t sid, bool enableReporting, float collectionInterval,
+                             bool isDiagnostics,
+                             object_id_t packetDestination = objects::NO_OBJECT) {
+    if (isDiagnostics) {
+      subdp::DiagnosticsHkPeriodicParams params(sid, enableReporting, collectionInterval);
+      return subscribeForDiagPeriodicPacket(params);
+    } else {
+      subdp::RegularHkPeriodicParams params(sid, enableReporting, collectionInterval);
+      return subscribeForRegularPeriodicPacket(params);
+    }
+  }
+
   /**
    * @brief   Subscribe for the  generation of packets if the dataset
    *          is marked as changed.
@@ -29,9 +95,28 @@ class ProvidesDataPoolSubscriptionIF {
    * @param packetDestination
    * @return
    */
-  virtual ReturnValue_t subscribeForUpdatePacket(sid_t sid, bool reportingEnabled,
-                                                 bool isDiagnostics,
-                                                 object_id_t packetDestination) = 0;
+  virtual ReturnValue_t subscribeForRegularUpdatePacket(subdp::RegularHkUpdateParams params) = 0;
+  virtual ReturnValue_t subscribeForDiagUpdatePacket(subdp::DiagnosticsHkUpdateParams params) = 0;
+
+  //  virtual ReturnValue_t
+  //  subscribeForUpdatePacket(sid_t sid, bool reportingEnabled, bool isDiagnostics) {
+  //    return subscribeForUpdatePacket(sid, reportingEnabled, isDiagnostics, objects::NO_OBJECT);
+  //  }
+
+  [[deprecated(
+      "Please use the new API which takes all arguments as one wrapper "
+      "struct")]] virtual ReturnValue_t
+  subscribeForUpdatePacket(sid_t sid, bool reportingEnabled, bool isDiagnostics,
+                           object_id_t packetDestination = objects::NO_OBJECT) {
+    if (isDiagnostics) {
+      subdp::DiagnosticsHkUpdateParams params(sid, reportingEnabled);
+      return subscribeForDiagUpdatePacket(params);
+    } else {
+      subdp::RegularHkUpdateParams params(sid, reportingEnabled);
+      return subscribeForRegularUpdatePacket(params);
+    }
+  }
+
   /**
    * @brief   Subscribe for a notification message which will be sent
    *          if a dataset has changed.
@@ -46,8 +131,7 @@ class ProvidesDataPoolSubscriptionIF {
    *                              Otherwise, only an notification message is sent.
    * @return
    */
-  virtual ReturnValue_t subscribeForSetUpdateMessage(const uint32_t setId,
-                                                     object_id_t destinationObject,
+  virtual ReturnValue_t subscribeForSetUpdateMessage(uint32_t setId, object_id_t destinationObject,
                                                      MessageQueueId_t targetQueueId,
                                                      bool generateSnapshot) = 0;
   /**
@@ -64,7 +148,7 @@ class ProvidesDataPoolSubscriptionIF {
    *                              only an notification message is sent.
    * @return
    */
-  virtual ReturnValue_t subscribeForVariableUpdateMessage(const lp_id_t localPoolId,
+  virtual ReturnValue_t subscribeForVariableUpdateMessage(lp_id_t localPoolId,
                                                           object_id_t destinationObject,
                                                           MessageQueueId_t targetQueueId,
                                                           bool generateSnapshot) = 0;

src/fsfw/devicehandlers/AssemblyBase.cpp

@@ -26,11 +26,7 @@ void AssemblyBase::performChildOperation() {
 
 void AssemblyBase::startTransition(Mode_t mode, Submode_t submode) {
   doStartTransition(mode, submode);
-  if (modeHelper.isForced()) {
-    triggerEvent(FORCING_MODE, mode, submode);
-  } else {
-    triggerEvent(CHANGING_MODE, mode, submode);
-  }
+  triggerModeHelperEvents(mode, submode);
 }
 
 void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) {
@@ -77,9 +73,10 @@ bool AssemblyBase::handleChildrenChangedHealth() {
   }
   HealthState healthState = healthHelper.healthTable->getHealth(iter->first);
   if (healthState == HasHealthIF::NEEDS_RECOVERY) {
-    triggerEvent(TRYING_RECOVERY);
+    triggerEvent(TRYING_RECOVERY, iter->first, 0);
     recoveryState = RECOVERY_STARTED;
     recoveringDevice = iter;
+    // The user needs to take care of commanding the children off in commandChildren
     doStartTransition(targetMode, targetSubmode);
   } else {
     triggerEvent(CHILD_CHANGED_HEALTH);
@@ -228,6 +225,9 @@ ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) {
 bool AssemblyBase::checkAndHandleRecovery() {
   switch (recoveryState) {
     case RECOVERY_STARTED:
+      // The recovery was already start in #handleChildrenChangedHealth and we just need
+      // to wait for an off time period.
+      // TODO: make time period configurable
       recoveryState = RECOVERY_WAIT;
       recoveryOffTimer.resetTimer();
       return true;
@@ -266,3 +266,11 @@ void AssemblyBase::overwriteDeviceHealth(object_id_t objectId, HasHealthIF::Heal
   modeHelper.setForced(true);
   sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL);
 }
+
+void AssemblyBase::triggerModeHelperEvents(Mode_t mode, Submode_t submode) {
+  if (modeHelper.isForced()) {
+    triggerEvent(FORCING_MODE, mode, submode);
+  } else {
+    triggerEvent(CHANGING_MODE, mode, submode);
+  }
+}

src/fsfw/devicehandlers/AssemblyBase.h

@@ -12,7 +12,8 @@
  * Documentation: Dissertation Baetz p.156, 157.
  *
  * This class reduces the complexity of controller components which would
- * otherwise be needed for the handling of redundant devices.
+ * otherwise be needed for the handling of redundant devices. However, it can also be used to
+ * manage the mode keeping and recovery of non-redundant devices
  *
  * The template class monitors mode and health state of its children
  * and checks availability of devices on every detected change.
@@ -26,11 +27,9 @@
  *
  * Important:
  *
- * The implementation must call registerChild(object_id_t child)
- * for all commanded children during initialization.
+ * The implementation must call #registerChild for all commanded children during initialization.
  * The implementation must call the initialization function of the base class.
  * (This will call the function in SubsystemBase)
- *
  */
 class AssemblyBase : public SubsystemBase {
  public:
@@ -47,9 +46,10 @@ class AssemblyBase : public SubsystemBase {
 
  protected:
   /**
-   * Command children to reach [mode,submode] combination
-   * Can be done by setting #commandsOutstanding correctly,
-   * or using executeTable()
+   * Command children to reach [mode,submode] combination. Can be done by setting
+   * #commandsOutstanding correctly, or using #executeTable. In case of an FDIR recovery,
+   * the user needs to ensure that the target devices are healthy. If a device is not healthy,
+   * a recovery might be on-going and the device needs to be commanded to off first.
    * @param mode
    * @param submode
    * @return
@@ -120,8 +120,19 @@ class AssemblyBase : public SubsystemBase {
 
   virtual ReturnValue_t handleHealthReply(CommandMessage *message);
 
-  virtual void performChildOperation();
+  /**
+   * @brief Default periodic handler
+   * @details
+   * This is the default periodic handler which will be called by the SubsystemBase
+   * performOperation. It performs the child transitions or reacts to changed health/mode states
+   * of children objects
+   */
+  virtual void performChildOperation() override;
 
+  /**
+   * This function handles changed mode or health states of children
+   * @return
+   */
   bool handleChildrenChanged();
 
   /**
@@ -134,12 +145,37 @@ class AssemblyBase : public SubsystemBase {
 
   bool handleChildrenChangedHealth();
 
+  /**
+   * Core transition handler. The default implementation will only do something if
+   * #commandsOutstanding is smaller or equal to zero, which means that all mode commands
+   * from the #doPerformTransition call were executed successfully.
+   *
+   * Unless a second step was requested, the function will then use #checkChildrenState to
+   * determine whether the target mode was reached.
+   *
+   * There is some special handling for certain (internal) modes:
+   *  - A second step is necessary. #commandChildren will be performed again
+   *  - The device health was overwritten. #commandChildren will be called
+   *  - A recovery is ongoing. #checkAndHandleRecovery will be called.
+   */
   virtual void handleChildrenTransition();
 
   ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode);
 
+  /**
+   * Calls #doStartTransition and triggers an informative event as well that the mode will
+   * change
+   * @param mode
+   * @param submode
+   */
   virtual void startTransition(Mode_t mode, Submode_t submode);
 
+  /**
+   * This function starts the transition by setting the internal #targetSubmode and #targetMode
+   * variables and then calling the #commandChildren function.
+   * @param mode
+   * @param submode
+   */
   virtual void doStartTransition(Mode_t mode, Submode_t submode);
 
   virtual bool isInTransition();
@@ -160,7 +196,7 @@ class AssemblyBase : public SubsystemBase {
    * Manages recovery of a device
    * @return true if recovery is still ongoing, false else.
    */
-  bool checkAndHandleRecovery();
+  virtual bool checkAndHandleRecovery();
 
   /**
    * Helper method to overwrite health state of one of the children.
@@ -168,6 +204,8 @@ class AssemblyBase : public SubsystemBase {
    * @param objectId Must be a registered child.
    */
   void overwriteDeviceHealth(object_id_t objectId, HasHealthIF::HealthState oldHealth);
+
+  void triggerModeHelperEvents(Mode_t mode, Submode_t submode);
 };
 
 #endif /* FSFW_DEVICEHANDLERS_ASSEMBLYBASE_H_ */

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -40,8 +40,9 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
       childTransitionDelay(5000),
       transitionSourceMode(_MODE_POWER_DOWN),
       transitionSourceSubMode(SUBMODE_NONE) {
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
   commandQueue = QueueFactory::instance()->createMessageQueue(
-      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE);
+      cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
   insertInCommandMap(RAW_COMMAND_ID);
   cookieInfo.state = COOKIE_UNUSED;
   cookieInfo.pendingCommand = deviceCommandMap.end();
@@ -49,9 +50,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
     printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase", returnvalue::FAILED,
                         "Invalid cookie");
   }
-  if (this->fdirInstance == nullptr) {
-    this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, defaultFdirParentId);
-  }
 }
 
 void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
@@ -129,6 +127,18 @@ ReturnValue_t DeviceHandlerBase::initialize() {
   if (result != returnvalue::OK) {
     return result;
   }
+  if (this->fdirInstance == nullptr) {
+    this->fdirInstance =
+        new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId);
+  }
+
+  if (this->parent != objects::NO_OBJECT) {
+    HasModesIF* modeIF = ObjectManager::instance()->get<HasModesIF>(this->parent);
+    HasHealthIF* healthIF = ObjectManager::instance()->get<HasHealthIF>(this->parent);
+    if (modeIF != nullptr and healthIF != nullptr) {
+      setParentQueue(modeIF->getCommandQueue());
+    }
+  }
 
   communicationInterface =
       ObjectManager::instance()->get<DeviceCommunicationIF>(deviceCommunicationId);
@@ -364,13 +374,12 @@ void DeviceHandlerBase::doStateMachine() {
       }
     } break;
     case _MODE_WAIT_OFF: {
-      uint32_t currentUptime;
-      Clock::getUptime(&currentUptime);
-
       if (powerSwitcher == nullptr) {
         setMode(MODE_OFF);
         break;
       }
+      uint32_t currentUptime;
+      Clock::getUptime(&currentUptime);
       if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) {
         triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0);
         setMode(MODE_ERROR_ON);
@@ -571,6 +580,9 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) {
   mode = newMode;
   modeChanged();
   setNormalDatapoolEntriesInvalid();
+  if (newMode == MODE_OFF) {
+    disableCommandsAndReplies();
+  }
   if (!isTransitionalMode()) {
     modeHelper.modeChanged(newMode, newSubmode);
     announceMode(false);
@@ -1276,6 +1288,7 @@ void DeviceHandlerBase::handleDeviceTm(const SerializeIF& dataSet, DeviceCommand
   if (iter->second.command != deviceCommandMap.end()) {
     MessageQueueId_t queueId = iter->second.command->second.sendReplyTo;
 
+    // This may fail, but we'll ignore the fault.
     if (queueId != NO_COMMANDER) {
       // This may fail, but we'll ignore the fault.
       actionHelper.reportData(queueId, replyId, const_cast<SerializeIF*>(&dataSet));
@@ -1318,18 +1331,22 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue
     return result;
   }
   DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId);
+  MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE;
   if (iter == deviceCommandMap.end()) {
     result = COMMAND_NOT_SUPPORTED;
   } else if (iter->second.isExecuting) {
     result = COMMAND_ALREADY_SENT;
   } else {
+    prevRecipient = iter->second.sendReplyTo;
+    iter->second.sendReplyTo = commandedBy;
     result = buildCommandFromCommand(actionId, data, size);
   }
   if (result == returnvalue::OK) {
-    iter->second.sendReplyTo = commandedBy;
     iter->second.isExecuting = true;
     cookieInfo.pendingCommand = iter;
     cookieInfo.state = COOKIE_WRITE_READY;
+  } else {
+    iter->second.sendReplyTo = prevRecipient;
   }
   return result;
 }
@@ -1454,6 +1471,8 @@ void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task) { executingTask = task;
 void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId,
                                        uint32_t parameter) {}
 
+Submode_t DeviceHandlerBase::getInitialSubmode() { return SUBMODE_NONE; }
+
 void DeviceHandlerBase::performOperationHook() {}
 
 ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
@@ -1476,7 +1495,7 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
   this->poolManager.initializeAfterTaskCreation();
 
   if (setStartupImmediately) {
-    startTransition(MODE_ON, SUBMODE_NONE);
+    startTransition(MODE_ON, getInitialSubmode());
   }
   return returnvalue::OK;
 }
@@ -1560,3 +1579,29 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI
   }
   return commandIter->second.sendReplyTo;
 }
+
+void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirInstance = fdir; }
+
+void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; }
+
+void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) {
+  this->powerSwitcher = switcher;
+}
+
+void DeviceHandlerBase::disableCommandsAndReplies() {
+  for (auto& command : deviceCommandMap) {
+    if (command.second.isExecuting) {
+      command.second.isExecuting = false;
+    }
+  }
+  for (auto& reply : deviceReplyMap) {
+    if (!reply.second.periodic) {
+      if (reply.second.countdown != nullptr) {
+        reply.second.countdown->timeOut();
+      } else {
+        reply.second.delayCycles = 0;
+      }
+      reply.second.active = false;
+    }
+  }
+}

src/fsfw/devicehandlers/DeviceHandlerBase.h

@@ -23,6 +23,7 @@
 #include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tasks/PeriodicTaskIF.h"
+#include "fsfw/util/dataWrapper.h"
 
 namespace Factory {
 void setStaticFrameworkObjectIds();
@@ -102,6 +103,9 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF *comCookie,
                     FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20);
 
+  void setCustomFdir(FailureIsolationBase *fdir);
+  void setParent(object_id_t parent);
+  void setPowerSwitcher(PowerSwitchIF *switcher);
   void setHkDestination(object_id_t hkDestination);
 
   /**
@@ -463,14 +467,14 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * @brief 	This is a helper method to insert replies in the reply map.
    * @param deviceCommand	Identifier of the reply to add.
    * @param maxDelayCycles The maximum number of delay cycles the reply waits
-   * until it times out.
+   *    until it times out.
    * @param periodic	Indicates if the command is periodic (i.e. it is sent
-   * by the device repeatedly without request) or not. Default is aperiodic (0).
-   * Please note that periodic replies are disabled by default. You can enable them with
-   * #updatePeriodicReply
+   *    by the device repeatedly without request) or not. Default is aperiodic (0).
+   *    Please note that periodic replies are disabled by default. You can enable them with
+   *    #updatePeriodicReply
    * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible
-   *                  to provide a pointer to a Countdown object which will signal the timeout
-   *                  when expired
+   *    to provide a pointer to a Countdown object which will signal the timeout
+   *    when expired
    * @return	- @c returnvalue::OK when the command was successfully inserted,
    *          - @c returnvalue::FAILED else.
    */
@@ -655,6 +659,12 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   virtual void debugInterface(uint8_t positionTracker = 0, object_id_t objectId = 0,
                               uint32_t parameter = 0);
 
+  /**
+   * @brief Can be overwritten by a child to specify the initial submode when device has been set
+   *        to startup immediately.
+   */
+  virtual Submode_t getInitialSubmode();
+
  protected:
   static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE;
 
@@ -773,11 +783,18 @@ class DeviceHandlerBase : public DeviceHandlerIF,
    * This is used to keep track of pending replies.
    */
   struct DeviceReplyInfo {
+    //! For Command-Reply combinations:
     //! The maximum number of cycles the handler should wait for a reply
     //! to this command.
+    //!
+    //! Reply Only:
+    //! For periodic replies, this variable will be the number of delay cycles between the replies.
+    //! For the non-periodic variant, this variable is not used as there is no meaningful
+    //! definition for delay
     uint16_t maxDelayCycles;
-    //! The currently remaining cycles the handler should wait for a reply,
-    //! 0 means there is no reply expected
+    //! This variable will be set to #maxDelayCycles if a reply is expected.
+    //! For non-periodic replies without a command, this variable is unused.
+    //! A runtime value of 0 means there is no reply is currently expected.
     uint16_t delayCycles;
     size_t replyLen = 0;  //!< Expected size of the reply.
     //! if this is !=0, the delayCycles will not be reset to 0 but to
@@ -833,6 +850,7 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   /** Pointer to the used FDIR instance. If not provided by child,
    * default class is instantiated. */
   FailureIsolationBase *fdirInstance;
+  object_id_t parent = objects::NO_OBJECT;
 
   //! To correctly delete the default instance.
   bool defaultFDIRUsed;
@@ -1308,6 +1326,11 @@ class DeviceHandlerBase : public DeviceHandlerIF,
   void printWarningOrError(sif::OutputTypes errorType, const char *functionName,
                            ReturnValue_t errorCode = returnvalue::FAILED,
                            const char *errorPrint = nullptr);
+
+  /**
+   * @brief Disables all commands and replies when device is set to MODE_OFF
+   */
+  void disableCommandsAndReplies();
 };
 
 #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */

src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp

@@ -29,6 +29,7 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
   switch (event->getEvent()) {
     case HasModesIF::MODE_TRANSITION_FAILED:
     case HasModesIF::OBJECT_IN_INVALID_MODE:
+    case DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT:
       // We'll try a recovery as long as defined in MAX_REBOOT.
       // Might cause some AssemblyBase cycles, so keep number low.
       handleRecovery(event->getEvent());

src/fsfw/devicehandlers/DeviceHandlerIF.h

@@ -109,6 +109,7 @@ class DeviceHandlerIF {
   static const Event INVALID_DEVICE_COMMAND = MAKE_EVENT(8, severity::LOW);
   static const Event MONITORING_LIMIT_EXCEEDED = MAKE_EVENT(9, severity::LOW);
   static const Event MONITORING_AMBIGUOUS = MAKE_EVENT(10, severity::HIGH);
+  static const Event DEVICE_WANTS_HARD_REBOOT = MAKE_EVENT(11, severity::HIGH);
 
   static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_IF;
 

src/fsfw/devicehandlers/DeviceTmReportingWrapper.h

@@ -1,9 +1,10 @@
 #ifndef FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_
 #define FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_
 
-#include "../action/HasActionsIF.h"
-#include "../objectmanager/SystemObjectIF.h"
-#include "../serialize/SerializeIF.h"
+#include "fsfw/action/HasActionsIF.h"
+#include "fsfw/objectmanager/SystemObjectIF.h"
+#include "fsfw/serialize/SerializeIF.h"
+#include "fsfw/util/dataWrapper.h"
 
 class DeviceTmReportingWrapper : public SerializeIF {
  public:

src/fsfw/devicehandlers/HealthDevice.cpp

@@ -8,7 +8,9 @@ HealthDevice::HealthDevice(object_id_t setObjectId, MessageQueueId_t parentQueue
       parentQueue(parentQueue),
       commandQueue(),
       healthHelper(this, setObjectId) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(3);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 HealthDevice::~HealthDevice() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }

src/fsfw/events/EventManager.cpp

@@ -18,8 +18,9 @@ const LocalPool::LocalPoolConfig EventManager::poolConfig = {
 EventManager::EventManager(object_id_t setObjectId)
     : SystemObject(setObjectId), factoryBackend(0, poolConfig, false, true) {
   mutex = MutexFactory::instance()->createMutex();
-  eventReportQueue = QueueFactory::instance()->createMessageQueue(MAX_EVENTS_PER_CYCLE,
-                                                                  EventMessage::EVENT_MESSAGE_SIZE);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  eventReportQueue = QueueFactory::instance()->createMessageQueue(
+      MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
 }
 
 EventManager::~EventManager() {
@@ -46,9 +47,20 @@ ReturnValue_t EventManager::performOperation(uint8_t opCode) {
 
 void EventManager::notifyListeners(EventMessage* message) {
   lockMutex();
-  for (auto iter = listenerList.begin(); iter != listenerList.end(); ++iter) {
-    if (iter->second.match(message)) {
-      MessageQueueSenderIF::sendMessage(iter->first, message, message->getSender());
+  for (auto& listener : listenerList) {
+    if (listener.second.match(message)) {
+      ReturnValue_t result =
+          MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender());
+      if (result != returnvalue::OK) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << std::hex << "EventManager::notifyListeners: MSG to 0x" << std::setfill('0')
+                   << std::setw(8) << listener.first << " failed with result 0x" << std::setw(4)
+                   << result << std::setfill(' ') << std::endl;
+#else
+        sif::printError("Sending message to listener 0x%08x failed with result %04x\n",
+                        listener.first, result);
+#endif
+      }
     }
   }
   unlockMutex();
@@ -194,4 +206,19 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage* messag
   }
 }
 
+void EventManager::printListeners() {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "Event manager listener MQ IDs:" << std::setfill('0') << std::hex << std::endl;
+  for (auto& listener : listenerList) {
+    sif::info << "0x" << std::setw(8) << listener.first << std::endl;
+  }
+  sif::info << std::dec << std::setfill(' ');
+#else
+  sif::printInfo("Event manager listener MQ IDs:\n");
+  for (auto& listener : listenerList) {
+    sif::printInfo("0x%08x\n", listener.first);
+  }
+#endif
+}
+
 #endif /* FSFW_OBJ_EVENT_TRANSLATION == 1 */

src/fsfw/events/EventManager.h

@@ -43,6 +43,7 @@ class EventManager : public EventManagerIF, public ExecutableObjectIF, public Sy
                                           object_id_t reporterFrom = 0, object_id_t reporterTo = 0,
                                           bool reporterInverted = false);
   ReturnValue_t performOperation(uint8_t opCode);
+  void printListeners();
 
  protected:
   MessageQueueIF* eventReportQueue = nullptr;

src/fsfw/fdir/FailureIsolationBase.cpp

@@ -9,8 +9,9 @@
 FailureIsolationBase::FailureIsolationBase(object_id_t owner, object_id_t parent,
                                            uint8_t messageDepth, uint8_t parameterDomainBase)
     : ownerId(owner), faultTreeParent(parent), parameterDomainBase(parameterDomainBase) {
-  eventQueue =
-      QueueFactory::instance()->createMessageQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE);
+  auto mqArgs = MqArgs(owner, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
 }
 
 FailureIsolationBase::~FailureIsolationBase() {
@@ -61,11 +62,12 @@ ReturnValue_t FailureIsolationBase::initialize() {
         ObjectManager::instance()->get<ConfirmsFailuresIF>(faultTreeParent);
     if (parentIF == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "FailureIsolationBase::intialize: Parent object"
-                 << "invalid." << std::endl;
-#endif
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-      sif::error << "Make sure it implements ConfirmsFailuresIF." << std::endl;
+      sif::error << "FailureIsolationBase::intialize: Parent object "
+                 << "invalid" << std::endl;
+      sif::error << "Make sure it implements ConfirmsFailuresIF" << std::endl;
+#else
+      sif::printError("FailureIsolationBase::intialize: Parent object invalid\n");
+      sif::printError("Make sure it implements ConfirmsFailuresIF\n");
 #endif
       return ObjectManagerIF::CHILD_INIT_FAILED;
       return returnvalue::FAILED;

src/fsfw/fdir/FailureIsolationBase.h

@@ -12,13 +12,12 @@
 class FailureIsolationBase : public ConfirmsFailuresIF, public HasParametersIF {
  public:
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_1;
-  static const Event FDIR_CHANGED_STATE =
-      MAKE_EVENT(1, severity::INFO);  //!< FDIR has an internal state, which changed from par2
-                                      //!< (oldState) to par1 (newState).
-  static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(
-      2, severity::MEDIUM);  //!< FDIR tries to restart device. Par1: event that caused recovery.
-  static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(
-      3, severity::MEDIUM);  //!< FDIR turns off device. Par1: event that caused recovery.
+  //! FDIR has an internal state, which changed from par2 (oldState) to par1 (newState).
+  static const Event FDIR_CHANGED_STATE = MAKE_EVENT(1, severity::INFO);
+  //! FDIR tries to restart device. Par1: event that caused recovery.
+  static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(2, severity::MEDIUM);
+  //! FDIR turns off device. Par1: event that caused recovery.
+  static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(3, severity::MEDIUM);
 
   FailureIsolationBase(object_id_t owner, object_id_t parent = objects::NO_OBJECT,
                        uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0);

src/fsfw/globalfunctions/CMakeLists.txt

@@ -4,6 +4,7 @@ target_sources(
           AsciiConverter.cpp
           CRC.cpp
           DleEncoder.cpp
+          DleParser.cpp
           PeriodicOperationDivider.cpp
           timevalOperations.cpp
           Type.cpp

src/fsfw/globalfunctions/DleParser.cpp

@@ -0,0 +1,230 @@
+#include "DleParser.h"
+
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#include <cstdio>
+
+DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
+                     BufPair decodedBuf, UserHandler handler, void* args)
+    : decodeRingBuf(decodeRingBuf),
+      decoder(decoder),
+      encodedBuf(encodedBuf),
+      decodedBuf(decodedBuf),
+      handler(handler),
+      ctx(args) {
+  if (handler == nullptr) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "DleParser::DleParser: Invalid user handler" << std::endl;
+#else
+    sif::printError("DleParser::DleParser: Invalid user handler\n");
+#endif
+  }
+}
+
+ReturnValue_t DleParser::passData(uint8_t* data, size_t len) {
+  if (data == nullptr or len == 0 or handler == nullptr) {
+    return returnvalue::FAILED;
+  }
+  size_t copyIntoRingBufFromHere = 0;
+  size_t copyAmount = len;
+  size_t startIdx = 0;
+  ReturnValue_t result = returnvalue::OK;
+  bool startFoundInThisPacket = false;
+  for (size_t idx = 0; idx < len; idx++) {
+    if (data[idx] == DleEncoder::STX_CHAR) {
+      if (not startFound and not startFoundInThisPacket) {
+        startIdx = idx;
+        copyIntoRingBufFromHere = idx;
+        copyAmount = len - idx;
+      } else {
+        // Maybe print warning, should not happen
+        decodeRingBuf.clear();
+        ErrorInfo info;
+        info.len = idx;
+        prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info);
+        handler(ctx);
+        copyIntoRingBufFromHere = idx;
+        copyAmount = len - idx;
+      }
+      startFound = true;
+      startFoundInThisPacket = true;
+    } else if (data[idx] == DleEncoder::ETX_CHAR) {
+      if (startFoundInThisPacket) {
+        size_t readLen = 0;
+        size_t decodedLen = 0;
+        result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first,
+                                decodedBuf.second, &decodedLen);
+        if (result == returnvalue::OK) {
+          ctx.setType(ContextType::PACKET_FOUND);
+          ctx.decodedPacket.first = decodedBuf.first;
+          ctx.decodedPacket.second = decodedLen;
+          this->handler(ctx);
+        } else if (result == DleEncoder::STREAM_TOO_SHORT) {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+          handler(ctx);
+        } else {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+          handler(ctx);
+        }
+        decodeRingBuf.clear();
+        if ((idx + 1) < len) {
+          copyIntoRingBufFromHere = idx + 1;
+          copyAmount = len - idx - 1;
+        } else {
+          copyAmount = 0;
+        }
+      } else if (startFound) {
+        // ETX found but STX was found in another mini packet. Reconstruct the full packet
+        // to decode it
+        result = decodeRingBuf.writeData(data, idx + 1);
+        if (result != returnvalue::OK) {
+          ErrorInfo info;
+          info.res = result;
+          prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
+          handler(ctx);
+        }
+        size_t fullEncodedLen = decodeRingBuf.getAvailableReadData();
+        if (fullEncodedLen > encodedBuf.second) {
+          ErrorInfo info;
+          info.len = fullEncodedLen;
+          prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info);
+          handler(ctx);
+          decodeRingBuf.clear();
+        } else {
+          size_t decodedLen = 0;
+          size_t readLen = 0;
+          decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true);
+          result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first,
+                                  decodedBuf.second, &decodedLen);
+          if (result == returnvalue::OK) {
+            if (this->handler != nullptr) {
+              ctx.setType(ContextType::PACKET_FOUND);
+              ctx.decodedPacket.first = decodedBuf.first;
+              ctx.decodedPacket.second = decodedLen;
+              this->handler(ctx);
+            }
+          } else if (result == DleEncoder::STREAM_TOO_SHORT) {
+            ErrorInfo info;
+            info.res = result;
+            prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info);
+            handler(ctx);
+          } else {
+            ErrorInfo info;
+            info.res = result;
+            prepareErrorContext(ErrorTypes::DECODE_ERROR, info);
+            handler(ctx);
+          }
+          decodeRingBuf.clear();
+          startFound = false;
+          startFoundInThisPacket = false;
+          if ((idx + 1) < len) {
+            copyIntoRingBufFromHere = idx + 1;
+            copyAmount = len - idx - 1;
+          } else {
+            copyAmount = 0;
+          }
+        }
+      } else {
+        // End data without preceeding STX
+        ErrorInfo info;
+        info.len = idx + 1;
+        prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info);
+        handler(ctx);
+        decodeRingBuf.clear();
+        if ((idx + 1) < len) {
+          copyIntoRingBufFromHere = idx + 1;
+          copyAmount = len - idx - 1;
+        } else {
+          copyAmount = 0;
+        }
+      }
+      startFoundInThisPacket = false;
+      startFound = false;
+    }
+  }
+  if (copyAmount > 0) {
+    result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount);
+    if (result != returnvalue::OK) {
+      ErrorInfo info;
+      info.res = result;
+      prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info);
+      handler(ctx);
+    }
+  }
+  return returnvalue::OK;
+}
+
+void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "DleParserBase::handleFoundPacket: Detected DLE packet with " << len << " bytes"
+            << std::endl;
+#else
+  sif::printInfo("DleParserBase::handleFoundPacket: Detected DLE packet with %d bytes\n", len);
+#endif
+#endif
+}
+
+void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) {
+  switch (err) {
+    case (ErrorTypes::NONE): {
+      errorPrinter("No error");
+      break;
+    }
+    case (ErrorTypes::DECODE_ERROR): {
+      errorPrinter("Decode Error");
+      break;
+    }
+    case (ErrorTypes::RING_BUF_ERROR): {
+      errorPrinter("Ring Buffer Error");
+      break;
+    }
+    case (ErrorTypes::ENCODED_BUF_TOO_SMALL):
+    case (ErrorTypes::DECODING_BUF_TOO_SMALL): {
+      char opt[64];
+      snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.len);
+      if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) {
+        errorPrinter("Encoded buf too small", opt);
+      } else {
+        errorPrinter("Decoding buf too small", opt);
+      }
+      break;
+    }
+    case (ErrorTypes::CONSECUTIVE_STX_CHARS): {
+      errorPrinter("Consecutive STX chars detected");
+      break;
+    }
+    case (ErrorTypes::CONSECUTIVE_ETX_CHARS): {
+      errorPrinter("Consecutive ETX chars detected");
+      break;
+    }
+  }
+}
+
+void DleParser::errorPrinter(const char* str, const char* opt) {
+  if (opt == nullptr) {
+    opt = "";
+  }
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+  sif::info << "DleParserBase::handleParseError: " << str << opt << std::endl;
+#else
+  sif::printInfo("DleParserBase::handleParseError: %s%s\n", str, opt);
+#endif
+#endif
+}
+
+void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) {
+  ctx.setType(ContextType::ERROR);
+  ctx.error.first = err;
+  ctx.error.second = info;
+}
+
+void DleParser::reset() {
+  startFound = false;
+  decodeRingBuf.clear();
+}

src/fsfw/globalfunctions/DleParser.h

@@ -0,0 +1,125 @@
+#pragma once
+
+#include <fsfw/container/SimpleRingBuffer.h>
+#include <fsfw/globalfunctions/DleEncoder.h>
+#include <fsfw/returnvalues/returnvalue.h>
+
+#include <cstddef>
+#include <utility>
+
+/**
+ * @brief This base helper class can be used to extract DLE encoded packets from a data stream
+ * @details
+ * The core API of the parser takes received packets which can contains DLE packets. The parser
+ * can deal with DLE packets split across multiple packets. It does so by using a dedicated
+ * decoding ring buffer. The user can process received packets and detect errors by
+ * overriding two provided virtual methods. This also allows detecting multiple DLE packets
+ * inside one passed packet.
+ */
+class DleParser {
+ public:
+  using BufPair = std::pair<uint8_t*, size_t>;
+
+  enum class ContextType { PACKET_FOUND, ERROR };
+
+  enum class ErrorTypes {
+    NONE,
+    ENCODED_BUF_TOO_SMALL,
+    DECODING_BUF_TOO_SMALL,
+    DECODE_ERROR,
+    RING_BUF_ERROR,
+    CONSECUTIVE_STX_CHARS,
+    CONSECUTIVE_ETX_CHARS
+  };
+
+  union ErrorInfo {
+    size_t len;
+    ReturnValue_t res;
+  };
+
+  using ErrorPair = std::pair<ErrorTypes, ErrorInfo>;
+
+  struct Context {
+   public:
+    Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); }
+
+    void setType(ContextType type) {
+      this->type = type;
+      if (type == ContextType::PACKET_FOUND) {
+        error.first = ErrorTypes::NONE;
+        error.second.len = 0;
+      } else {
+        decodedPacket.first = nullptr;
+        decodedPacket.second = 0;
+      }
+    }
+
+    ContextType getType() const { return type; }
+
+    BufPair decodedPacket = {};
+    ErrorPair error;
+    void* userArgs;
+
+   private:
+    ContextType type;
+  };
+
+  using UserHandler = void (*)(const Context& ctx);
+
+  /**
+   * Base class constructor
+   * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets
+   * split across multiple packets
+   * @param decoder Decoder instance
+   * @param encodedBuf Buffer used to store encoded packets. It has to be large enough to hold
+   * the largest expected encoded DLE packet size
+   * @param decodedBuf Buffer used to store decoded packets. It has to be large enough to hold the
+   * largest expected decoded DLE packet size
+   * @param handler Function which will be called on a found packet
+   * @param args Arbitrary user argument
+   */
+  DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf,
+            BufPair decodedBuf, UserHandler handler, void* args);
+
+  /**
+   * This function allows to pass new data into the parser. It then scans for DLE packets
+   * automatically and inserts (part of) the packet into a ring buffer if necessary.
+   * @param data
+   * @param len
+   * @return
+   */
+  ReturnValue_t passData(uint8_t* data, size_t len);
+
+  /**
+   * Example found packet handler
+   * function call
+   * @param packet Decoded packet
+   * @param len Length of detected packet
+   */
+  void defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args);
+  /**
+   * Will be called if an error occured in the #passData call
+   * @param err
+   * @param ctx Context information depending on the error type
+   *  - For buffer length errors, will be set to the detected packet length which is too large
+   *  - For decode or ring buffer errors, will be set to the result returned from the failed call
+   */
+  static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx);
+
+  static void errorPrinter(const char* str, const char* opt = nullptr);
+
+  void prepareErrorContext(ErrorTypes err, ErrorInfo ctx);
+  /**
+   * Resets the parser by resetting the internal states and clearing the decoding ring buffer
+   */
+  void reset();
+
+ private:
+  SimpleRingBuffer& decodeRingBuf;
+  DleEncoder& decoder;
+  BufPair encodedBuf;
+  BufPair decodedBuf;
+  UserHandler handler = nullptr;
+  Context ctx;
+  bool startFound = false;
+};

src/fsfw/health/HasHealthIF.h

@@ -16,26 +16,24 @@ class HasHealthIF {
   };
 
   static const uint8_t INTERFACE_ID = CLASS_ID::HAS_HEALTH_IF;
-  static const ReturnValue_t OBJECT_NOT_HEALTHY = MAKE_RETURN_CODE(1);
-  static const ReturnValue_t INVALID_HEALTH_STATE = MAKE_RETURN_CODE(2);
+  static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = returnvalue::makeCode(INTERFACE_ID, 1);
+  static constexpr ReturnValue_t INVALID_HEALTH_STATE = returnvalue::makeCode(INTERFACE_ID, 2);
+  static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = returnvalue::makeCode(INTERFACE_ID, 3);
 
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1;
+  //! P1: New Health, P2: Old Health
   static const Event HEALTH_INFO = MAKE_EVENT(6, severity::INFO);
   static const Event CHILD_CHANGED_HEALTH = MAKE_EVENT(7, severity::INFO);
   static const Event CHILD_PROBLEMS = MAKE_EVENT(8, severity::LOW);
-  static const Event OVERWRITING_HEALTH =
-      MAKE_EVENT(9, severity::LOW);  //!< Assembly overwrites health information of children to keep
-                                     //!< satellite alive.
-  static const Event TRYING_RECOVERY =
-      MAKE_EVENT(10, severity::MEDIUM);  //!< Someone starts a recovery of a component (typically
-                                         //!< power-cycle). No parameters.
-  static const Event RECOVERY_STEP =
-      MAKE_EVENT(11, severity::MEDIUM);  //!< Recovery is ongoing. Comes twice during recovery. P1:
-                                         //!< 0 for the first, 1 for the second event. P2: 0
-  static const Event RECOVERY_DONE = MAKE_EVENT(
-      12,
-      severity::MEDIUM);  //!< Recovery was completed. Not necessarily successful. No parameters.
-
+  //! Assembly overwrites health information of children to keep satellite alive.
+  static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, severity::LOW);
+  //! Someone starts a recovery of a component (typically power-cycle). No parameters.
+  static const Event TRYING_RECOVERY = MAKE_EVENT(10, severity::MEDIUM);
+  //! Recovery is ongoing. Comes twice during recovery.
+  //! P1: 0 for the first, 1 for the second event. P2: 0
+  static const Event RECOVERY_STEP = MAKE_EVENT(11, severity::MEDIUM);
+  //! Recovery was completed. Not necessarily successful. No parameters.
+  static const Event RECOVERY_DONE = MAKE_EVENT(12, severity::MEDIUM);
   virtual ~HasHealthIF() {}
 
   virtual MessageQueueId_t getCommandQueue() const = 0;

src/fsfw/health/HealthTable.h

@@ -9,8 +9,8 @@
 
 class HealthTable : public HealthTableIF, public SystemObject {
  public:
-  HealthTable(object_id_t objectid);
-  virtual ~HealthTable();
+  explicit HealthTable(object_id_t objectid);
+  ~HealthTable() override;
 
   void setMutexTimeout(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs);
 

src/fsfw/housekeeping/AcceptsHkPacketsIF.h

@@ -1,12 +1,12 @@
 #ifndef FRAMEWORK_HOUSEKEEPING_ACCEPTSHKPACKETSIF_H_
 #define FRAMEWORK_HOUSEKEEPING_ACCEPTSHKPACKETSIF_H_
 
-#include "../ipc/MessageQueueMessageIF.h"
+#include "fsfw/ipc/MessageQueueMessageIF.h"
 
 class AcceptsHkPacketsIF {
  public:
-  virtual ~AcceptsHkPacketsIF(){};
-  virtual MessageQueueId_t getHkQueue() const = 0;
+  virtual ~AcceptsHkPacketsIF() = default;
+  [[nodiscard]] virtual MessageQueueId_t getHkQueue() const = 0;
 };
 
 #endif /* FRAMEWORK_HOUSEKEEPING_ACCEPTSHKPACKETSIF_H_ */

src/fsfw/internalerror/InternalErrorReporter.cpp

@@ -7,11 +7,13 @@
 
 InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth)
     : SystemObject(setObjectId),
-      commandQueue(QueueFactory::instance()->createMessageQueue(messageQueueDepth)),
       poolManager(this, commandQueue),
       internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID),
       internalErrorDataset(this) {
   mutex = MutexFactory::instance()->createMutex();
+  auto mqArgs = MqArgs(setObjectId, static_cast<void *>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 InternalErrorReporter::~InternalErrorReporter() { MutexFactory::instance()->deleteMutex(mutex); }
@@ -36,15 +38,14 @@ ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) {
     if ((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::debug << "InternalErrorReporter::performOperation: Errors "
-                 << "occured!" << std::endl;
-      sif::debug << "Queue errors: " << newQueueHits << std::endl;
-      sif::debug << "TM errors: " << newTmHits << std::endl;
-      sif::debug << "Store errors: " << newStoreHits << std::endl;
+                 << "occured: Queue | TM | Store : " << newQueueHits << " | " << newTmHits << " | "
+                 << newStoreHits << std::endl;
 #else
-      sif::printDebug("InternalErrorReporter::performOperation: Errors occured!\n");
-      sif::printDebug("Queue errors: %lu\n", static_cast<unsigned int>(newQueueHits));
-      sif::printDebug("TM errors: %lu\n", static_cast<unsigned int>(newTmHits));
-      sif::printDebug("Store errors: %lu\n", static_cast<unsigned int>(newStoreHits));
+      sif::printDebug(
+          "InternalErrorReporter::performOperation: Errors occured: Queue | TM | Store: %lu | %lu "
+          "| %lu\n",
+          static_cast<unsigned int>(newQueueHits), static_cast<unsigned int>(newTmHits),
+          static_cast<unsigned int>(newStoreHits));
 #endif
     }
   }
@@ -126,11 +127,12 @@ MessageQueueId_t InternalErrorReporter::getCommandQueue() const {
 
 ReturnValue_t InternalErrorReporter::initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
                                                              LocalDataPoolManager &poolManager) {
-  localDataPoolMap.emplace(errorPoolIds::TM_HITS, new PoolEntry<uint32_t>());
-  localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS, new PoolEntry<uint32_t>());
-  localDataPoolMap.emplace(errorPoolIds::STORE_HITS, new PoolEntry<uint32_t>());
-  poolManager.subscribeForPeriodicPacket(internalErrorSid, false, getPeriodicOperationFrequency(),
-                                         true);
+  localDataPoolMap.emplace(errorPoolIds::TM_HITS, &tmHitsEntry);
+  localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS, &queueHitsEntry);
+  localDataPoolMap.emplace(errorPoolIds::STORE_HITS, &storeHitsEntry);
+  poolManager.subscribeForDiagPeriodicPacket(subdp::DiagnosticsHkPeriodicParams(
+      internalErrorSid, false,
+      static_cast<float>(getPeriodicOperationFrequency()) / static_cast<float>(1000.0)));
   internalErrorDataset.setValidity(true, true);
   return returnvalue::OK;
 }

src/fsfw/internalerror/InternalErrorReporter.h

@@ -72,6 +72,9 @@ class InternalErrorReporter : public SystemObject,
   uint32_t queueHits = 0;
   uint32_t tmHits = 0;
   uint32_t storeHits = 0;
+  PoolEntry<uint32_t> tmHitsEntry = PoolEntry<uint32_t>();
+  PoolEntry<uint32_t> storeHitsEntry = PoolEntry<uint32_t>();
+  PoolEntry<uint32_t> queueHitsEntry = PoolEntry<uint32_t>();
 
   uint32_t getAndResetQueueHits();
   void incrementQueueHits();

src/fsfw/internalerror/InternalErrorReporterIF.h

@@ -12,7 +12,7 @@
  */
 class InternalErrorReporterIF {
  public:
-  virtual ~InternalErrorReporterIF() {}
+  virtual ~InternalErrorReporterIF() = default;
   /**
    * @brief Function to be called if a message queue could not be sent.
    * @details OSAL Implementations should call this function to indicate that

src/fsfw/ipc/CommandMessageIF.h

@@ -34,7 +34,7 @@ class CommandMessageIF {
   static const Command_t CMD_NONE = MAKE_COMMAND_ID(0);
   static const Command_t REPLY_COMMAND_OK = MAKE_COMMAND_ID(1);
   //! Reply indicating that the current command was rejected,
-  //! par1 should contain the error code
+  //! Parameter 1 should contain the error code
   static const Command_t REPLY_REJECTED = MAKE_COMMAND_ID(2);
 
   virtual ~CommandMessageIF(){};

src/fsfw/ipc/MessageQueueBase.cpp

@@ -8,7 +8,7 @@ MessageQueueBase::MessageQueueBase(MessageQueueId_t id, MessageQueueId_t default
   }
 }
 
-MessageQueueBase::~MessageQueueBase() {}
+MessageQueueBase::~MessageQueueBase() = default;
 
 ReturnValue_t MessageQueueBase::sendToDefault(MessageQueueMessageIF* message) {
   return sendToDefaultFrom(message, this->getId(), false);

src/fsfw/ipc/MessageQueueBase.h

@@ -7,28 +7,28 @@
 class MessageQueueBase : public MessageQueueIF {
  public:
   MessageQueueBase(MessageQueueId_t id, MessageQueueId_t defaultDest, MqArgs* mqArgs);
-  virtual ~MessageQueueBase();
+  ~MessageQueueBase() override;
 
   // Default implementations for MessageQueueIF where possible
-  virtual MessageQueueId_t getLastPartner() const override;
-  virtual MessageQueueId_t getId() const override;
-  virtual MqArgs& getMqArgs() override;
-  virtual void setDefaultDestination(MessageQueueId_t defaultDestination) override;
-  virtual MessageQueueId_t getDefaultDestination() const override;
-  virtual bool isDefaultDestinationSet() const override;
-  virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                    bool ignoreFault) override;
-  virtual ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
-  virtual ReturnValue_t reply(MessageQueueMessageIF* message) override;
-  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
-                                       MessageQueueId_t* receivedFrom) override;
-  virtual ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
-                                          bool ignoreFault = false) override;
+  [[nodiscard]] MessageQueueId_t getLastPartner() const override;
+  [[nodiscard]] MessageQueueId_t getId() const override;
+  MqArgs& getMqArgs() override;
+  void setDefaultDestination(MessageQueueId_t defaultDestination) override;
+  [[nodiscard]] MessageQueueId_t getDefaultDestination() const override;
+  [[nodiscard]] bool isDefaultDestinationSet() const override;
+  ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                            bool ignoreFault) override;
+  ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override;
+  ReturnValue_t reply(MessageQueueMessageIF* message) override;
+  ReturnValue_t receiveMessage(MessageQueueMessageIF* message,
+                               MessageQueueId_t* receivedFrom) override;
+  ReturnValue_t sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
+                                  bool ignoreFault = false) override;
 
   // OSAL specific, forward the abstract function
-  virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) = 0;
-  virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
-                                        MessageQueueId_t sentFrom, bool ignoreFault = false) = 0;
+  ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override = 0;
+  ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
+                                MessageQueueId_t sentFrom, bool ignoreFault = false) override = 0;
 
  protected:
   MessageQueueId_t id = MessageQueueIF::NO_QUEUE;

src/fsfw/ipc/MessageQueueIF.h

@@ -30,7 +30,7 @@ class MessageQueueIF {
   //! [EXPORT] : [COMMENT] Returned if the target destination is invalid.
   static constexpr ReturnValue_t DESTINATION_INVALID = MAKE_RETURN_CODE(4);
 
-  virtual ~MessageQueueIF() {}
+  virtual ~MessageQueueIF() = default;
   /**
    * @brief	This operation sends a message to the last communication partner.
    * @details
@@ -82,11 +82,11 @@ class MessageQueueIF {
   /**
    * @brief	This method returns the message queue ID of the last communication partner.
    */
-  virtual MessageQueueId_t getLastPartner() const = 0;
+  [[nodiscard]] virtual MessageQueueId_t getLastPartner() const = 0;
   /**
    * @brief	This method returns the message queue ID  of this class's message queue.
    */
-  virtual MessageQueueId_t getId() const = 0;
+  [[nodiscard]] virtual MessageQueueId_t getId() const = 0;
 
   /**
    * @brief	With the sendMessage call, a queue message is sent to a receiving queue.
@@ -159,9 +159,9 @@ class MessageQueueIF {
   /**
    * @brief	This method is a simple getter for the default destination.
    */
-  virtual MessageQueueId_t getDefaultDestination() const = 0;
+  [[nodiscard]] virtual MessageQueueId_t getDefaultDestination() const = 0;
 
-  virtual bool isDefaultDestinationSet() const = 0;
+  [[nodiscard]] virtual bool isDefaultDestinationSet() const = 0;
 
   virtual MqArgs& getMqArgs() = 0;
 };

src/fsfw/ipc/MessageQueueMessage.cpp

@@ -10,10 +10,10 @@ MessageQueueMessage::MessageQueueMessage() : messageSize(getMinimumMessageSize()
 }
 
 MessageQueueMessage::MessageQueueMessage(uint8_t* data, size_t size)
-    : messageSize(this->HEADER_SIZE + size) {
-  if (size <= this->MAX_DATA_SIZE) {
-    memcpy(this->getData(), data, size);
-    this->messageSize = this->HEADER_SIZE + size;
+    : messageSize(MessageQueueMessage::HEADER_SIZE + size) {
+  if (size <= MessageQueueMessage::MAX_DATA_SIZE) {
+    std::memcpy(MessageQueueMessage::getData(), data, size);
+    this->messageSize = MessageQueueMessage::HEADER_SIZE + size;
   } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "MessageQueueMessage: Passed size larger than maximum"
@@ -21,21 +21,23 @@ MessageQueueMessage::MessageQueueMessage(uint8_t* data, size_t size)
                  << std::endl;
 #endif
     memset(this->internalBuffer, 0, sizeof(this->internalBuffer));
-    this->messageSize = this->HEADER_SIZE;
+    this->messageSize = MessageQueueMessage::HEADER_SIZE;
   }
 }
 
-MessageQueueMessage::~MessageQueueMessage() {}
+MessageQueueMessage::~MessageQueueMessage() = default;
 
 const uint8_t* MessageQueueMessage::getBuffer() const { return this->internalBuffer; }
 
 uint8_t* MessageQueueMessage::getBuffer() { return this->internalBuffer; }
 
 const uint8_t* MessageQueueMessage::getData() const {
-  return this->internalBuffer + this->HEADER_SIZE;
+  return this->internalBuffer + MessageQueueMessage::HEADER_SIZE;
 }
 
-uint8_t* MessageQueueMessage::getData() { return this->internalBuffer + this->HEADER_SIZE; }
+uint8_t* MessageQueueMessage::getData() {
+  return this->internalBuffer + MessageQueueMessage::HEADER_SIZE;
+}
 
 MessageQueueId_t MessageQueueMessage::getSender() const {
   MessageQueueId_t temp_id;
@@ -58,14 +60,22 @@ void MessageQueueMessage::print(bool printWholeMessage) {
   }
 }
 
-void MessageQueueMessage::clear() { memset(this->getBuffer(), 0, this->MAX_MESSAGE_SIZE); }
+void MessageQueueMessage::clear() {
+  memset(this->getBuffer(), 0, MessageQueueMessage::MAX_MESSAGE_SIZE);
+}
 
 size_t MessageQueueMessage::getMessageSize() const { return this->messageSize; }
 
-void MessageQueueMessage::setMessageSize(size_t messageSize) { this->messageSize = messageSize; }
+void MessageQueueMessage::setMessageSize(size_t messageSize_) { this->messageSize = messageSize_; }
 
-size_t MessageQueueMessage::getMinimumMessageSize() const { return this->MIN_MESSAGE_SIZE; }
+size_t MessageQueueMessage::getMinimumMessageSize() const {
+  return MessageQueueMessage::MIN_MESSAGE_SIZE;
+}
 
-size_t MessageQueueMessage::getMaximumMessageSize() const { return this->MAX_MESSAGE_SIZE; }
+size_t MessageQueueMessage::getMaximumMessageSize() const {
+  return MessageQueueMessage::MAX_MESSAGE_SIZE;
+}
 
-size_t MessageQueueMessage::getMaximumDataSize() const { return this->MAX_DATA_SIZE; }
+size_t MessageQueueMessage::getMaximumDataSize() const {
+  return MessageQueueMessage::MAX_DATA_SIZE;
+}

src/fsfw/ipc/MessageQueueMessage.h

@@ -25,6 +25,30 @@
  */
 class MessageQueueMessage : public MessageQueueMessageIF {
  public:
+  /**
+   * @brief	This constant defines the maximum size of the data content,
+   * 			excluding the header.
+   * @details
+   * It may be changed if necessary, but in general should be kept
+   * as small as possible.
+   */
+  static const size_t MAX_DATA_SIZE = 24;
+  /**
+   * @brief	This constant defines the maximum total size in bytes
+   * 			of a sent message.
+   * @details
+   * It is the sum of the maximum data and the header size. Be aware that
+   * this constant is used to define the buffer sizes for every message
+   * queue in the system. So, a change here may have significant impact on
+   * the required resources.
+   */
+  static constexpr size_t MAX_MESSAGE_SIZE = MAX_DATA_SIZE + HEADER_SIZE;
+  /**
+   * @brief Defines the minimum size of a message where only the
+   * 		  header is included
+   */
+  static constexpr size_t MIN_MESSAGE_SIZE = HEADER_SIZE;
+
   /**
    * @brief	The class is initialized empty with this constructor.
    * @details
@@ -50,59 +74,12 @@ class MessageQueueMessage : public MessageQueueMessageIF {
    * @brief	As no memory is allocated in this class,
    * 		    the destructor is empty.
    */
-  virtual ~MessageQueueMessage();
+  ~MessageQueueMessage() override;
 
-  /**
-   * @brief	The size information of each message is stored in
-   *  			this attribute.
-   * @details
-   * It is public to simplify usage and to allow for passing the size
-   * address as a pointer. Care must be taken when inheriting from this class,
-   * as every child class is responsible for managing the size information by
-   * itself. When using the class to receive a message, the size information
-   * is updated automatically.
-   *
-   * Please note that the minimum size is limited by the size of the header
-   * while the maximum size is limited by the maximum allowed message size.
-   */
-  size_t messageSize;
-  /**
-   * @brief	This constant defines the maximum size of the data content,
-   * 			excluding the header.
-   * @details
-   * It may be changed if necessary, but in general should be kept
-   * as small as possible.
-   */
-  static const size_t MAX_DATA_SIZE = 24;
-
-  /**
-   * @brief	This constant defines the maximum total size in bytes
-   * 			of a sent message.
-   * @details
-   * It is the sum of the maximum data and the header size. Be aware that
-   * this constant is used to define the buffer sizes for every message
-   * queue in the system. So, a change here may have significant impact on
-   * the required resources.
-   */
-  static constexpr size_t MAX_MESSAGE_SIZE = MAX_DATA_SIZE + HEADER_SIZE;
-  /**
-   * @brief Defines the minimum size of a message where only the
-   * 		  header is included
-   */
-  static constexpr size_t MIN_MESSAGE_SIZE = HEADER_SIZE;
-
- private:
-  /**
-   * @brief	This is the internal buffer that contains the
-   * 			actual message data.
-   */
-  uint8_t internalBuffer[MAX_MESSAGE_SIZE];
-
- public:
   /**
    * @brief	This method is used to get the complete data of the message.
    */
-  const uint8_t* getBuffer() const override;
+  [[nodiscard]] const uint8_t* getBuffer() const override;
   /**
    * @brief	This method is used to get the complete data of the message.
    */
@@ -112,7 +89,7 @@ class MessageQueueMessage : public MessageQueueMessageIF {
    * @details
    * It shall be used by child classes to add data at the right position.
    */
-  const uint8_t* getData() const override;
+  [[nodiscard]] const uint8_t* getData() const override;
   /**
    * @brief	This method is used to fetch the data content of the message.
    * @details
@@ -123,7 +100,7 @@ class MessageQueueMessage : public MessageQueueMessageIF {
    * @brief	This method is used to extract the sender's message
    * 			queue id information from a received message.
    */
-  MessageQueueId_t getSender() const override;
+  [[nodiscard]] MessageQueueId_t getSender() const override;
   /**
    * @brief	With this method, the whole content
    * 			and the message size is set to zero.
@@ -138,16 +115,40 @@ class MessageQueueMessage : public MessageQueueMessageIF {
    */
   void setSender(MessageQueueId_t setId) override;
 
-  virtual size_t getMessageSize() const override;
-  virtual void setMessageSize(size_t messageSize) override;
-  virtual size_t getMinimumMessageSize() const override;
-  virtual size_t getMaximumMessageSize() const override;
-  virtual size_t getMaximumDataSize() const override;
+  [[nodiscard]] size_t getMessageSize() const override;
+  void setMessageSize(size_t messageSize) override;
+  [[nodiscard]] size_t getMinimumMessageSize() const override;
+  [[nodiscard]] size_t getMaximumMessageSize() const override;
+  [[nodiscard]] size_t getMaximumDataSize() const override;
 
   /**
    * @brief	This is a debug method that prints the content.
    */
   void print(bool printWholeMessage);
+
+  /**
+   * TODO: This really should not be public. If it should be possible to pass size address as a
+   *       pointer, add a getter function returning a const reference to the size
+   * @brief	The size information of each message is stored in
+   *  			this attribute.
+   * @details
+   * It is public to simplify usage and to allow for passing the size
+   * address as a pointer. Care must be taken when inheriting from this class,
+   * as every child class is responsible for managing the size information by
+   * itself. When using the class to receive a message, the size information
+   * is updated automatically.
+   *
+   * Please note that the minimum size is limited by the size of the header
+   * while the maximum size is limited by the maximum allowed message size.
+   */
+  size_t messageSize;
+
+ private:
+  /**
+   * @brief	This is the internal buffer that contains the
+   * 			actual message data.
+   */
+  uint8_t internalBuffer[MAX_MESSAGE_SIZE] = {};
 };
 
 #endif /* FSFW_IPC_MESSAGEQUEUEMESSAGE_H_ */

src/fsfw/ipc/MessageQueueMessageIF.h

@@ -14,7 +14,7 @@ class MessageQueueMessageIF {
    */
   static const size_t HEADER_SIZE = sizeof(MessageQueueId_t);
 
-  virtual ~MessageQueueMessageIF(){};
+  virtual ~MessageQueueMessageIF() = default;
 
   /**
    * @brief	With this method, the whole content and the message
@@ -29,7 +29,7 @@ class MessageQueueMessageIF {
    * @brief	Get read-only pointer to the complete data of the message.
    * @return
    */
-  virtual const uint8_t* getBuffer() const = 0;
+  [[nodiscard]] virtual const uint8_t* getBuffer() const = 0;
 
   /**
    * @brief	This method is used to get the complete data of the message.
@@ -48,14 +48,14 @@ class MessageQueueMessageIF {
    * @brief	This method is used to extract the sender's message queue id
    * 			information from a received message.
    */
-  virtual MessageQueueId_t getSender() const = 0;
+  [[nodiscard]] virtual MessageQueueId_t getSender() const = 0;
 
   /**
    * @brief	This method is used to fetch the data content of the message.
    * @details
    * It shall be used by child classes to add data at the right position.
    */
-  virtual const uint8_t* getData() const = 0;
+  [[nodiscard]] virtual const uint8_t* getData() const = 0;
   /**
    * @brief	This method is used to fetch the data content of the message.
    * @details
@@ -67,12 +67,28 @@ class MessageQueueMessageIF {
    * Get constant message size of current message implementation.
    * @return
    */
-  virtual size_t getMessageSize() const = 0;
+  [[nodiscard]] virtual size_t getMessageSize() const = 0;
 
+  /**
+   * Sets the current message size of a given message
+   * @param messageSize
+   */
   virtual void setMessageSize(size_t messageSize) = 0;
-  virtual size_t getMinimumMessageSize() const = 0;
-  virtual size_t getMaximumMessageSize() const = 0;
-  virtual size_t getMaximumDataSize() const = 0;
+  /**
+   * Returns the smallest possible message size, including any headers
+   * @return
+   */
+  [[nodiscard]] virtual size_t getMinimumMessageSize() const = 0;
+  /**
+   * Returns the largest possible message size, including any headers
+   * @return
+   */
+  [[nodiscard]] virtual size_t getMaximumMessageSize() const = 0;
+  /**
+   * Returns the largest possible data size without any headers
+   * @return
+   */
+  [[nodiscard]] virtual size_t getMaximumDataSize() const = 0;
 };
 
 #endif /* FRAMEWORK_IPC_MESSAGEQUEUEMESSAGEIF_H_ */

src/fsfw/ipc/MessageQueueSenderIF.h

@@ -1,14 +1,14 @@
 #ifndef FSFW_IPC_MESSAGEQUEUESENDERIF_H_
 #define FSFW_IPC_MESSAGEQUEUESENDERIF_H_
 
-#include "../objectmanager/ObjectManagerIF.h"
 #include "MessageQueueIF.h"
 #include "MessageQueueMessageIF.h"
+#include "fsfw/objectmanager/ObjectManagerIF.h"
 
 class MessageQueueSenderIF {
  public:
-  virtual ~MessageQueueSenderIF() {}
-
+  virtual ~MessageQueueSenderIF() = default;
+  MessageQueueSenderIF() = delete;
   /**
    * Allows sending messages without actually "owning" a message queue.
    * Not sure whether this is actually a good idea.
@@ -16,9 +16,6 @@ class MessageQueueSenderIF {
   static ReturnValue_t sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                    MessageQueueId_t sentFrom = MessageQueueIF::NO_QUEUE,
                                    bool ignoreFault = false);
-
- private:
-  MessageQueueSenderIF() {}
 };
 
 #endif /* FSFW_IPC_MESSAGEQUEUESENDERIF_H_ */

src/fsfw/modes/HasModesIF.h

@@ -19,32 +19,33 @@ class HasModesIF {
   static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04);
 
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER;
-  static const Event CHANGING_MODE =
-      MAKE_EVENT(0, severity::INFO);  //!< An object announces changing the mode. p1: target mode.
-                                      //!< p2: target submode
-  static const Event MODE_INFO = MAKE_EVENT(
-      1,
-      severity::INFO);  //!< An Object announces its mode; parameter1 is mode, parameter2 is submode
+  //! An object announces changing the mode. p1: target mode. p2: target submode
+  static const Event CHANGING_MODE = MAKE_EVENT(0, severity::INFO);
+  //! An Object announces its mode; parameter1 is mode, parameter2 is submode
+  static const Event MODE_INFO = MAKE_EVENT(1, severity::INFO);
   static const Event FALLBACK_FAILED = MAKE_EVENT(2, severity::HIGH);
   static const Event MODE_TRANSITION_FAILED = MAKE_EVENT(3, severity::LOW);
   static const Event CANT_KEEP_MODE = MAKE_EVENT(4, severity::HIGH);
-  static const Event OBJECT_IN_INVALID_MODE =
-      MAKE_EVENT(5, severity::LOW);  //!< Indicates a bug or configuration failure: Object is in a
-                                     //!< mode it should never be in.
-  static const Event FORCING_MODE = MAKE_EVENT(
-      6, severity::MEDIUM);  //!< The mode is changed, but for some reason, the change is forced,
-                             //!< i.e. EXTERNAL_CONTROL ignored. p1: target mode. p2: target submode
-  static const Event MODE_CMD_REJECTED =
-      MAKE_EVENT(7, severity::LOW);  //!< A mode command was rejected by the called object. Par1:
-                                     //!< called object id, Par2: return code.
+  //! Indicates a bug or configuration failure: Object is in a mode it should never be in.
+  static const Event OBJECT_IN_INVALID_MODE = MAKE_EVENT(5, severity::LOW);
+  //! The mode is changed, but for some reason, the change is forced, i.e. EXTERNAL_CONTROL ignored.
+  //! p1: target mode. p2: target submode
+  static const Event FORCING_MODE = MAKE_EVENT(6, severity::MEDIUM);
+  //! A mode command was rejected by the called object. Par1: called object id, Par2: return code.
+  static const Event MODE_CMD_REJECTED = MAKE_EVENT(7, severity::LOW);
 
-  static const Mode_t MODE_ON =
-      1;  //!< The device is powered and ready to perform operations. In this mode, no commands are
-          //!< sent by the device handler itself, but direct commands van be commanded and will be
-          //!< interpreted
-  static const Mode_t MODE_OFF = 0;  //!< The device is powered off. The only command accepted in
-                                     //!< this mode is a mode change to on.
-  static const Submode_t SUBMODE_NONE = 0;  //!< To avoid checks against magic number "0".
+  //! The device is powered and ready to perform operations. In this mode, no commands are
+  //! sent by the device handler itself, but direct commands van be commanded and will be
+  //! interpreted
+  static constexpr Mode_t MODE_ON = 1;
+  //! The device is powered off. The only command accepted in this mode is a mode change to on.
+  static constexpr Mode_t MODE_OFF = 0;
+
+  static constexpr Mode_t MODE_INVALID = -1;
+  static constexpr Mode_t MODE_UNDEFINED = -2;
+
+  //! To avoid checks against magic number "0".
+  static const Submode_t SUBMODE_NONE = 0;
 
   virtual ~HasModesIF() {}
   virtual MessageQueueId_t getCommandQueue() const = 0;

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));

src/fsfw/objectmanager/ObjectManager.cpp

@@ -21,7 +21,7 @@ void ObjectManager::setObjectFactoryFunction(produce_function_t objFactoryFunc,
   this->factoryArgs = factoryArgs;
 }
 
-ObjectManager::ObjectManager() {}
+ObjectManager::ObjectManager() = default;
 
 ObjectManager::~ObjectManager() {
   for (auto const& iter : objectList) {
@@ -53,7 +53,7 @@ ReturnValue_t ObjectManager::insert(object_id_t id, SystemObjectIF* object) {
 }
 
 ReturnValue_t ObjectManager::remove(object_id_t id) {
-  if (this->getSystemObject(id) != NULL) {
+  if (this->getSystemObject(id) != nullptr) {
     this->objectList.erase(id);
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     // sif::debug << "ObjectManager::removeObject: Object " << std::hex
@@ -95,13 +95,16 @@ void ObjectManager::initialize() {
   for (auto const& it : objectList) {
     result = it.second->initialize();
     if (result != returnvalue::OK) {
+#if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-      object_id_t var = it.first;
       sif::error << "ObjectManager::initialize: Object 0x" << std::hex << std::setw(8)
-                 << std::setfill('0') << var
-                 << " failed to "
-                    "initialize with code 0x"
-                 << result << std::dec << std::setfill(' ') << std::endl;
+                 << std::setfill('0') << it.first << " failed to initialize with code 0x" << result
+                 << std::dec << std::setfill(' ') << std::endl;
+#else
+      sif::printError(
+          "ObjectManager::initialize: Object 0x%08x failed to initialize with code 0x%04x\n", var,
+          it.first);
+#endif
 #endif
       errorCount++;
     }

src/fsfw/objectmanager/ObjectManager.h

@@ -38,7 +38,7 @@ class ObjectManager : public ObjectManagerIF {
   /**
    *  @brief  In the class's destructor, all objects in the list are deleted.
    */
-  virtual ~ObjectManager();
+  ~ObjectManager() override;
   ReturnValue_t insert(object_id_t id, SystemObjectIF* object) override;
   ReturnValue_t remove(object_id_t id) override;
   void initialize() override;

src/fsfw/objectmanager/SystemObjectIF.h

@@ -46,7 +46,7 @@ class SystemObjectIF : public EventReportingProxyIF {
    * Therefore, a two-step initialization resolves this problem and prevents
    * circular dependencies of not-fully initialized objects on start up.
    * @return	- @c returnvalue::OK in case the initialization was successful
-   * 			- @c returnvalue::FAILED otherwise
+   * 		- @c returnvalue::FAILED otherwise
    */
   virtual ReturnValue_t initialize() = 0;
   /**

src/fsfw/objectmanager/frameworkObjects.h

@@ -33,6 +33,7 @@ enum framework_objects : object_id_t {
   TC_STORE = 0x534f0100,
   TM_STORE = 0x534f0200,
   TIME_STAMPER = 0x53500010,
+  VERIFICATION_REPORTER = 0x53500020,
 
   FSFW_OBJECTS_END = 0x53ffffff,
   NO_OBJECT = 0xFFFFFFFF

src/fsfw/osal/CMakeLists.txt

@@ -16,7 +16,9 @@ elseif(FSFW_OSAL MATCHES "host")
 
 else()
 
-  message(WARNING "The OS_FSFW variable was not set. Assuming host OS..")
+  message(
+    WARNING
+      "${MSG_PREFIX} The FSFW_OSAL variable was not set. Assuming host OS..")
   # Not set. Assumuing this is a host build, try to determine host OS
   if(WIN32)
     add_subdirectory(host)

src/fsfw/osal/common/TcpTmTcServer.cpp

@@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) {
 
   while (true) {
     ssize_t retval = recv(connSocket, reinterpret_cast<char*>(receptionBuffer.data()),
-                          receptionBuffer.capacity(), tcpConfig.tcpFlags);
+                          receptionBuffer.size(), tcpConfig.tcpFlags);
     if (retval == 0) {
       size_t availableReadData = ringBuffer.getAvailableReadData();
       if (availableReadData > lastRingBufferSize) {
@@ -335,31 +335,27 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) {
   }
   ringBuffer.readData(receptionBuffer.data(), readAmount, true);
   const uint8_t* bufPtr = receptionBuffer.data();
-  const uint8_t** bufPtrPtr = &bufPtr;
-  size_t startIdx = 0;
-  size_t foundSize = 0;
-  size_t readLen = 0;
-  while (readLen < readAmount) {
-    if (spacePacketParser == nullptr) {
-      return returnvalue::FAILED;
-    }
-    result =
-        spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen);
+  SpacePacketParser::FoundPacketInfo info;
+  if (spacePacketParser == nullptr) {
+    return returnvalue::FAILED;
+  }
+  spacePacketParser->reset();
+  while (spacePacketParser->getAmountRead() < readAmount) {
+    result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info);
     switch (result) {
       case (SpacePacketParser::NO_PACKET_FOUND):
       case (SpacePacketParser::SPLIT_PACKET): {
         break;
       }
       case (returnvalue::OK): {
-        result = handleTcReception(receptionBuffer.data() + startIdx, foundSize);
+        result = handleTcReception(receptionBuffer.data() + info.startIdx, info.sizeFound);
         if (result != returnvalue::OK) {
           status = result;
         }
       }
     }
-    ringBuffer.deleteData(foundSize);
+    ringBuffer.deleteData(info.sizeFound);
     lastRingBufferSize = ringBuffer.getAvailableReadData();
-    std::memset(receptionBuffer.data() + startIdx, 0, foundSize);
   }
   return status;
 }

src/fsfw/osal/host/Clock.cpp

@@ -146,7 +146,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* to) {
-  struct tm time_tm;
+  struct tm time_tm {};
 
   time_tm.tm_year = from->year - 1900;
   time_tm.tm_mon = from->month - 1;

src/fsfw/osal/host/MessageQueue.cpp

@@ -65,11 +65,11 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     // But I will still return a failure here.
     return returnvalue::FAILED;
   }
-  MessageQueue* targetQueue =
+  auto* targetQueue =
       dynamic_cast<MessageQueue*>(QueueMapManager::instance()->getMessageQueue(sendTo));
   if (targetQueue == nullptr) {
     if (not ignoreFault) {
-      InternalErrorReporterIF* internalErrorReporter =
+      auto* internalErrorReporter =
           ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
       if (internalErrorReporter != nullptr) {
         internalErrorReporter->queueMessageNotSent();
@@ -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();

src/fsfw/osal/host/MessageQueue.h

@@ -68,7 +68,7 @@ class MessageQueue : public MessageQueueBase {
    * @details	This is accomplished by using the delete call provided
    * 			by the operating system.
    */
-  virtual ~MessageQueue();
+  ~MessageQueue() override;
 
   // Implement non-generic MessageQueueIF functions not handled by MessageQueueBase
   virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
@@ -111,8 +111,6 @@ class MessageQueue : public MessageQueueBase {
   size_t messageDepth = 0;
 
   MutexIF* queueLock;
-
-  MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE;
 };
 
 #endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */

src/fsfw/osal/host/QueueFactory.cpp

@@ -13,7 +13,6 @@ ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo,
                                                 MessageQueueMessageIF* message,
                                                 MessageQueueId_t sentFrom, bool ignoreFault) {
   return MessageQueue::sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault);
-  return returnvalue::OK;
 }
 
 QueueFactory* QueueFactory::instance() {
@@ -23,9 +22,9 @@ QueueFactory* QueueFactory::instance() {
   return factoryInstance;
 }
 
-QueueFactory::QueueFactory() {}
+QueueFactory::QueueFactory() = default;
 
-QueueFactory::~QueueFactory() {}
+QueueFactory::~QueueFactory() = default;
 
 MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
                                                  MqArgs* args) {

src/fsfw/osal/host/QueueMapManager.cpp

@@ -54,7 +54,7 @@ MessageQueueIF* QueueMapManager::getMessageQueue(MessageQueueId_t messageQueueId
   } else {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "QueueMapManager::getQueueHandle: The ID " << messageQueueId
-                 << " does not exists in the map!" << std::endl;
+                 << " does not exist in the map" << std::endl;
 #else
     sif::printWarning("QueueMapManager::getQueueHandle: The ID %d does not exist in the map!\n",
                       messageQueueId);

src/fsfw/osal/linux/Clock.cpp

@@ -2,23 +2,22 @@
 
 #include <linux/sysinfo.h>
 #include <sys/sysinfo.h>
-#include <sys/time.h>
-#include <time.h>
 #include <unistd.h>
 
+#include <ctime>
 #include <fstream>
 
 #include "fsfw/ipc/MutexGuard.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-uint32_t Clock::getTicksPerSecond(void) {
+uint32_t Clock::getTicksPerSecond() {
   uint32_t ticks = sysconf(_SC_CLK_TCK);
   return ticks;
 }
 
 ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {
-  timespec timeUnix;
-  timeval timeTimeval;
+  timespec timeUnix{};
+  timeval timeTimeval{};
   convertTimeOfDayToTimeval(time, &timeTimeval);
   timeUnix.tv_sec = timeTimeval.tv_sec;
   timeUnix.tv_nsec = (__syscall_slong_t)timeTimeval.tv_usec * 1000;
@@ -32,7 +31,7 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::setClock(const timeval* time) {
-  timespec timeUnix;
+  timespec timeUnix{};
   timeUnix.tv_sec = time->tv_sec;
   timeUnix.tv_nsec = (__syscall_slong_t)time->tv_usec * 1000;
   int status = clock_settime(CLOCK_REALTIME, &timeUnix);
@@ -44,7 +43,7 @@ ReturnValue_t Clock::setClock(const timeval* time) {
 }
 
 ReturnValue_t Clock::getClock_timeval(timeval* time) {
-  timespec timeUnix;
+  timespec timeUnix{};
   int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
   if (status != 0) {
     return returnvalue::FAILED;
@@ -55,18 +54,18 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) {
 }
 
 ReturnValue_t Clock::getClock_usecs(uint64_t* time) {
-  timeval timeVal;
+  timeval timeVal{};
   ReturnValue_t result = getClock_timeval(&timeVal);
   if (result != returnvalue::OK) {
     return result;
   }
-  *time = (uint64_t)timeVal.tv_sec * 1e6 + timeVal.tv_usec;
+  *time = static_cast<uint64_t>(timeVal.tv_sec) * 1e6 + timeVal.tv_usec;
 
   return returnvalue::OK;
 }
 
 timeval Clock::getUptime() {
-  timeval uptime;
+  timeval uptime{};
   auto result = getUptime(&uptime);
   if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -99,7 +98,7 @@ ReturnValue_t Clock::getUptime(timeval* uptime) {
 //}
 
 ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) {
-  timeval uptime;
+  timeval uptime{};
   ReturnValue_t result = getUptime(&uptime);
   if (result != returnvalue::OK) {
     return result;
@@ -109,7 +108,7 @@ ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) {
 }
 
 ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
-  timespec timeUnix;
+  timespec timeUnix{};
   int status = clock_gettime(CLOCK_REALTIME, &timeUnix);
   if (status != 0) {
     // TODO errno
@@ -122,7 +121,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
   MutexGuard helper(timeMutex);
   // gmtime writes its output in a global buffer which is not Thread Safe
   // Therefore we have to use a Mutex here
-  struct tm* timeInfo;
+  struct std::tm* timeInfo;
   timeInfo = gmtime(&timeUnix.tv_sec);
   time->year = timeInfo->tm_year + 1900;
   time->month = timeInfo->tm_mon + 1;
@@ -136,7 +135,7 @@ ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) {
 }
 
 ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, timeval* to) {
-  tm fromTm;
+  std::tm fromTm{};
   // Note: Fails for years before AD
   fromTm.tm_year = from->year - 1900;
   fromTm.tm_mon = from->month - 1;

src/fsfw/parameters/HasParametersIF.h

@@ -66,7 +66,8 @@ class HasParametersIF {
    * @param newValues
    * @param startAtIndex Linear index, runs left to right, top to bottom for
    * matrix indexes.
-   * @return
+   * @return returnvalue::OK if parameter is valid and a set function of the parameter wrapper was
+   * called.
    */
   virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
                                      ParameterWrapper *parameterWrapper,

src/fsfw/parameters/ParameterMessage.cpp

@@ -44,7 +44,7 @@ store_address_t ParameterMessage::getParameterLoadCommand(const CommandMessage*
   *pfc = packedParamSettings >> 16 & 0xff;
   *rows = packedParamSettings >> 8 & 0xff;
   *columns = packedParamSettings & 0xff;
-  return message->getParameter2();
+  return static_cast<store_address_t>(message->getParameter2());
 }
 
 void ParameterMessage::clear(CommandMessage* message) {

src/fsfw/parameters/ParameterWrapper.cpp

@@ -211,9 +211,13 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
   if (data == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable or "
+                    "data pointer not set"
+                 << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Called on read-only variable!\n");
+    sif::printWarning(
+        "ParameterWrapper::copyFrom: Called on read-only variable "
+        "or data pointer not set\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
     return READONLY;
@@ -222,9 +226,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
   if (from->readonlyData == nullptr) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Source not set!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Source not set" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Source not set!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Source not set\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
     return SOURCE_NOT_SET;
@@ -233,9 +237,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
   if (type != from->type) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
     return DATATYPE_MISSMATCH;
@@ -245,9 +249,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
   if (rows == 0 or columns == 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero!" << std::endl;
+    sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero" << std::endl;
 #else
-    sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero!\n");
+    sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero\n");
 #endif
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
     return COLUMN_OR_ROWS_ZERO;

src/fsfw/power/PowerSwitchIF.h

@@ -29,9 +29,9 @@ class PowerSwitchIF {
   static const ReturnValue_t FUSE_ON = MAKE_RETURN_CODE(3);
   static const ReturnValue_t FUSE_OFF = MAKE_RETURN_CODE(4);
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_2;
-  static const Event SWITCH_WENT_OFF = MAKE_EVENT(
-      0, severity::LOW);  //!< Someone detected that a switch went off which shouldn't. Severity:
-                          //!< Low, Parameter1: switchId1, Parameter2: switchId2
+  //!< Someone detected that a switch went off which shouldn't. Severity:
+  //!< Low, Parameter1: switchId1, Parameter2: switchId2
+  static const Event SWITCH_WENT_OFF = MAKE_EVENT(0, severity::LOW);
   /**
    * send a direct command to the Power Unit to enable/disable the specified switch.
    *

src/fsfw/pus/CService200ModeCommanding.cpp

@@ -41,7 +41,7 @@ ReturnValue_t CService200ModeCommanding::getMessageQueueAndObject(uint8_t subser
 
 ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue(
     MessageQueueId_t *messageQueueToSet, object_id_t *objectId) {
-  HasModesIF *destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
+  auto *destination = ObjectManager::instance()->get<HasModesIF>(*objectId);
   if (destination == nullptr) {
     return CommandingServiceBase::INVALID_OBJECT;
   }
@@ -96,13 +96,13 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply
 ReturnValue_t CService200ModeCommanding::prepareModeReply(const CommandMessage *reply,
                                                           object_id_t objectId) {
   ModePacket modeReplyPacket(objectId, ModeMessage::getMode(reply), ModeMessage::getSubmode(reply));
-  return sendTmPacket(Subservice::REPLY_MODE_REPLY, &modeReplyPacket);
+  return sendTmPacket(Subservice::REPLY_MODE_REPLY, modeReplyPacket);
 }
 
 ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(const CommandMessage *reply,
                                                                object_id_t objectId) {
   ModePacket wrongModeReply(objectId, ModeMessage::getMode(reply), ModeMessage::getSubmode(reply));
-  ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, &wrongModeReply);
+  ReturnValue_t result = sendTmPacket(Subservice::REPLY_WRONG_MODE_REPLY, wrongModeReply);
   if (result == returnvalue::OK) {
     // We want to produce an error here in any case because the mode was not correct
     return returnvalue::FAILED;
@@ -113,7 +113,7 @@ ReturnValue_t CService200ModeCommanding::prepareWrongModeReply(const CommandMess
 ReturnValue_t CService200ModeCommanding::prepareCantReachModeReply(const CommandMessage *reply,
                                                                    object_id_t objectId) {
   CantReachModePacket cantReachModePacket(objectId, ModeMessage::getCantReachModeReason(reply));
-  ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE, &cantReachModePacket);
+  ReturnValue_t result = sendTmPacket(Subservice::REPLY_CANT_REACH_MODE, cantReachModePacket);
   if (result == returnvalue::OK) {
     // We want to produce an error here in any case because the mode was not reached
     return returnvalue::FAILED;

src/fsfw/pus/CService201HealthCommanding.cpp

@@ -102,5 +102,5 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep
   auto health = static_cast<uint8_t>(HealthMessage::getHealth(reply));
   auto oldHealth = static_cast<uint8_t>(HealthMessage::getOldHealth(reply));
   HealthSetReply healthSetReply(health, oldHealth);
-  return sendTmPacket(Subservice::REPLY_HEALTH_SET, &healthSetReply);
+  return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply);
 }

src/fsfw/pus/Service11TelecommandScheduling.h

@@ -38,8 +38,9 @@ class Service11TelecommandScheduling final : public PusServiceBase {
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_11;
 
   static constexpr ReturnValue_t INVALID_TYPE_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 1);
-  static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 2);
-  static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 3);
+  static constexpr ReturnValue_t INVALID_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 2);
+  static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 3);
+  static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4);
 
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;
 
@@ -71,8 +72,7 @@ class Service11TelecommandScheduling final : public PusServiceBase {
     TO_TIMETAG = 3
   };
 
-  Service11TelecommandScheduling(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                                 AcceptsTelecommandsIF* tcRecipient,
+  Service11TelecommandScheduling(PsbParams params, AcceptsTelecommandsIF* tcRecipient,
                                  uint16_t releaseTimeMarginSeconds = DEFAULT_RELEASE_TIME_MARGIN,
                                  bool debugMode = false);
 
@@ -156,7 +156,7 @@ class Service11TelecommandScheduling final : public PusServiceBase {
    * @param data The Application data of the TC (get via getApplicationData()).
    * @return requestId
    */
-  uint64_t getRequestIdFromDataTC(const uint8_t* data) const;
+  [[nodiscard]] uint64_t getRequestIdFromTc() const;
 
   /**
    * @brief Extracts the Request ID from the Application Data directly, assuming it is packed

src/fsfw/pus/Service11TelecommandScheduling.tpp

@@ -11,9 +11,9 @@ static constexpr auto DEF_END = SerializeIF::Endianness::BIG;
 
 template <size_t MAX_NUM_TCS>
 inline Service11TelecommandScheduling<MAX_NUM_TCS>::Service11TelecommandScheduling(
-    object_id_t objectId, uint16_t apid, uint8_t serviceId, AcceptsTelecommandsIF *tcRecipient,
-    uint16_t releaseTimeMarginSeconds, bool debugMode)
-    : PusServiceBase(objectId, apid, serviceId),
+    PsbParams params, AcceptsTelecommandsIF *tcRecipient, uint16_t releaseTimeMarginSeconds,
+    bool debugMode)
+    : PusServiceBase(params),
       RELEASE_TIME_MARGIN_SECONDS(releaseTimeMarginSeconds),
       debugMode(debugMode),
       tcRecipient(tcRecipient) {}
@@ -32,11 +32,8 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::handleRequest(
 #endif
   }
   // Get de-serialized Timestamp
-  const uint8_t *data = currentPacket.getApplicationData();
-  size_t size = currentPacket.getApplicationDataSize();
-  if (data == nullptr) {
-    return handleInvalidData("handleRequest");
-  }
+  const uint8_t *data = currentPacket.getUserData();
+  size_t size = currentPacket.getUserDataLen();
   switch (subservice) {
     case Subservice::ENABLE_SCHEDULING: {
       schedulingEnabled = true;
@@ -78,11 +75,11 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::performService
   // NOTE: The iterator is increased in the loop here. Increasing the iterator as for-loop arg
   // does not work in this case as we are deleting the current element here.
   for (auto it = telecommandMap.begin(); it != telecommandMap.end();) {
-    if (it->first <= tNow.tv_sec) {
+    if (it->first <= static_cast<uint32_t>(tNow.tv_sec)) {
       if (schedulingEnabled) {
         // release tc
         TmTcMessage releaseMsg(it->second.storeAddr);
-        auto sendRet = this->requestQueue->sendMessage(recipientMsgQueueId, &releaseMsg, false);
+        auto sendRet = psbParams.reqQueue->sendMessage(recipientMsgQueueId, &releaseMsg, false);
 
         if (sendRet != returnvalue::OK) {
           return sendRet;
@@ -175,7 +172,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   // store currentPacket and receive the store address
   store_address_t addr{};
   if (tcStore->addData(&addr, data, size) != returnvalue::OK ||
-      addr.raw == storeId::INVALID_STORE_ADDRESS) {
+      addr.raw == store_address_t::INVALID_RAW) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service11TelecommandScheduling::doInsertActivity: Adding data to TC Store failed"
                << std::endl;
@@ -190,8 +187,7 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivi
   TelecommandStruct tc;
   tc.seconds = timestamp;
   tc.storeAddr = addr;
-  tc.requestId =
-      getRequestIdFromDataTC(data);  // TODO: Missing sanity check of the returned request id
+  tc.requestId = getRequestIdFromTc();  // TODO: Missing sanity check of the returned request id
 
   auto it = telecommandMap.insert(std::pair<uint32_t, TelecommandStruct>(timestamp, tc));
   if (it == telecommandMap.end()) {
@@ -455,13 +451,10 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doFilterTimesh
 }
 
 template <size_t MAX_NUM_TCS>
-inline uint64_t Service11TelecommandScheduling<MAX_NUM_TCS>::getRequestIdFromDataTC(
-    const uint8_t *data) const {
-  TcPacketPus mask(data);
-
-  uint32_t sourceId = mask.getSourceId();
-  uint16_t apid = mask.getAPID();
-  uint16_t sequenceCount = mask.getPacketSequenceCount();
+inline uint64_t Service11TelecommandScheduling<MAX_NUM_TCS>::getRequestIdFromTc() const {
+  uint32_t sourceId = currentPacket.getSourceId();
+  uint16_t apid = currentPacket.getApid();
+  uint16_t sequenceCount = currentPacket.getSequenceCount();
 
   return buildRequestId(sourceId, apid, sequenceCount);
 }
@@ -571,6 +564,9 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getMapFilterFr
       if (result != returnvalue::OK) {
         return result;
       }
+      if (fromTimestamp > toTimestamp) {
+        return INVALID_TIME_WINDOW;
+      }
       itBegin = telecommandMap.begin();
       itEnd = telecommandMap.begin();
 
@@ -586,17 +582,6 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::getMapFilterFr
     default:
       return returnvalue::FAILED;
   }
-
-  // additional security check, this should never be true
-  if (itBegin > itEnd) {
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "11::getMapFilterFromData: itBegin > itEnd\n" << std::endl;
-#else
-    sif::printError("11::getMapFilterFromData: itBegin > itEnd\n");
-#endif
-    return returnvalue::FAILED;
-  }
-
   // the map range should now be set according to the sent filter.
   return returnvalue::OK;
 }

src/fsfw/pus/Service17Test.cpp

@@ -1,39 +1,33 @@
 #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/tmHelpers.h"
 
-Service17Test::Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId)
-    : PusServiceBase(objectId, apid, serviceId), packetSubCounter(0) {}
+Service17Test::Service17Test(PsbParams params)
+    : PusServiceBase(params),
+      storeHelper(params.apid),
+      tmHelper(params.serviceId, storeHelper, sendHelper) {}
 
-Service17Test::~Service17Test() {}
+Service17Test::~Service17Test() = default;
 
 ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
   switch (subservice) {
     case Subservice::CONNECTION_TEST: {
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-      TmPacketStoredPusA connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
-                                          packetSubCounter++);
-#else
-      TmPacketStoredPusC connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
-                                          packetSubCounter++);
-#endif
-      connectionPacket.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
-      return returnvalue::OK;
+      ReturnValue_t result = tmHelper.prepareTmPacket(Subservice::CONNECTION_TEST_REPORT);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      return tmHelper.storeAndSendTmPacket();
     }
     case Subservice::EVENT_TRIGGER_TEST: {
-#if FSFW_USE_PUS_C_TELEMETRY == 0
-      TmPacketStoredPusA connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
-                                          packetSubCounter++);
-#else
-      TmPacketStoredPusC connectionPacket(apid, serviceId, Subservice::CONNECTION_TEST_REPORT,
-                                          packetSubCounter++);
-#endif
-      connectionPacket.sendPacket(requestQueue->getDefaultDestination(), requestQueue->getId());
       triggerEvent(TEST, 1234, 5678);
-      return returnvalue::OK;
+      ReturnValue_t result = tmHelper.prepareTmPacket(Subservice::EVENT_TRIGGER_TEST);
+      if (result != returnvalue::OK) {
+        return result;
+      }
+      return tmHelper.storeAndSendTmPacket();
     }
     default:
       return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
@@ -41,3 +35,23 @@ ReturnValue_t Service17Test::handleRequest(uint8_t subservice) {
 }
 
 ReturnValue_t Service17Test::performService() { return returnvalue::OK; }
+
+ReturnValue_t Service17Test::initialize() {
+  ReturnValue_t result = PusServiceBase::initialize();
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  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_);
+}

src/fsfw/pus/Service17Test.h

@@ -3,6 +3,8 @@
 
 #include "fsfw/objectmanager/SystemObject.h"
 #include "fsfw/tmtcservices/PusServiceBase.h"
+#include "fsfw/tmtcservices/TmStoreAndSendHelper.h"
+#include "fsfw/tmtcservices/TmStoreHelper.h"
 
 /**
  * @brief Test Service
@@ -32,13 +34,19 @@ class Service17Test : public PusServiceBase {
     EVENT_TRIGGER_TEST = 128,
   };
 
-  Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId);
-  virtual ~Service17Test();
-  virtual ReturnValue_t handleRequest(uint8_t subservice) override;
-  virtual ReturnValue_t performService() override;
+  explicit Service17Test(PsbParams params);
+
+  void setCustomTmStore(StorageManagerIF& tmStore);
+
+  ~Service17Test() override;
+  ReturnValue_t handleRequest(uint8_t subservice) override;
+  ReturnValue_t performService() override;
+  ReturnValue_t initialize() override;
 
  protected:
-  uint16_t packetSubCounter = 0;
+  TmStoreHelper storeHelper;
+  TmSendHelper sendHelper;
+  TmStoreAndSendWrapper tmHelper;
 };
 
 #endif /* FSFW_PUS_SERVICE17TEST_H_ */

src/fsfw/pus/Service1TelecommandVerification.cpp

@@ -3,19 +3,21 @@
 #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/tmHelpers.h"
 
 Service1TelecommandVerification::Service1TelecommandVerification(object_id_t objectId,
                                                                  uint16_t apid, uint8_t serviceId,
                                                                  object_id_t targetDestination,
-                                                                 uint16_t messageQueueDepth)
+                                                                 uint16_t messageQueueDepth,
+                                                                 TimeStamperIF* timeStamper)
     : SystemObject(objectId),
       apid(apid),
       serviceId(serviceId),
-      targetDestination(targetDestination) {
+      targetDestination(targetDestination),
+      storeHelper(apid),
+      tmHelper(serviceId, storeHelper, sendHelper) {
   tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
 }
 
@@ -47,6 +49,19 @@ ReturnValue_t Service1TelecommandVerification::performOperation(uint8_t operatio
 ReturnValue_t Service1TelecommandVerification::sendVerificationReport(
     PusVerificationMessage* message) {
   ReturnValue_t result;
+  uint8_t reportId = message->getReportId();
+  if (reportId == 0 or reportId > 8) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "Service1TelecommandVerification::sendVerificationReport: Invalid report ID "
+               << static_cast<int>(reportId) << " detected" << std::endl;
+#else
+    sif::printError(
+        "Service1TelecommandVerification::sendVerificationReport: Invalid report ID "
+        "%d detected\n",
+        reportId);
+#endif
+    return returnvalue::FAILED;
+  }
   if (message->getReportId() % 2 == 0) {
     result = generateFailureReport(message);
   } else {
@@ -67,32 +82,37 @@ 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;
+  ReturnValue_t result =
+      storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return tmHelper.storeAndSendTmPacket();
 }
 
 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;
+  ReturnValue_t result =
+      storeHelper.preparePacket(serviceId, message->getReportId(), packetSubCounter++);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  result = storeHelper.setSourceDataSerializable(report);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  return tmHelper.storeAndSendTmPacket();
 }
 
 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"
@@ -102,6 +122,33 @@ ReturnValue_t Service1TelecommandVerification::initialize() {
 #endif
     return ObjectManagerIF::CHILD_INIT_FAILED;
   }
+  if (tmQueue == nullptr) {
+    return ObjectManagerIF::CHILD_INIT_FAILED;
+  }
   tmQueue->setDefaultDestination(funnel->getReportReceptionQueue());
+  if (tmStore == nullptr) {
+    tmStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TM_STORE);
+    if (tmStore == nullptr) {
+      return ObjectManager::CHILD_INIT_FAILED;
+    }
+    storeHelper.setTmStore(*tmStore);
+  }
+  if (timeStamper == nullptr) {
+    timeStamper = ObjectManager::instance()->get<TimeStamperIF>(objects::TIME_STAMPER);
+    if (timeStamper == nullptr) {
+      return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+  } else {
+  }
+  storeHelper.setTimeStamper(*timeStamper);
+
+  sendHelper.setMsgQueue(*tmQueue);
+  if (errReporter == nullptr) {
+    errReporter =
+        ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
+    if (errReporter != nullptr) {
+      sendHelper.setInternalErrorReporter(*errReporter);
+    }
+  }
   return SystemObject::initialize();
 }

src/fsfw/pus/Service1TelecommandVerification.h

@@ -7,6 +7,9 @@
 #include "fsfw/tasks/ExecutableObjectIF.h"
 #include "fsfw/tmtcservices/AcceptsVerifyMessageIF.h"
 #include "fsfw/tmtcservices/PusVerificationReport.h"
+#include "fsfw/tmtcservices/TmSendHelper.h"
+#include "fsfw/tmtcservices/TmStoreAndSendHelper.h"
+#include "fsfw/tmtcservices/TmStoreHelper.h"
 
 /**
  * @brief Verify TC acceptance, start, progress and execution.
@@ -43,14 +46,15 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_1;
 
   Service1TelecommandVerification(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                                  object_id_t targetDestination, uint16_t messageQueueDepth);
-  virtual ~Service1TelecommandVerification();
+                                  object_id_t targetDestination, uint16_t messageQueueDepth,
+                                  TimeStamperIF* timeStamper = nullptr);
+  ~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().
@@ -58,7 +62,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
@@ -79,6 +83,12 @@ class Service1TelecommandVerification : public AcceptsVerifyMessageIF,
 
   uint16_t packetSubCounter = 0;
 
+  TmSendHelper sendHelper;
+  TmStoreHelper storeHelper;
+  TmStoreAndSendWrapper tmHelper;
+  InternalErrorReporterIF* errReporter = nullptr;
+  TimeStamperIF* timeStamper = nullptr;
+  StorageManagerIF* tmStore = nullptr;
   MessageQueueIF* tmQueue = nullptr;
 
   enum class Subservice : uint8_t {

src/fsfw/pus/Service20ParameterManagement.cpp

@@ -14,7 +14,7 @@ Service20ParameterManagement::Service20ParameterManagement(object_id_t objectId,
     : CommandingServiceBase(objectId, apid, serviceId, numberOfParallelCommands,
                             commandTimeoutSeconds) {}
 
-Service20ParameterManagement::~Service20ParameterManagement() {}
+Service20ParameterManagement::~Service20ParameterManagement() = default;
 
 ReturnValue_t Service20ParameterManagement::isValidSubservice(uint8_t subservice) {
   switch (static_cast<Subservice>(subservice)) {
@@ -64,8 +64,7 @@ ReturnValue_t Service20ParameterManagement::checkAndAcquireTargetID(object_id_t*
 ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue(
     MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
   // check ReceivesParameterMessagesIF property of target
-  ReceivesParameterMessagesIF* possibleTarget =
-      ObjectManager::instance()->get<ReceivesParameterMessagesIF>(*objectId);
+  auto* possibleTarget = ObjectManager::instance()->get<ReceivesParameterMessagesIF>(*objectId);
   if (possibleTarget == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire"
@@ -137,7 +136,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 != returnvalue::OK) {
     return result;
   }
@@ -169,7 +168,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 != returnvalue::OK) {
         return returnvalue::FAILED;
       }
@@ -177,8 +176,7 @@ ReturnValue_t Service20ParameterManagement::handleReply(const CommandMessage* re
       ParameterId_t parameterId = ParameterMessage::getParameterId(reply);
       ParameterDumpReply parameterReply(objectId, parameterId, parameterData.second.data(),
                                         parameterData.second.size());
-      sendTmPacket(static_cast<uint8_t>(Subservice::PARAMETER_DUMP_REPLY), &parameterReply);
-      return returnvalue::OK;
+      return sendTmPacket(static_cast<uint8_t>(Subservice::PARAMETER_DUMP_REPLY), parameterReply);
     }
     default:
       return CommandingServiceBase::INVALID_REPLY;

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 != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service2DeviceAccess::sendWiretappingTm: Data Lost in "
@@ -147,10 +147,12 @@ 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);
+  result = sendTmPacket(subservice, tmPacket.objectId, tmPacket.data, size);
+  if (result != returnvalue::OK) {
+    // TODO: Warning
+    return;
+  }
 }
 
 MessageQueueId_t Service2DeviceAccess::getDeviceQueue() { return commandQueue->getId(); }

src/fsfw/pus/Service3Housekeeping.cpp

@@ -208,17 +208,17 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply,
       ReturnValue_t error = returnvalue::FAILED;
       HousekeepingMessage::getHkRequestFailureReply(reply, &error);
       failureParameter2 = error;
-      return CommandingServiceBase::EXECUTION_COMPLETE;
+      return returnvalue::FAILED;
     }
 
     default:
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::warning << "Service3Housekeeping::handleReply: Invalid reply with "
-                   << "reply command " << command << "!" << std::endl;
+                   << "reply command " << command << std::endl;
 #else
       sif::printWarning(
           "Service3Housekeeping::handleReply: Invalid reply with "
-          "reply command %hu!\n",
+          "reply command %hu\n",
           command);
 #endif
       return CommandingServiceBase::INVALID_REPLY;
@@ -248,19 +248,28 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) {
     case (HousekeepingMessage::HK_REQUEST_FAILURE): {
       break;
     }
+    case (CommandMessage::REPLY_REJECTED): {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+      sif::warning << "Service3Housekeeping::handleUnrequestedReply: Unexpected reply "
+                      "rejected with error code"
+                   << reply->getParameter() << std::endl;
+#else
+#endif
+      break;
+    }
 
     default: {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::warning << "Service3Housekeeping::handleUnrequestedReply: Invalid reply with reply "
                       "command "
-                   << command << "!" << std::endl;
+                   << command << "" << std::endl;
 #else
       sif::printWarning(
           "Service3Housekeeping::handleUnrequestedReply: Invalid reply with "
-          "reply command %hu!\n",
+          "reply command %hu\n",
           command);
 #endif
-      return;
+      break;
     }
   }
 
@@ -275,6 +284,7 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) {
         "Could not generate reply!\n");
 #endif
   }
+  CommandingServiceBase::handleUnrequestedReply(reply);
 }
 
 MessageQueueId_t Service3Housekeeping::getHkQueue() const { return commandQueue->getId(); }
@@ -284,14 +294,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 != returnvalue::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,

src/fsfw/pus/Service5EventReporting.cpp

@@ -5,14 +5,17 @@
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/pus/servicepackets/Service5Packets.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
-#include "fsfw/tmtcpacket/pus/tm/TmPacketStored.h"
+#include "fsfw/tmtcservices/tmHelpers.h"
 
-Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t apid,
-                                               uint8_t serviceId, size_t maxNumberReportsPerCycle,
+Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumberReportsPerCycle,
                                                uint32_t messageQueueDepth)
-    : PusServiceBase(objectId, apid, serviceId),
+    : PusServiceBase(params),
+      storeHelper(params.apid),
+      tmHelper(params.serviceId, storeHelper, sendHelper),
       maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
-  eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+  auto mqArgs = MqArgs(getObjectId(), static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 Service5EventReporting::~Service5EventReporting() {
@@ -36,24 +39,15 @@ ReturnValue_t Service5EventReporting::performService() {
       }
     }
   }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::warning << "Service5EventReporting::generateEventReport: Too many events" << std::endl;
-#endif
   return returnvalue::OK;
 }
 
 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(psbParams.serviceId, message.getSeverity(), tmHelper.sendCounter);
+  storeHelper.setSourceDataSerializable(report);
+  ReturnValue_t result = tmHelper.storeAndSendTmPacket();
   if (result != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service5EventReporting::generateEventReport: "
@@ -86,14 +80,19 @@ 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 != returnvalue::OK) {
+    return result;
+  }
   auto* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
   if (manager == nullptr) {
     return returnvalue::FAILED;
   }
   // register Service 5 as listener for events
-  ReturnValue_t result = manager->registerListener(eventQueue->getId(), true);
+  result = manager->registerListener(eventQueue->getId(), true);
   if (result != returnvalue::OK) {
     return result;
   }
-  return PusServiceBase::initialize();
+  initializeTmHelpers(sendHelper, storeHelper);
+  return result;
 }

src/fsfw/pus/Service5EventReporting.h

@@ -3,6 +3,7 @@
 
 #include "fsfw/events/EventMessage.h"
 #include "fsfw/tmtcservices/PusServiceBase.h"
+#include "fsfw/tmtcservices/TmStoreAndSendHelper.h"
 
 /**
  * @brief Report on-board events like information or errors
@@ -40,9 +41,9 @@
  */
 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(PsbParams params, size_t maxNumberReportsPerCycle = 10,
+                         uint32_t messageQueueDepth = 10);
+  ~Service5EventReporting() override;
 
   /***
    * Check for events and generate event reports if required.
@@ -74,9 +75,11 @@ class Service5EventReporting : public PusServiceBase {
   };
 
  private:
-  uint16_t packetSubCounter = 0;
   MessageQueueIF* eventQueue = nullptr;
   bool enableEventReport = true;
+  TmSendHelper sendHelper;
+  TmStoreHelper storeHelper;
+  TmStoreAndSendWrapper tmHelper;
   const uint8_t maxNumberReportsPerCycle;
 
   ReturnValue_t generateEventReport(EventMessage message);

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 != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::error << "Service 8: Could not retrieve data for data reply" << std::endl;
@@ -138,9 +138,9 @@ ReturnValue_t Service8FunctionManagement::handleDataReply(const CommandMessage*
     return result;
   }
   DataReply dataReply(objectId, actionId, buffer, size);
-  result = sendTmPacket(static_cast<uint8_t>(Subservice::REPLY_DIRECT_COMMANDING_DATA), &dataReply);
+  result = sendTmPacket(static_cast<uint8_t>(Subservice::REPLY_DIRECT_COMMANDING_DATA), dataReply);
 
-  auto deletionResult = IPCStore->deleteData(storeId);
+  auto deletionResult = ipcStore->deleteData(storeId);
   if (deletionResult != returnvalue::OK) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
     sif::warning << "Service8FunctionManagement::handleReply: Deletion"

src/fsfw/pus/Service8FunctionManagement.h

@@ -32,7 +32,7 @@ class Service8FunctionManagement : public CommandingServiceBase {
  public:
   Service8FunctionManagement(object_id_t objectId, uint16_t apid, uint8_t serviceId,
                              uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60);
-  virtual ~Service8FunctionManagement();
+  ~Service8FunctionManagement() override;
 
  protected:
   /* CSB abstract functions implementation . See CSB documentation. */

src/fsfw/pus/Service9TimeManagement.cpp

@@ -5,11 +5,9 @@
 #include "fsfw/serviceinterface/ServiceInterface.h"
 #include "fsfw/timemanager/CCSDSTime.h"
 
-Service9TimeManagement::Service9TimeManagement(object_id_t objectId, uint16_t apid,
-                                               uint8_t serviceId)
-    : PusServiceBase(objectId, apid, serviceId) {}
+Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {}
 
-Service9TimeManagement::~Service9TimeManagement() {}
+Service9TimeManagement::~Service9TimeManagement() = default;
 
 ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK; }
 
@@ -25,7 +23,7 @@ ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
 
 ReturnValue_t Service9TimeManagement::setTime() {
   Clock::TimeOfDay_t timeToSet;
-  TimePacket timePacket(currentPacket.getApplicationData(), currentPacket.getApplicationDataSize());
+  TimePacket timePacket(currentPacket.getUserData(), currentPacket.getUserDataLen());
   ReturnValue_t result =
       CCSDSTime::convertFromCcsds(&timeToSet, timePacket.getTime(), timePacket.getTimeSize());
   if (result != returnvalue::OK) {

src/fsfw/pus/Service9TimeManagement.h

@@ -6,26 +6,26 @@
 class Service9TimeManagement : public PusServiceBase {
  public:
   static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
-  static constexpr Event CLOCK_SET =
-      MAKE_EVENT(0, severity::INFO);  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
-  static constexpr Event CLOCK_SET_FAILURE =
-      MAKE_EVENT(1, severity::LOW);  //!< Clock could not be set. P1: Returncode.
+  //!< Clock has been set. P1: New Uptime. P2: Old Uptime
+  static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
+  //!< Clock could not be set. P1: Returncode.
+  static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW);
 
   static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
 
   /**
    * 	@brief This service provides the capability to set the on-board time.
    */
-  Service9TimeManagement(object_id_t objectId, uint16_t apid, uint8_t serviceId);
+  explicit Service9TimeManagement(PsbParams params);
 
-  virtual ~Service9TimeManagement();
+  ~Service9TimeManagement() override;
 
-  virtual ReturnValue_t performService() override;
+  ReturnValue_t performService() override;
 
   /**
    * 	@brief Sets the onboard-time by retrieving the time to set from TC[9,128].
    */
-  virtual ReturnValue_t handleRequest(uint8_t subservice) override;
+  ReturnValue_t handleRequest(uint8_t subservice) override;
 
   virtual ReturnValue_t setTime();
 

src/fsfw/pus/servicepackets/Service1Packets.h

@@ -50,7 +50,7 @@ class FailureReport : public SerializeIF {  //!< [EXPORT] : [SUBSERVICE] 2, 4, 6
     if (result != returnvalue::OK) {
       return result;
     }
-    if (failureSubtype == tc_verification::PROGRESS_FAILURE) {
+    if (failureSubtype == tcverif::PROGRESS_FAILURE) {
       result = SerializeAdapter::serialize(&stepNumber, buffer, size, maxSize, streamEndianness);
       if (result != returnvalue::OK) {
         return result;
@@ -73,7 +73,7 @@ class FailureReport : public SerializeIF {  //!< [EXPORT] : [SUBSERVICE] 2, 4, 6
     size_t size = 0;
     size += SerializeAdapter::getSerializedSize(&packetId);
     size += sizeof(packetSequenceControl);
-    if (failureSubtype == tc_verification::PROGRESS_FAILURE) {
+    if (failureSubtype == tcverif::PROGRESS_FAILURE) {
       size += SerializeAdapter::getSerializedSize(&stepNumber);
     }
     size += SerializeAdapter::getSerializedSize(&errorCode);
@@ -130,7 +130,7 @@ class SuccessReport : public SerializeIF {  //!< [EXPORT] : [SUBSERVICE] 1, 3, 5
     if (result != returnvalue::OK) {
       return result;
     }
-    if (subtype == tc_verification::PROGRESS_SUCCESS) {
+    if (subtype == tcverif::PROGRESS_SUCCESS) {
       result = SerializeAdapter::serialize(&stepNumber, buffer, size, maxSize, streamEndianness);
       if (result != returnvalue::OK) {
         return result;
@@ -143,7 +143,7 @@ class SuccessReport : public SerializeIF {  //!< [EXPORT] : [SUBSERVICE] 1, 3, 5
     size_t size = 0;
     size += SerializeAdapter::getSerializedSize(&packetId);
     size += sizeof(packetSequenceControl);
-    if (subtype == tc_verification::PROGRESS_SUCCESS) {
+    if (subtype == tcverif::PROGRESS_SUCCESS) {
       size += SerializeAdapter::getSerializedSize(&stepNumber);
     }
     return size;

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

src/fsfw/returnvalues/FwClassIds.h

@@ -34,9 +34,10 @@ enum : uint8_t {
   FIFO_CLASS,                     // FF
   MESSAGE_PROXY,                  // MQP
   TRIPLE_REDUNDACY_CHECK,         // TRC
-  TC_PACKET_CHECK,                // TCC
+  PACKET_CHECK,                   // TCC
   PACKET_DISTRIBUTION,            // TCD
-  ACCEPTS_TELECOMMANDS_IF,        // PUS
+  ACCEPTS_TELECOMMANDS_IF,        // ATC
+  PUS_IF,                         // PUS
   DEVICE_SERVICE_BASE,            // DSB
   COMMAND_SERVICE_BASE,           // CSB
   TM_STORE_BACKEND_IF,            // TMB

src/fsfw/returnvalues/HasReturnvaluesIF.h

@@ -0,0 +1,8 @@
+#ifndef FSFW_RETURNVALUES_HASRETURNVALUES_IF_H_
+#define FSFW_RETURNVALUES_HASRETURNVALUES_IF_H_
+
+#warning "This header is deprecated, please include returnvalue.h"
+
+#include "returnvalue.h"
+
+#endif /* FSFW_RETURNVALUES_HASRETURNVALUES_IF_H_ */

src/fsfw/retval.h

@@ -0,0 +1,6 @@
+#ifndef FSFW_RETVAL_H
+#define FSFW_RETVAL_H
+
+#include "fsfw/returnvalues/returnvalue.h"
+
+#endif  // FSFW_RETVAL_H

src/fsfw/serialize.h

@@ -1,10 +1,10 @@
-#ifndef FSFW_INC_FSFW_SERIALIZE_H_
-#define FSFW_INC_FSFW_SERIALIZE_H_
+#ifndef FSFW_SERIALIZE_H_
+#define FSFW_SERIALIZE_H_
 
-#include "fsfw/serialize/EndianConverter.h"
-#include "fsfw/serialize/SerialArrayListAdapter.h"
-#include "fsfw/serialize/SerialBufferAdapter.h"
-#include "fsfw/serialize/SerialLinkedListAdapter.h"
-#include "fsfw/serialize/SerializeElement.h"
+#include "serialize/EndianConverter.h"
+#include "serialize/SerialArrayListAdapter.h"
+#include "serialize/SerialBufferAdapter.h"
+#include "serialize/SerialLinkedListAdapter.h"
+#include "serialize/SerializeElement.h"
 
-#endif /* FSFW_INC_FSFW_SERIALIZE_H_ */
+#endif /* FSFW_SERIALIZE_H_ */

src/fsfw/serialize/SerializeAdapter.h

@@ -74,7 +74,7 @@ class SerializeAdapter {
       return returnvalue::FAILED;
     }
     InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
-    uint8_t **tempPtr = const_cast<uint8_t **>(&buffer);
+    auto **tempPtr = const_cast<uint8_t **>(&buffer);
     size_t tmpSize = 0;
     ReturnValue_t result = adapter.serialize(object, tempPtr, &tmpSize, maxSize, streamEndianness);
     if (serSize != nullptr) {
@@ -232,7 +232,7 @@ class SerializeAdapter {
       }
     }
 
-    uint32_t getSerializedSize(const T *object) { return sizeof(T); }
+    uint32_t getSerializedSize(const T *) { return sizeof(T); }
   };
 
   /**

src/fsfw/storagemanager/LocalPool.cpp

@@ -194,8 +194,6 @@ void LocalPool::clearStore() {
     for (auto& size : sizeList) {
       size = STORAGE_FREE;
     }
-    //        std::memset(sizeList[index], 0xff,
-    //                numberOfElements[index] * sizeof(size_type));
   }
 }
 
@@ -338,3 +336,16 @@ void LocalPool::clearSubPool(max_subpools_t subpoolIndex) {
 }
 
 LocalPool::max_subpools_t LocalPool::getNumberOfSubPools() const { return NUMBER_OF_SUBPOOLS; }
+
+bool LocalPool::hasDataAtId(store_address_t storeId) const {
+  if (storeId.poolIndex >= NUMBER_OF_SUBPOOLS) {
+    return false;
+  }
+  if ((storeId.packetIndex >= numberOfElements[storeId.poolIndex])) {
+    return false;
+  }
+  if (sizeLists[storeId.poolIndex][storeId.packetIndex] != STORAGE_FREE) {
+    return true;
+  }
+  return false;
+}

src/fsfw/storagemanager/LocalPool.h

@@ -81,7 +81,7 @@ class LocalPool : public SystemObject, public StorageManagerIF {
   /**
    * @brief	In the LocalPool's destructor all allocated memory is freed.
    */
-  virtual ~LocalPool(void);
+  ~LocalPool() override;
 
   /**
    * Documentation: See StorageManagerIF.h
@@ -132,6 +132,7 @@ class LocalPool : public SystemObject, public StorageManagerIF {
    * @return
    */
   max_subpools_t getNumberOfSubPools() const override;
+  bool hasDataAtId(store_address_t storeId) const override;
 
  protected:
   /**

src/fsfw/storagemanager/PoolManager.cpp

@@ -7,7 +7,7 @@ PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPo
   mutex = MutexFactory::instance()->createMutex();
 }
 
-PoolManager::~PoolManager(void) { MutexFactory::instance()->deleteMutex(mutex); }
+PoolManager::~PoolManager() { MutexFactory::instance()->deleteMutex(mutex); }
 
 ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* address,
                                         bool ignoreFault) {
@@ -17,7 +17,7 @@ ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* addr
 }
 
 ReturnValue_t PoolManager::deleteData(store_address_t storeId) {
-#if FSFW_VERBOSE_LEVEL >= 2
+#if FSFW_VERBOSE_LEVEL >= 2 && FSFW_OBJ_EVENT_TRANSLATION == 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
   sif::debug << "PoolManager( " << translateObject(getObjectId()) << " )::deleteData from store "
              << storeId.poolIndex << ". id is " << storeId.packetIndex << std::endl;

src/fsfw/storagemanager/StorageManagerIF.h

@@ -55,7 +55,7 @@ class StorageManagerIF {
   /**
    * @brief This is the empty virtual destructor as required for C++ interfaces.
    */
-  virtual ~StorageManagerIF(){};
+  ~StorageManagerIF() = default;
   /**
    * @brief	With addData, a free storage position is allocated and data
    * 			stored there.
@@ -63,9 +63,8 @@ class StorageManagerIF {
    * @param storageId A pointer to the storageId to retrieve.
    * @param data	The data to be stored in the StorageManager.
    * @param size	The amount of data to be stored.
-   * @return	Returns @li returnvalue::OK if data was added.
-   * 					@li	returnvalue::FAILED if data could not be added.
-   * 						storageId is unchanged then.
+   * @return	Returns @returnvalue::OK if data was added.
+   * 		@returnvalue::FAILED if data could not be added, storageId is unchanged then.
    */
   virtual ReturnValue_t addData(store_address_t* storageId, const uint8_t* data, size_t size,
                                 bool ignoreFault = false) = 0;
@@ -116,8 +115,8 @@ class StorageManagerIF {
    * @param packet_ptr	The passed pointer address is set to the the memory
    * 						position
    * @param size			The exact size of the stored data is returned here.
-   * @return	@li returnvalue::OK on success.
-   * 			@li	returnvalue::FAILED if fetching data did not work
+   * @return	@returnvalue::OK on success.
+   * 		@returnvalue::FAILED if fetching data did not work
    * 				(e.g. an illegal packet_id was passed).
    */
   virtual ReturnValue_t getData(store_address_t packet_id, const uint8_t** packet_ptr,
@@ -157,11 +156,12 @@ class StorageManagerIF {
    * @param size		The size of the space to be reserved.
    * @param p_data	A pointer to the element data is returned here.
    * @return	Returns @li returnvalue::OK if data was added.
-   * 					@li	returnvalue::FAILED if data could not be added.
-   * 						storageId is unchanged then.
+   * 		@returnvalue::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;
+
+  [[nodiscard]] virtual bool hasDataAtId(store_address_t storeId) const = 0;
 
   /**
    * Clears the whole store.
@@ -192,7 +192,7 @@ class StorageManagerIF {
    * 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_ */

src/fsfw/storagemanager/storeAddress.h

@@ -3,26 +3,26 @@
 
 #include <cstdint>
 
-namespace storeId {
-static constexpr uint32_t INVALID_STORE_ADDRESS = 0xffffffff;
-}
-
 /**
  * This union defines the type that identifies where a data packet is
  * stored in the store. It comprises of a raw part to read it as raw value and
  * a structured part to use it in pool-like stores.
  */
 union store_address_t {
+ public:
+  static constexpr uint32_t INVALID_RAW = 0xffffffff;
   /**
    * Default Constructor, initializing to INVALID_ADDRESS
    */
-  store_address_t() : raw(storeId::INVALID_STORE_ADDRESS) {}
+  store_address_t() : raw(INVALID_RAW) {}
   /**
    * Constructor to create an address object using the raw address
    *
    * @param rawAddress
    */
-  store_address_t(uint32_t rawAddress) : raw(rawAddress) {}
+  explicit store_address_t(uint32_t rawAddress) : raw(rawAddress) {}
+
+  static store_address_t invalid() { return {}; };
 
   /**
    * Constructor to create an address object using pool
@@ -52,6 +52,12 @@ union store_address_t {
   uint32_t raw;
 
   bool operator==(const store_address_t& other) const { return raw == other.raw; }
+  bool operator!=(const store_address_t& other) const { return raw != other.raw; }
+
+  store_address_t& operator=(const uint32_t rawAddr) {
+    raw = rawAddr;
+    return *this;
+  }
 };
 
 #endif /* FSFW_STORAGEMANAGER_STOREADDRESS_H_ */

src/fsfw/subsystem/Subsystem.h

@@ -33,8 +33,12 @@ struct SequenceEntry : public TableSequenceBase {
 };
 
 /**
- * @brief   TODO: documentation missing
+ * @brief   This class extends the SubsystemBase to perform the management of mode tables
+ *          and mode sequences
  * @details
+ * This class is able to use mode tables and sequences to command all its children into the
+ * right mode. Fallback sequences can be used to handle failed transitions or have a fallback
+ * in case a component can't keep its current mode.
  */
 class Subsystem : public SubsystemBase, public HasModeSequenceIF {
  public:

src/fsfw/subsystem/SubsystemBase.cpp

@@ -8,11 +8,13 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t
                              uint16_t commandQueueDepth)
     : SystemObject(setObjectId),
       mode(initialMode),
-      commandQueue(QueueFactory::instance()->createMessageQueue(commandQueueDepth,
-                                                                CommandMessage::MAX_MESSAGE_SIZE)),
       healthHelper(this, setObjectId),
       modeHelper(this),
-      parentId(parent) {}
+      parentId(parent) {
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs);
+}
 
 SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
 
@@ -31,8 +33,9 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) {
       info.mode = MODE_OFF;
     }
   } else {
+    // intentional to force an initial command during system startup
     info.commandQueue = child->getCommandQueue();
-    info.mode = -1;  // intentional to force an initial command during system startup
+    info.mode = HasModesIF::MODE_UNDEFINED;
   }
 
   info.submode = SUBMODE_NONE;

src/fsfw/subsystem/SubsystemBase.h

@@ -15,7 +15,14 @@
 
 /**
  * @defgroup subsystems Subsystem Objects
- * Contains all Subsystem and Assemblies
+ * All Subsystem and Assemblies can derive from this class. It contains helper classes to
+ * perform mode and health handling, which allows OBSW developers to build a mode tree for
+ * the whole satellite.
+ *
+ * Aside from setting up a mode tree and being able to executing mode tables, this class does not
+ * provide an implementation on what to do with the features. To build a mode tree, helper classes
+ * like the #AssemblyBase or the #Subsystem class extend and use the functionality of the base
+ * class.
  */
 class SubsystemBase : public SystemObject,
                       public HasModesIF,
@@ -95,6 +102,7 @@ class SubsystemBase : public SystemObject,
                                        Submode_t targetSubmode);
 
   /**
+   * This function takes care of sending all according mode commands specified inside a mode table.
    * We need to know the target Submode, as children are able to inherit the submode
    * Still, we have a default for all child implementations which do not use submode inheritance
    */

src/fsfw/tcdistribution/CCSDSDistributor.cpp

@@ -2,12 +2,13 @@
 
 #include "fsfw/objectmanager/ObjectManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
-#include "fsfw/tmtcpacket/SpacePacketBase.h"
+#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
 
 #define CCSDS_DISTRIBUTOR_DEBUGGING 0
 
-CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId)
-    : TcDistributor(setObjectId), defaultApid(setDefaultApid) {}
+CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId,
+                                   CcsdsPacketCheckIF* packetChecker)
+    : TcDistributor(setObjectId), defaultApid(setDefaultApid), packetChecker(packetChecker) {}
 
 CCSDSDistributor::~CCSDSDistributor() = default;
 
@@ -25,7 +26,7 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
 #endif
   const uint8_t* packet = nullptr;
   size_t size = 0;
-  ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(), &packet, &size);
+  ReturnValue_t result = tcStore->getData(currentMessage.getStorageId(), &packet, &size);
   if (result != returnvalue::OK) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
@@ -40,19 +41,21 @@ TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() {
 #endif
     return queueMap.end();
   }
-  SpacePacketBase currentPacket(packet);
-
+  SpacePacketReader currentPacket(packet, size);
+  result = packetChecker->checkPacket(currentPacket, size);
+  if (result != returnvalue::OK) {
+  }
 #if FSFW_CPP_OSTREAM_ENABLED == 1 && CCSDS_DISTRIBUTOR_DEBUGGING == 1
-  sif::info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex
-            << currentPacket.getAPID() << std::dec << std::endl;
+  sif::info << "CCSDSDistributor::selectDestination has packet with APID 0x" << std::hex
+            << currentPacket.getApid() << std::dec << std::endl;
 #endif
-  auto position = this->queueMap.find(currentPacket.getAPID());
+  auto position = this->queueMap.find(currentPacket.getApid());
   if (position != this->queueMap.end()) {
     return position;
   } else {
     // The APID was not found. Forward packet to main SW-APID anyway to
     //  create acceptance failure report.
-    return this->queueMap.find(this->defaultApid);
+    return queueMap.find(this->defaultApid);
   }
 }
 
@@ -80,6 +83,9 @@ ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid, MessageQueueI
 uint16_t CCSDSDistributor::getIdentifier() { return 0; }
 
 ReturnValue_t CCSDSDistributor::initialize() {
+  if (packetChecker == nullptr) {
+    packetChecker = new CcsdsPacketChecker(ccsds::PacketType::TC);
+  }
   ReturnValue_t status = this->TcDistributor::initialize();
   this->tcStore = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
   if (this->tcStore == nullptr) {