diff --git a/hal/src/fsfw_hal/linux/gpio/Gpio.h b/hal/src/fsfw_hal/linux/gpio/Gpio.h
new file mode 100644
index 000000000..2ffe2146f
--- /dev/null
+++ b/hal/src/fsfw_hal/linux/gpio/Gpio.h
@@ -0,0 +1,30 @@
+#ifndef FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_
+#define FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_
+
+#include "fsfw_hal/common/gpio/GpioIF.h"
+#include "fsfw_hal/common/gpio/gpioDefinitions.h"
+
+/**
+ * @brief   Additional abstraction layer for handling GPIOs.
+ *
+ * @author  J. Meier
+ */
+class Gpio {
+ public:
+  Gpio(gpioId_t gpioId, GpioIF* gpioIF) : gpioId(gpioId), gpioIF(gpioIF) {
+      if (gpioIF == nullptr) {
+          sif::error << "Gpio::Gpio: Invalid GpioIF" << std::endl;
+      }
+  }
+  ReturnValue_t pullHigh() {
+      return gpioIF->pullHigh(gpioId);
+  }
+  ReturnValue_t pullLow() {
+      return gpioIF->pullLow(gpioId);
+  }
+ private:
+  gpioId_t gpioId = gpio::NO_GPIO;
+  GpioIF* gpioIF = nullptr;
+};
+
+#endif /* FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_ */
diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp
index 0abff60e0..3cfa0417e 100644
--- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp
+++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp
@@ -269,6 +269,50 @@ void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCooki
       cfsetispeed(options, B460800);
       cfsetospeed(options, B460800);
       break;
+    case 500000:
+      cfsetispeed(options, B500000);
+      cfsetospeed(options, B500000);
+      break;
+    case 576000:
+      cfsetispeed(options, B576000);
+      cfsetospeed(options, B576000);
+      break;
+    case 921600:
+      cfsetispeed(options, B921600);
+      cfsetospeed(options, B921600);
+      break;
+    case 1000000:
+      cfsetispeed(options, B1000000);
+      cfsetospeed(options, B1000000);
+      break;
+    case 1152000:
+      cfsetispeed(options, B1152000);
+      cfsetospeed(options, B1152000);
+      break;
+    case 1500000:
+      cfsetispeed(options, B1500000);
+      cfsetospeed(options, B1500000);
+      break;
+    case 2000000:
+      cfsetispeed(options, B2000000);
+      cfsetospeed(options, B2000000);
+      break;
+    case 2500000:
+      cfsetispeed(options, B2500000);
+      cfsetospeed(options, B2500000);
+      break;
+    case 3000000:
+      cfsetispeed(options, B3000000);
+      cfsetospeed(options, B3000000);
+      break;
+    case 3500000:
+      cfsetispeed(options, B3500000);
+      cfsetospeed(options, B3500000);
+      break;
+    case 4000000:
+      cfsetispeed(options, B4000000);
+      cfsetospeed(options, B4000000);
+      break;
     default:
 #if FSFW_CPP_OSTREAM_ENABLED == 1
       sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl;
diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
index 99e80a8e7..63d907667 100644
--- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
+++ b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp
@@ -1,4 +1,4 @@
-#include "fsfw_hal/linux/uart/UartCookie.h"
+#include "UartCookie.h"
 
 #include <fsfw/serviceinterface.h>
 
diff --git a/src/fsfw/cfdp/CFDPHandler.cpp b/src/fsfw/cfdp/CFDPHandler.cpp
index 96baa98c7..09a24186b 100644
--- a/src/fsfw/cfdp/CFDPHandler.cpp
+++ b/src/fsfw/cfdp/CFDPHandler.cpp
@@ -12,7 +12,9 @@ object_id_t CFDPHandler::packetDestination = 0;
 
 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;
 }
 
diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp
index 953dacb46..0e4ff970c 100644
--- a/src/fsfw/controller/ControllerBase.cpp
+++ b/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); }
diff --git a/src/fsfw/datapool/CMakeLists.txt b/src/fsfw/datapool/CMakeLists.txt
index be4606aaa..535be2b08 100644
--- a/src/fsfw/datapool/CMakeLists.txt
+++ b/src/fsfw/datapool/CMakeLists.txt
@@ -1,4 +1,4 @@
-target_sources(${LIB_FSFW_NAME} PRIVATE
-	PoolDataSetBase.cpp
-	PoolEntry.cpp
+target_sources(${LIB_FSFW_NAME} PRIVATE 
+    PoolDataSetBase.cpp
+    PoolEntry.cpp
 )
\ No newline at end of file
diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp
index c974601cc..82aefc18c 100644
--- a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp
+++ b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp
@@ -47,13 +47,14 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId,
   HasLocalDataPoolIF* hkOwner = ObjectManager::instance()->get<HasLocalDataPoolIF>(poolOwner);
   if (hkOwner == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "LocalPoolVariable: The supplied pool owner did not implement the correct "
-                  "interface HasLocalDataPoolIF!"
-               << std::endl;
+    sif::error << "LocalPoolVariable: The supplied pool owner 0x" << std::hex << poolOwner
+               << std::dec << " did not implement the correct interface "
+               << "HasLocalDataPoolIF" << std::endl;
 #else
     sif::printError(
-        "LocalPoolVariable: The supplied pool owner did not implement the correct "
-        "interface HasLocalDataPoolIF!\n");
+        "LocalPoolVariable: The supplied pool owner 0x%08x did not implement the correct "
+        "interface HasLocalDataPoolIF\n",
+        poolOwner);
 #endif
     return;
   }
diff --git a/src/fsfw/datapoollocal/StaticLocalDataSet.h b/src/fsfw/datapoollocal/StaticLocalDataSet.h
index dd264e582..59047d67b 100644
--- a/src/fsfw/datapoollocal/StaticLocalDataSet.h
+++ b/src/fsfw/datapoollocal/StaticLocalDataSet.h
@@ -46,7 +46,7 @@ class StaticLocalDataSet : public LocalPoolDataSetBase {
   }
 
  private:
-  std::array<PoolVariableIF*, NUM_VARIABLES> poolVarList;
+  std::array<PoolVariableIF*, NUM_VARIABLES> poolVarList = {};
 };
 
 #endif /* FSFW_DATAPOOLLOCAL_STATICLOCALDATASET_H_ */
diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp
index c29022e53..414098adf 100644
--- a/src/fsfw/devicehandlers/AssemblyBase.cpp
+++ b/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);
+  }
+}
diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h
index 3e2359282..a6e6e3dbb 100644
--- a/src/fsfw/devicehandlers/AssemblyBase.h
+++ b/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,13 +46,14 @@ 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
-   *    - @c RETURN_OK if ok
+   *    - @c RETURN_OK if OK
    *    - @c NEED_SECOND_STEP if children need to be commanded again
    */
   virtual ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) = 0;
@@ -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_ */
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 08a15d5ff..18f8a2f7c 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -39,8 +39,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();
@@ -48,9 +49,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device
     printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase",
                         HasReturnvaluesIF::RETURN_FAILED, "Invalid cookie");
   }
-  if (this->fdirInstance == nullptr) {
-    this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, defaultFdirParentId);
-  }
 }
 
 void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) {
@@ -126,6 +124,17 @@ ReturnValue_t DeviceHandlerBase::initialize() {
   if (result != RETURN_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);
@@ -1385,6 +1394,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,
@@ -1407,7 +1418,7 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() {
   this->poolManager.initializeAfterTaskCreation();
 
   if (setStartupImmediately) {
-    startTransition(MODE_ON, SUBMODE_NONE);
+    startTransition(MODE_ON, getInitialSubmode());
   }
   return HasReturnvaluesIF::RETURN_OK;
 }
@@ -1491,3 +1502,9 @@ 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; }
diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h
index c8b8b86d7..a8de34eb8 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerBase.h
+++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h
@@ -103,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);
 
   /**
@@ -647,6 +650,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;
 
@@ -818,6 +827,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;
diff --git a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
index 48783c200..88fba19f4 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
+++ b/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());
diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h
index 1fc63d3b2..1ea742f7e 100644
--- a/src/fsfw/devicehandlers/DeviceHandlerIF.h
+++ b/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;
 
diff --git a/src/fsfw/devicehandlers/HealthDevice.cpp b/src/fsfw/devicehandlers/HealthDevice.cpp
index a626fa6c4..5514b65f4 100644
--- a/src/fsfw/devicehandlers/HealthDevice.cpp
+++ b/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); }
diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp
index aaa7d6c52..824682da3 100644
--- a/src/fsfw/events/EventManager.cpp
+++ b/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 != HasReturnvaluesIF::RETURN_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();
@@ -189,4 +201,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 */
diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h
index f2d642ffb..e2a8dd950 100644
--- a/src/fsfw/events/EventManager.h
+++ b/src/fsfw/events/EventManager.h
@@ -42,6 +42,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;
diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp
index fedef8696..c17cf6240 100644
--- a/src/fsfw/fdir/FailureIsolationBase.cpp
+++ b/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() {
@@ -51,11 +52,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 RETURN_FAILED;
diff --git a/src/fsfw/fdir/FailureIsolationBase.h b/src/fsfw/fdir/FailureIsolationBase.h
index 85d18add8..7d1280837 100644
--- a/src/fsfw/fdir/FailureIsolationBase.h
+++ b/src/fsfw/fdir/FailureIsolationBase.h
@@ -14,13 +14,12 @@ class FailureIsolationBase : public HasReturnvaluesIF,
                              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);
diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h
index 41abeef33..bdd1ce96c 100644
--- a/src/fsfw/health/HasHealthIF.h
+++ b/src/fsfw/health/HasHealthIF.h
@@ -23,19 +23,15 @@ class HasHealthIF {
   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;
diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp
index fa16ec3f6..a3f9645b7 100644
--- a/src/fsfw/internalerror/InternalErrorReporter.cpp
+++ b/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
     }
   }
diff --git a/src/fsfw/ipc/QueueFactory.h b/src/fsfw/ipc/QueueFactory.h
index 864c456d4..8069d8369 100644
--- a/src/fsfw/ipc/QueueFactory.h
+++ b/src/fsfw/ipc/QueueFactory.h
@@ -5,6 +5,7 @@
 
 #include "MessageQueueIF.h"
 #include "MessageQueueMessage.h"
+#include "definitions.h"
 
 /**
  * Creates message queues.
@@ -22,7 +23,8 @@ class QueueFactory {
   static QueueFactory* instance();
 
   MessageQueueIF* createMessageQueue(uint32_t messageDepth = 3,
-                                     size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+                                     size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+                                     MqArgs* args = nullptr);
 
   void deleteMessageQueue(MessageQueueIF* queue);
 
diff --git a/src/fsfw/ipc/definitions.h b/src/fsfw/ipc/definitions.h
new file mode 100644
index 000000000..1cc7a3c4f
--- /dev/null
+++ b/src/fsfw/ipc/definitions.h
@@ -0,0 +1,12 @@
+#ifndef FSFW_SRC_FSFW_IPC_DEFINITIONS_H_
+#define FSFW_SRC_FSFW_IPC_DEFINITIONS_H_
+#include <fsfw/objectmanager/SystemObjectIF.h>
+
+struct MqArgs {
+  MqArgs(){};
+  MqArgs(object_id_t objectId, void* args = nullptr) : objectId(objectId), args(args) {}
+  object_id_t objectId = 0;
+  void* args = nullptr;
+};
+
+#endif /* FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ */
diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h
index 0ebe77d81..57c4fb664 100644
--- a/src/fsfw/modes/HasModesIF.h
+++ b/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;
diff --git a/src/fsfw/objectmanager/ObjectManager.cpp b/src/fsfw/objectmanager/ObjectManager.cpp
index 2017938a1..39fef5b55 100644
--- a/src/fsfw/objectmanager/ObjectManager.cpp
+++ b/src/fsfw/objectmanager/ObjectManager.cpp
@@ -95,13 +95,16 @@ void ObjectManager::initialize() {
   for (auto const& it : objectList) {
     result = it.second->initialize();
     if (result != RETURN_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++;
     }
diff --git a/src/fsfw/osal/freertos/MessageQueue.cpp b/src/fsfw/osal/freertos/MessageQueue.cpp
index a8333fe55..927d7820b 100644
--- a/src/fsfw/osal/freertos/MessageQueue.cpp
+++ b/src/fsfw/osal/freertos/MessageQueue.cpp
@@ -4,7 +4,7 @@
 #include "fsfw/osal/freertos/QueueMapManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
     : maxMessageSize(maxMessageSize) {
   handle = xQueueCreate(messageDepth, maxMessageSize);
   if (handle == nullptr) {
diff --git a/src/fsfw/osal/freertos/MessageQueue.h b/src/fsfw/osal/freertos/MessageQueue.h
index 1cb343d13..fc1d78e50 100644
--- a/src/fsfw/osal/freertos/MessageQueue.h
+++ b/src/fsfw/osal/freertos/MessageQueue.h
@@ -7,6 +7,7 @@
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
 #include "fsfw/ipc/MessageQueueMessageIF.h"
+#include "fsfw/ipc/definitions.h"
 #include "queue.h"
 
 /**
@@ -53,7 +54,8 @@ class MessageQueue : public MessageQueueIF {
    * This should be left default.
    */
   MessageQueue(size_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
 
   /** Copying message queues forbidden */
   MessageQueue(const MessageQueue&) = delete;
diff --git a/src/fsfw/osal/freertos/QueueFactory.cpp b/src/fsfw/osal/freertos/QueueFactory.cpp
index f4941481d..8424123cc 100644
--- a/src/fsfw/osal/freertos/QueueFactory.cpp
+++ b/src/fsfw/osal/freertos/QueueFactory.cpp
@@ -22,8 +22,9 @@ QueueFactory::QueueFactory() {}
 
 QueueFactory::~QueueFactory() {}
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
diff --git a/src/fsfw/osal/host/MessageQueue.cpp b/src/fsfw/osal/host/MessageQueue.cpp
index d328fb820..c6f85382f 100644
--- a/src/fsfw/osal/host/MessageQueue.cpp
+++ b/src/fsfw/osal/host/MessageQueue.cpp
@@ -8,7 +8,7 @@
 #include "fsfw/osal/host/QueueMapManager.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
+MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
     : messageSize(maxMessageSize), messageDepth(messageDepth) {
   queueLock = MutexFactory::instance()->createMutex();
   auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId);
diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h
index 49375bb56..d090d269a 100644
--- a/src/fsfw/osal/host/MessageQueue.h
+++ b/src/fsfw/osal/host/MessageQueue.h
@@ -8,6 +8,7 @@
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
 #include "fsfw/ipc/MutexIF.h"
+#include "fsfw/ipc/definitions.h"
 #include "fsfw/timemanager/Clock.h"
 
 /**
@@ -54,7 +55,8 @@ class MessageQueue : public MessageQueueIF {
    * This should be left default.
    */
   MessageQueue(size_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
 
   /** Copying message queues forbidden */
   MessageQueue(const MessageQueue&) = delete;
diff --git a/src/fsfw/osal/host/QueueFactory.cpp b/src/fsfw/osal/host/QueueFactory.cpp
index 3c63e6c9a..732892caf 100644
--- a/src/fsfw/osal/host/QueueFactory.cpp
+++ b/src/fsfw/osal/host/QueueFactory.cpp
@@ -27,12 +27,13 @@ QueueFactory::QueueFactory() {}
 
 QueueFactory::~QueueFactory() {}
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
   // A thread-safe queue can be implemented  by using a combination
   // of std::queue and std::mutex. This uses dynamic memory allocation
   // which could be alleviated by using a custom allocator, external library
   // (etl::queue) or simply using std::queue, we're on a host machine anyway.
-  return new MessageQueue(messageDepth, maxMessageSize);
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
diff --git a/src/fsfw/osal/linux/CMakeLists.txt b/src/fsfw/osal/linux/CMakeLists.txt
index dcdade67f..679b29319 100644
--- a/src/fsfw/osal/linux/CMakeLists.txt
+++ b/src/fsfw/osal/linux/CMakeLists.txt
@@ -1,29 +1,29 @@
 target_sources(${LIB_FSFW_NAME} PRIVATE
     Clock.cpp
-	BinarySemaphore.cpp
-	CountingSemaphore.cpp
-	FixedTimeslotTask.cpp
-	InternalErrorCodes.cpp
-	MessageQueue.cpp
-	Mutex.cpp
-	MutexFactory.cpp
-	PeriodicPosixTask.cpp
-	PosixThread.cpp
-	QueueFactory.cpp
-	SemaphoreFactory.cpp
-	TaskFactory.cpp
-	tcpipHelpers.cpp
-	unixUtility.cpp
+    BinarySemaphore.cpp
+    CountingSemaphore.cpp
+    FixedTimeslotTask.cpp
+    InternalErrorCodes.cpp
+    MessageQueue.cpp
+    Mutex.cpp
+    MutexFactory.cpp
+    PeriodicPosixTask.cpp
+    PosixThread.cpp
+    QueueFactory.cpp
+    SemaphoreFactory.cpp
+    TaskFactory.cpp
+    tcpipHelpers.cpp
+    unixUtility.cpp
 )
 
 find_package(Threads REQUIRED)
 
 target_link_libraries(${LIB_FSFW_NAME} PRIVATE
-	${CMAKE_THREAD_LIBS_INIT}
-	rt
+    ${CMAKE_THREAD_LIBS_INIT}
+    rt
 )
 
 target_link_libraries(${LIB_FSFW_NAME} INTERFACE
-	${CMAKE_THREAD_LIBS_INIT}
+    ${CMAKE_THREAD_LIBS_INIT}
 )
 
diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp
index f876ec6e9..bfc1d0f74 100644
--- a/src/fsfw/osal/linux/MessageQueue.cpp
+++ b/src/fsfw/osal/linux/MessageQueue.cpp
@@ -11,7 +11,7 @@
 #include "fsfw/osal/linux/unixUtility.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize)
+MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args)
     : id(MessageQueueIF::NO_QUEUE),
       lastPartner(MessageQueueIF::NO_QUEUE),
       defaultDestination(MessageQueueIF::NO_QUEUE),
@@ -37,6 +37,9 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize)
     // Successful mq_open call
     this->id = tempId;
   }
+  if (args != nullptr) {
+    this->mqArgs = *args;
+  }
 }
 
 MessageQueue::~MessageQueue() {
@@ -240,9 +243,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
                                                         bool ignoreFault) {
   if (message == nullptr) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl;
+    sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr" << std::endl;
 #else
-    sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n");
+    sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr\n");
 #endif
     return HasReturnvaluesIF::RETURN_FAILED;
   }
@@ -256,7 +259,7 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
     if (!ignoreFault) {
       InternalErrorReporterIF* internalErrorReporter =
           ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
-      if (internalErrorReporter != NULL) {
+      if (internalErrorReporter != nullptr) {
         internalErrorReporter->queueMessageNotSent();
       }
     }
diff --git a/src/fsfw/osal/linux/MessageQueue.h b/src/fsfw/osal/linux/MessageQueue.h
index dbf6555e8..58afccd60 100644
--- a/src/fsfw/osal/linux/MessageQueue.h
+++ b/src/fsfw/osal/linux/MessageQueue.h
@@ -6,6 +6,7 @@
 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
+#include "fsfw/ipc/definitions.h"
 /**
  * @brief		This class manages sending and receiving of message queue messages.
  *
@@ -42,7 +43,8 @@ class MessageQueue : public MessageQueueIF {
    * 							This should be left default.
    */
   MessageQueue(uint32_t messageDepth = 3,
-               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
   /**
    * @brief	The destructor deletes the formerly created message queue.
    * @details	This is accomplished by using the delete call provided by the operating system.
@@ -184,6 +186,8 @@ class MessageQueue : public MessageQueueIF {
    */
   char name[16];
 
+  MqArgs mqArgs = {};
+
   static uint16_t queueCounter;
   const size_t maxMessageSize;
 
diff --git a/src/fsfw/osal/linux/QueueFactory.cpp b/src/fsfw/osal/linux/QueueFactory.cpp
index d1b1cfdb7..24ace1ae3 100644
--- a/src/fsfw/osal/linux/QueueFactory.cpp
+++ b/src/fsfw/osal/linux/QueueFactory.cpp
@@ -28,8 +28,9 @@ QueueFactory::QueueFactory() {}
 
 QueueFactory::~QueueFactory() {}
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
diff --git a/src/fsfw/osal/rtems/MessageQueue.cpp b/src/fsfw/osal/rtems/MessageQueue.cpp
index e45679d52..870730677 100644
--- a/src/fsfw/osal/rtems/MessageQueue.cpp
+++ b/src/fsfw/osal/rtems/MessageQueue.cpp
@@ -6,7 +6,7 @@
 #include "fsfw/osal/rtems/RtemsBasic.h"
 #include "fsfw/serviceinterface/ServiceInterface.h"
 
-MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size)
+MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, MqArgs* args)
     : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) {
   rtems_name name = ('Q' << 24) + (queueCounter++ << 8);
   rtems_status_code status =
diff --git a/src/fsfw/osal/rtems/MessageQueue.h b/src/fsfw/osal/rtems/MessageQueue.h
index 89aae2adb..0f1ee6eee 100644
--- a/src/fsfw/osal/rtems/MessageQueue.h
+++ b/src/fsfw/osal/rtems/MessageQueue.h
@@ -5,6 +5,7 @@
 #include "fsfw/internalerror/InternalErrorReporterIF.h"
 #include "fsfw/ipc/MessageQueueIF.h"
 #include "fsfw/ipc/MessageQueueMessage.h"
+#include "fsfw/ipc/definitions.h"
 
 /**
  *	@brief		This class manages sending and receiving of message queue messages.
@@ -34,7 +35,8 @@ class MessageQueue : public MessageQueueIF {
    * 							This should be left default.
    */
   MessageQueue(size_t message_depth = 3,
-               size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE);
+               size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE,
+               MqArgs* args = nullptr);
   /**
    * @brief	The destructor deletes the formerly created message queue.
    * @details	This is accomplished by using the delete call provided by the operating system.
diff --git a/src/fsfw/osal/rtems/QueueFactory.cpp b/src/fsfw/osal/rtems/QueueFactory.cpp
index 3d517f609..2519f444b 100644
--- a/src/fsfw/osal/rtems/QueueFactory.cpp
+++ b/src/fsfw/osal/rtems/QueueFactory.cpp
@@ -49,8 +49,9 @@ QueueFactory::QueueFactory() {}
 
 QueueFactory::~QueueFactory() {}
 
-MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) {
-  return new MessageQueue(messageDepth, maxMessageSize);
+MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize,
+                                                 MqArgs* args) {
+  return new MessageQueue(messageDepth, maxMessageSize, args);
 }
 
 void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; }
diff --git a/src/fsfw/parameters/HasParametersIF.h b/src/fsfw/parameters/HasParametersIF.h
index 48557b4a4..e95b69aba 100644
--- a/src/fsfw/parameters/HasParametersIF.h
+++ b/src/fsfw/parameters/HasParametersIF.h
@@ -66,7 +66,7 @@ class HasParametersIF {
    * @param newValues
    * @param startAtIndex Linear index, runs left to right, top to bottom for
    * matrix indexes.
-   * @return
+   * @return RETURN_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,
diff --git a/src/fsfw/parameters/ParameterWrapper.cpp b/src/fsfw/parameters/ParameterWrapper.cpp
index 27552290e..e772f1b69 100644
--- a/src/fsfw/parameters/ParameterWrapper.cpp
+++ b/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;
diff --git a/src/fsfw/power/PowerSensor.cpp b/src/fsfw/power/PowerSensor.cpp
index e73b12d07..1936e8eec 100644
--- a/src/fsfw/power/PowerSensor.cpp
+++ b/src/fsfw/power/PowerSensor.cpp
@@ -15,8 +15,9 @@ PowerSensor::PowerSensor(object_id_t objectId, sid_t setId, VariableIds ids, Def
                    limits.currentMin, limits.currentMax, events.currentLow, events.currentHigh),
       voltageLimit(objectId, MODULE_ID_VOLTAGE, ids.pidVoltage, confirmationCount,
                    limits.voltageMin, limits.voltageMax, events.voltageLow, events.voltageHigh) {
-  commandQueue =
-      QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 PowerSensor::~PowerSensor() { QueueFactory::instance()->deleteMessageQueue(commandQueue); }
diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h
index bc883fbc6..40ac3282d 100644
--- a/src/fsfw/power/PowerSwitchIF.h
+++ b/src/fsfw/power/PowerSwitchIF.h
@@ -1,6 +1,7 @@
 #ifndef FSFW_POWER_POWERSWITCHIF_H_
 #define FSFW_POWER_POWERSWITCHIF_H_
 
+#include "definitions.h"
 #include "../events/Event.h"
 #include "../returnvalues/HasReturnvaluesIF.h"
 #include "definitions.h"
diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp
index 7608c6e75..9e3263c62 100644
--- a/src/fsfw/power/PowerSwitcher.cpp
+++ b/src/fsfw/power/PowerSwitcher.cpp
@@ -1,3 +1,4 @@
+#include "definitions.h"
 #include "fsfw/power/PowerSwitcher.h"
 
 #include "definitions.h"
diff --git a/src/fsfw/pus/Service1TelecommandVerification.cpp b/src/fsfw/pus/Service1TelecommandVerification.cpp
index 13d6a1c41..772137dd5 100644
--- a/src/fsfw/pus/Service1TelecommandVerification.cpp
+++ b/src/fsfw/pus/Service1TelecommandVerification.cpp
@@ -16,7 +16,9 @@ Service1TelecommandVerification::Service1TelecommandVerification(object_id_t obj
       apid(apid),
       serviceId(serviceId),
       targetDestination(targetDestination) {
-  tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  tmQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 Service1TelecommandVerification::~Service1TelecommandVerification() {
diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp
index 4517bc262..a24079452 100644
--- a/src/fsfw/pus/Service5EventReporting.cpp
+++ b/src/fsfw/pus/Service5EventReporting.cpp
@@ -12,7 +12,9 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t ap
                                                uint32_t messageQueueDepth)
     : PusServiceBase(objectId, apid, serviceId),
       maxNumberReportsPerCycle(maxNumberReportsPerCycle) {
-  eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 Service5EventReporting::~Service5EventReporting() {
@@ -36,9 +38,6 @@ ReturnValue_t Service5EventReporting::performService() {
       }
     }
   }
-#if FSFW_CPP_OSTREAM_ENABLED == 1
-  sif::warning << "Service5EventReporting::generateEventReport: Too many events" << std::endl;
-#endif
   return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -87,7 +86,7 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) {
 // to be registered to the event manager to listen for events.
 ReturnValue_t Service5EventReporting::initialize() {
   EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
-  if (manager == NULL) {
+  if (manager == nullptr) {
     return RETURN_FAILED;
   }
   // register Service 5 as listener for events
diff --git a/src/fsfw/pus/Service5EventReporting.h b/src/fsfw/pus/Service5EventReporting.h
index 74264130b..c1032d189 100644
--- a/src/fsfw/pus/Service5EventReporting.h
+++ b/src/fsfw/pus/Service5EventReporting.h
@@ -41,7 +41,7 @@
 class Service5EventReporting : public PusServiceBase {
  public:
   Service5EventReporting(object_id_t objectId, uint16_t apid, uint8_t serviceId,
-                         size_t maxNumberReportsPerCycle = 10, uint32_t messageQueueDepth = 10);
+                         size_t maxNumberReportsPerCycle, uint32_t messageQueueDepth);
   virtual ~Service5EventReporting();
 
   /***
diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h
index 9369e207e..1bea2f519 100644
--- a/src/fsfw/pus/Service9TimeManagement.h
+++ b/src/fsfw/pus/Service9TimeManagement.h
@@ -6,10 +6,10 @@
 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;
 
diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h
index 2c78c8cd3..953bb6821 100644
--- a/src/fsfw/subsystem/Subsystem.h
+++ b/src/fsfw/subsystem/Subsystem.h
@@ -12,8 +12,12 @@
 #include "modes/ModeDefinitions.h"
 
 /**
- * @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:
diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp
index 104db3c32..aec96434d 100644
--- a/src/fsfw/subsystem/SubsystemBase.cpp
+++ b/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;
diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h
index 8cfd5be07..0853ed6ea 100644
--- a/src/fsfw/subsystem/SubsystemBase.h
+++ b/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,
@@ -96,6 +103,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
    */
@@ -123,11 +131,11 @@ class SubsystemBase : public SystemObject,
   virtual void performChildOperation() = 0;
 
   virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
-                                         uint32_t *msToReachTheMode) = 0;
+                                         uint32_t *msToReachTheMode) override = 0;
 
-  virtual void startTransition(Mode_t mode, Submode_t submode) = 0;
+  virtual void startTransition(Mode_t mode, Submode_t submode) override = 0;
 
-  virtual void getMode(Mode_t *mode, Submode_t *submode);
+  virtual void getMode(Mode_t *mode, Submode_t *submode) override;
 
   virtual void setToExternalControl();
 
diff --git a/src/fsfw/tcdistribution/TcDistributor.cpp b/src/fsfw/tcdistribution/TcDistributor.cpp
index a650546c1..408e736e5 100644
--- a/src/fsfw/tcdistribution/TcDistributor.cpp
+++ b/src/fsfw/tcdistribution/TcDistributor.cpp
@@ -5,7 +5,9 @@
 #include "fsfw/tmtcservices/TmTcMessage.h"
 
 TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) {
-  tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS);
+  auto mqArgs = MqArgs(objectId);
+  tcQueue = QueueFactory::instance()->createMessageQueue(
+      DISTRIBUTER_MAX_PACKETS, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); }
diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.cpp b/src/fsfw/thermal/AbstractTemperatureSensor.cpp
index 68cd3aca5..f7b313862 100644
--- a/src/fsfw/thermal/AbstractTemperatureSensor.cpp
+++ b/src/fsfw/thermal/AbstractTemperatureSensor.cpp
@@ -4,14 +4,13 @@
 
 AbstractTemperatureSensor::AbstractTemperatureSensor(object_id_t setObjectid,
                                                      ThermalModuleIF *thermalModule)
-    : SystemObject(setObjectid),
-      commandQueue(NULL),
-      healthHelper(this, setObjectid),
-      parameterHelper(this) {
-  if (thermalModule != NULL) {
+    : SystemObject(setObjectid), healthHelper(this, setObjectid), parameterHelper(this) {
+  if (thermalModule != nullptr) {
     thermalModule->registerSensor(this);
   }
-  commandQueue = QueueFactory::instance()->createMessageQueue();
+  auto mqArgs = MqArgs(setObjectid, static_cast<void *>(this));
+  commandQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 AbstractTemperatureSensor::~AbstractTemperatureSensor() {
diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.h b/src/fsfw/thermal/AbstractTemperatureSensor.h
index 0c6493fee..b790b0cae 100644
--- a/src/fsfw/thermal/AbstractTemperatureSensor.h
+++ b/src/fsfw/thermal/AbstractTemperatureSensor.h
@@ -51,7 +51,7 @@ class AbstractTemperatureSensor : public HasHealthIF,
   HasHealthIF::HealthState getHealth();
 
  protected:
-  MessageQueueIF* commandQueue;
+  MessageQueueIF* commandQueue = nullptr;
   HealthHelper healthHelper;
   ParameterHelper parameterHelper;
 
diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp
index 4f0f80600..46f5c822a 100644
--- a/src/fsfw/thermal/Heater.cpp
+++ b/src/fsfw/thermal/Heater.cpp
@@ -12,7 +12,9 @@ Heater::Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1)
       switch1(switch1),
       heaterOnCountdown(10800000) /*about two orbits*/,
       parameterHelper(this) {
-  eventQueue = QueueFactory::instance()->createMessageQueue();
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  eventQueue = QueueFactory::instance()->createMessageQueue(
+      3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 Heater::~Heater() { QueueFactory::instance()->deleteMessageQueue(eventQueue); }
diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h
index 99e8a56ad..e9afff2e3 100644
--- a/src/fsfw/timemanager/Clock.h
+++ b/src/fsfw/timemanager/Clock.h
@@ -99,6 +99,13 @@ class Clock {
    */
   static ReturnValue_t getDateAndTime(TimeOfDay_t *time);
 
+  /**
+   * Convert to time of day struct given the POSIX timeval struct
+   * @param from
+   * @param to
+   * @return
+   */
+  static ReturnValue_t convertTimevalToTimeOfDay(const timeval *from, TimeOfDay_t *to);
   /**
    * Converts a time of day struct to POSIX seconds.
    * @param time The time of day as input
diff --git a/src/fsfw/timemanager/ClockCommon.cpp b/src/fsfw/timemanager/ClockCommon.cpp
index e5749b19f..18407362e 100644
--- a/src/fsfw/timemanager/ClockCommon.cpp
+++ b/src/fsfw/timemanager/ClockCommon.cpp
@@ -1,7 +1,9 @@
+#include <ctime>
+
 #include "fsfw/ipc/MutexGuard.h"
 #include "fsfw/timemanager/Clock.h"
 
-ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) {
+ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) {
   uint16_t leapSeconds;
   ReturnValue_t result = getLeapSeconds(&leapSeconds);
   if (result != HasReturnvaluesIF::RETURN_OK) {
@@ -31,7 +33,7 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) {
   return HasReturnvaluesIF::RETURN_OK;
 }
 
-ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
+ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) {
   if (timeMutex == nullptr) {
     return HasReturnvaluesIF::RETURN_FAILED;
   }
@@ -42,9 +44,22 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) {
   return HasReturnvaluesIF::RETURN_OK;
 }
 
+ReturnValue_t Clock::convertTimevalToTimeOfDay(const timeval* from, TimeOfDay_t* to) {
+  struct tm* timeInfo;
+  timeInfo = gmtime(&from->tv_sec);
+  to->year = timeInfo->tm_year + 1900;
+  to->month = timeInfo->tm_mon + 1;
+  to->day = timeInfo->tm_mday;
+  to->hour = timeInfo->tm_hour;
+  to->minute = timeInfo->tm_min;
+  to->second = timeInfo->tm_sec;
+  to->usecond = from->tv_usec;
+  return HasReturnvaluesIF::RETURN_OK;
+}
+
 ReturnValue_t Clock::checkOrCreateClockMutex() {
   if (timeMutex == nullptr) {
-    MutexFactory *mutexFactory = MutexFactory::instance();
+    MutexFactory* mutexFactory = MutexFactory::instance();
     if (mutexFactory == nullptr) {
       return HasReturnvaluesIF::RETURN_FAILED;
     }
diff --git a/src/fsfw/tmstorage/TmStoreBackendIF.h b/src/fsfw/tmstorage/TmStoreBackendIF.h
index d9f1a17b9..1e08342a1 100644
--- a/src/fsfw/tmstorage/TmStoreBackendIF.h
+++ b/src/fsfw/tmstorage/TmStoreBackendIF.h
@@ -33,50 +33,47 @@ class TmStoreBackendIF : public HasParametersIF {
   static const ReturnValue_t INVALID_REQUEST = MAKE_RETURN_CODE(15);
 
   static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MEMORY;
-  static const Event STORE_SEND_WRITE_FAILED =
-      MAKE_EVENT(0, severity::LOW);  //!< Initiating sending data to store failed. Low, par1:
-                                     //!< returnCode, par2: integer (debug info)
-  static const Event STORE_WRITE_FAILED = MAKE_EVENT(
-      1, severity::LOW);  //!< Data was sent, but writing failed. Low, par1: returnCode, par2: 0
-  static const Event STORE_SEND_READ_FAILED =
-      MAKE_EVENT(2, severity::LOW);  //!< Initiating reading data from store failed. Low, par1:
-                                     //!< returnCode, par2: 0
-  static const Event STORE_READ_FAILED = MAKE_EVENT(
-      3, severity::LOW);  //!< Data was requested, but access failed. Low, par1: returnCode, par2: 0
-  static const Event UNEXPECTED_MSG =
-      MAKE_EVENT(4, severity::LOW);  //!< An unexpected TM packet or data message occurred. Low,
-                                     //!< par1: 0, par2: integer (debug info)
-  static const Event STORING_FAILED = MAKE_EVENT(
-      5, severity::LOW);  //!< Storing data failed. May simply be a full store. Low, par1:
-                          //!< returnCode, par2: integer (sequence count of failed packet).
-  static const Event TM_DUMP_FAILED =
-      MAKE_EVENT(6, severity::LOW);  //!< Dumping retrieved data failed. Low, par1: returnCode,
-                                     //!< par2: integer (sequence count of failed packet).
-  static const Event STORE_INIT_FAILED =
-      MAKE_EVENT(7, severity::LOW);  //!< Corrupted init data or read error. Low, par1: returnCode,
-                                     //!< par2: integer (debug info)
-  static const Event STORE_INIT_EMPTY = MAKE_EVENT(
-      8, severity::INFO);  //!< Store was not initialized. Starts empty. Info, parameters both zero.
-  static const Event STORE_CONTENT_CORRUPTED =
-      MAKE_EVENT(9, severity::LOW);  //!< Data was read out, but it is inconsistent. Low par1:
-                                     //!< Memory address of corruption, par2: integer (debug info)
-  static const Event STORE_INITIALIZE =
-      MAKE_EVENT(10, severity::INFO);  //!< Info event indicating the store will be initialized,
-                                       //!< either at boot or after IOB switch. Info. pars: 0
-  static const Event INIT_DONE = MAKE_EVENT(
-      11, severity::INFO);  //!< Info event indicating the store was successfully initialized,
-                            //!< either at boot or after IOB switch. Info. pars: 0
-  static const Event DUMP_FINISHED = MAKE_EVENT(
-      12, severity::INFO);  //!< Info event indicating that dumping finished successfully. par1:
-                            //!< Number of dumped packets. par2: APID/SSC (16bits each)
-  static const Event DELETION_FINISHED = MAKE_EVENT(
-      13, severity::INFO);  //!< Info event indicating that deletion finished successfully. par1:
-                            //!< Number of deleted packets. par2: APID/SSC (16bits each)
-  static const Event DELETION_FAILED = MAKE_EVENT(
-      14,
-      severity::LOW);  //!< Info event indicating that something went wrong during deletion. pars: 0
-  static const Event AUTO_CATALOGS_SENDING_FAILED =
-      MAKE_EVENT(15, severity::INFO);  //!< Info that the a auto catalog report failed
+  //! Initiating sending data to store failed. Low, par1:
+  //! returnCode, par2: integer (debug info)
+  static const Event STORE_SEND_WRITE_FAILED = MAKE_EVENT(0, severity::LOW);
+  //! Data was sent, but writing failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_WRITE_FAILED = MAKE_EVENT(1, severity::LOW);
+  //! Initiating reading data from store failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_SEND_READ_FAILED = MAKE_EVENT(2, severity::LOW);
+  //! Data was requested, but access failed. Low, par1: returnCode, par2: 0
+  static const Event STORE_READ_FAILED = MAKE_EVENT(3, severity::LOW);
+  //! An unexpected TM packet or data message occurred. Low, par1: 0, par2: integer (debug info)
+  static const Event UNEXPECTED_MSG = MAKE_EVENT(4, severity::LOW);
+  //! Storing data failed. May simply be a full store. Low, par1: returnCode,
+  //! par2: integer (sequence count of failed packet).
+  static const Event STORING_FAILED = MAKE_EVENT(5, severity::LOW);
+  //! Dumping retrieved data failed. Low, par1: returnCode,
+  //! par2: integer (sequence count of failed packet).
+  static const Event TM_DUMP_FAILED = MAKE_EVENT(6, severity::LOW);
+  //! Corrupted init data or read error. Low, par1: returnCode, par2: integer (debug info)
+  //! Store was not initialized. Starts empty. Info, parameters both zero.
+  static const Event STORE_INIT_FAILED = MAKE_EVENT(7, severity::LOW);
+  //! Data was read out, but it is inconsistent. Low par1:
+  //! Memory address of corruption, par2: integer (debug info)
+  static const Event STORE_INIT_EMPTY = MAKE_EVENT(8, severity::INFO);
+
+  static const Event STORE_CONTENT_CORRUPTED = MAKE_EVENT(9, severity::LOW);
+  //! Info event indicating the store will be initialized, either at boot or after IOB switch.
+  //! Info. pars: 0
+  static const Event STORE_INITIALIZE = MAKE_EVENT(10, severity::INFO);
+  //! Info event indicating the store was successfully initialized, either at boot or after
+  //! IOB switch. Info. pars: 0
+  static const Event INIT_DONE = MAKE_EVENT(11, severity::INFO);
+  //! Info event indicating that dumping finished successfully.
+  //! par1: Number of dumped packets. par2: APID/SSC (16bits each)
+  static const Event DUMP_FINISHED = MAKE_EVENT(12, severity::INFO);
+  //! Info event indicating that deletion finished successfully.
+  //! par1:Number of deleted packets. par2: APID/SSC (16bits each)
+  static const Event DELETION_FINISHED = MAKE_EVENT(13, severity::INFO);
+  //! Info event indicating that something went wrong during deletion. pars: 0
+  static const Event DELETION_FAILED = MAKE_EVENT(14, severity::LOW);
+  //! Info that the a auto catalog report failed
+  static const Event AUTO_CATALOGS_SENDING_FAILED = MAKE_EVENT(15, severity::INFO);
 
   virtual ~TmStoreBackendIF() {}
 
diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
index bbdf8d2af..1bbd7fd17 100644
--- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp
+++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp
@@ -20,8 +20,10 @@ CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t a
       service(service),
       timeoutSeconds(commandTimeoutSeconds),
       commandMap(numberOfParallelCommands) {
-  commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
-  requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  size_t mqSz = MessageQueueMessage::MAX_MESSAGE_SIZE;
+  commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs);
+  requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs);
 }
 
 void CommandingServiceBase::setPacketSource(object_id_t packetSource) {
diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp
index 3af2b82ce..565ac5cd6 100644
--- a/src/fsfw/tmtcservices/PusServiceBase.cpp
+++ b/src/fsfw/tmtcservices/PusServiceBase.cpp
@@ -13,7 +13,9 @@ object_id_t PusServiceBase::packetDestination = 0;
 
 PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId)
     : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId) {
-  requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION);
+  auto mqArgs = MqArgs(setObjectId, static_cast<void*>(this));
+  requestQueue = QueueFactory::instance()->createMessageQueue(
+      PUS_SERVICE_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 PusServiceBase::~PusServiceBase() { QueueFactory::instance()->deleteMessageQueue(requestQueue); }
diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h
index e981a9452..6f2778a41 100644
--- a/src/fsfw/tmtcservices/SourceSequenceCounter.h
+++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h
@@ -19,6 +19,19 @@ class SourceSequenceCounter {
   void reset(uint16_t toValue = 0) {
     sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT);
   }
+  SourceSequenceCounter& operator++(int) {
+    this->increment();
+    return *this;
+  }
+  SourceSequenceCounter& operator--(int) {
+    this->decrement();
+    return *this;
+  }
+  SourceSequenceCounter& operator=(const uint16_t& newCount) {
+    sequenceCount = newCount;
+    return *this;
+  }
+  operator uint16_t() { return this->get(); }
 };
 
 #endif /* FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ */
diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp
index 8ea671195..cc6ec5993 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.cpp
+++ b/src/fsfw/tmtcservices/TmTcBridge.cpp
@@ -15,7 +15,9 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_i
       tcDestination(tcDestination)
 
 {
-  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
+  auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
+  tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(
+      TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
 }
 
 TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
@@ -172,15 +174,18 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
   }
 
   if (tmFifo->full()) {
+    if (warningSwitch) {
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-    sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
-                    "of stored packet IDs reached!"
-                 << std::endl;
+      sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
+                      "of stored packet IDs reached!"
+                   << std::endl;
 #else
-    sif::printWarning(
-        "TmTcBridge::storeDownlinkData: TM downlink max. number "
-        "of stored packet IDs reached!\n");
+      sif::printWarning(
+          "TmTcBridge::storeDownlinkData: TM downlink max. number "
+          "of stored packet IDs reached!\n");
 #endif
+      warningSwitch = true;
+    }
     if (overwriteOld) {
       tmFifo->retrieve(&storeId);
       tmStore->deleteData(storeId);
diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h
index 237f1f3ec..81d8e5d8f 100644
--- a/src/fsfw/tmtcservices/TmTcBridge.h
+++ b/src/fsfw/tmtcservices/TmTcBridge.h
@@ -72,6 +72,8 @@ class TmTcBridge : public AcceptsTelemetryIF,
   virtual uint16_t getIdentifier() override;
   virtual MessageQueueId_t getRequestQueue() override;
 
+  bool warningSwitch = true;
+
  protected:
   //! Cached for initialize function.
   object_id_t tmStoreId = objects::NO_OBJECT;
diff --git a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
index 3ad26876c..fa4e09080 100644
--- a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
+++ b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp
@@ -73,7 +73,7 @@ TEST_CASE("Command Executor", "[cmd-exec]") {
     REQUIRE(result != CommandExecutor::COMMAND_ERROR);
     // This ensures that the tests do not block indefinitely
     usleep(500);
-    REQUIRE(limitIdx < 500);
+    REQUIRE(limitIdx < 50000);
   }
   limitIdx = 0;
   CHECK(bytesHaveBeenRead == true);